//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
}
