//read telemetry from Flash Memory and send radio packets back void telemFlashReadback(unsigned int count) { unsigned char status = 0; unsigned int sampLen = sizeof(telemStruct_t); unsigned long sampNum = 0; telemU data; DisableIntT5; // prevent MPU access to SPI2 for(sampNum = 0; sampNum < count; sampNum++) { dfmemReadSample(sampNum, sampLen, (unsigned char *) &data); if ((sampNum+1) != data.telemStruct.sampleIndex) while(1) // hang here if bad read { blink_leds(1,200); } radioConfirmationPacket(RADIO_DEST_ADDR, CMD_SPECIAL_TELEMETRY, status, sampLen, (unsigned char *) &data); // delay_ms(25); // slow down for XBee 57.6 K blink_leds(1,20); // wait 40 ms to give plenty of time to send packets } // wait for DMA to finish and release SPI2 while(port_status[1] == STAT_SPI_BUSY) { blink_leds(1,300); // waste some time } EnableIntT5; }
// set up velocity profile structure - assume 4 set points for now, generalize later void cmdSetVelProfile(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame){ int interval[NUM_VELS], delta[NUM_VELS], vel[NUM_VELS]; int idx = 0, i = 0; for(i = 0; i < NUM_VELS; i ++) { interval[i] = frame[idx]+ (frame[idx+1]<<8); idx+=2; } for(i = 0; i < NUM_VELS; i ++) { delta[i] = frame[idx]+ (frame[idx+1]<<8); idx+=2; } for(i = 0; i < NUM_VELS; i ++) { vel[i] = frame[idx]+ (frame[idx+1]<<8); idx+=2; } setPIDVelProfile(0, interval, delta, vel); for(i = 0; i < NUM_VELS; i ++) { interval[i] = frame[idx]+ (frame[idx+1]<<8); idx+=2; } for(i = 0; i < NUM_VELS; i ++) { delta[i] = frame[idx]+ (frame[idx+1]<<8); idx+=2; } for(i = 0; i < NUM_VELS; i ++) { vel[i] = frame[idx]+ (frame[idx+1]<<8); idx+=2; } setPIDVelProfile(1, interval, delta, vel); //Send confirmation packet radioConfirmationPacket(RADIO_DEST_ADDR, CMD_SET_VEL_PROFILE, status, 48, frame); return; //success }
// report motor position and reset motor position (from Hall angular sensors) // note motor_count is long (4 bytes) revs+frac rev void cmdZeroPos(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame) { long motor_count[2]; motor_count[0] = pidObjs[0].p_state; motor_count[1] = pidObjs[1].p_state; radioConfirmationPacket(RADIO_DEST_ADDR, CMD_ZERO_POS,\ status, sizeof(motor_count), (unsigned char *)motor_count); pidZeroPos(0); pidZeroPos(1); }
void cmdSetPIDGains(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame){ int Kp, Ki, Kd, Kaw, ff; int idx = 0; Kp = frame[idx] + (frame[idx+1] << 8); idx+=2; Ki = frame[idx] + (frame[idx+1] << 8); idx+=2; Kd = frame[idx] + (frame[idx+1] << 8); idx+=2; Kaw = frame[idx] + (frame[idx+1] << 8); idx+=2; ff = frame[idx] + (frame[idx+1] << 8); idx+=2; pidSetGains(0,Kp,Ki,Kd,Kaw, ff); Kp = frame[idx] + (frame[idx+1] << 8); idx+=2; Ki = frame[idx] + (frame[idx+1] << 8); idx+=2; Kd = frame[idx] + (frame[idx+1] << 8); idx+=2; Kaw = frame[idx] + (frame[idx+1] << 8); idx+=2; ff = frame[idx] + (frame[idx+1] << 8); idx+=2; pidSetGains(1,Kp,Ki,Kd,Kaw, ff); //Send confirmation packet radioConfirmationPacket(RADIO_DEST_ADDR, CMD_SET_PID_GAINS, status, 20, frame); return; //success }