static void cmdGetImuLoop(unsigned char status, unsigned char length, unsigned char *frame) {
unsigned int count;
unsigned long tic;
unsigned char *tic_char;
Payload pld;
LED_RED = 1;
count = frame[0] + (frame[1] << 8);
tic_char = (unsigned char*) &tic;
swatchReset();
tic = swatchTic();
while (count) {
pld = payCreateEmpty(16); // data length = 16
paySetData(pld, 4, tic_char);
payAppendData(pld, 4, 6, xlReadXYZ());
payAppendData(pld, 10, 6, gyroReadXYZ());
paySetStatus(pld, status);
paySetType(pld, CMD_GET_IMU_DATA);
radioSendPayload(macGetDestAddr(), pld);
count--;
payDelete(pld);
delay_ms(4);
tic = swatchTic();
}
LED_RED = 0;
}
// record current state to telemU structure
void telemSaveSample(unsigned long sampIdx)
{ telemU data;
data.telemStruct.sampleIndex = sampIdx;
//Stopwatch was already started in the cmdSpecialTelemetry function
data.telemStruct.timeStamp = (long)swatchTic();
// since T1 has higher priority, these state readings might get interrupted
CRITICAL_SECTION_START // need coherent sample without T1 int updates
// save Hall encoder position instead of commanded thrust
data.telemStruct.posL = pidObjs[0].p_state;
data.telemStruct.posR = pidObjs[1].p_state;
// save output instead of reading PWM (sync issue?)
data.telemStruct.dcL = pidObjs[0].output; // left
data.telemStruct.dcR = pidObjs[1].output; // right
data.telemStruct.bemfL = bemf[0];
data.telemStruct.bemfR = bemf[1];
CRITICAL_SECTION_END
data.telemStruct.gyroX = gdata[0] - offsx;
data.telemStruct.gyroY = gdata[1] - offsy;
data.telemStruct.gyroZ = gdata[2] - offsz;
data.telemStruct.gyroAvg = gyroAvg;
data.telemStruct.accelX = xldata[0];
data.telemStruct.accelY = xldata[1];
data.telemStruct.accelZ = xldata[2];
data.telemStruct.Vbatt = (int) adcGetVbatt();
data.telemStruct.sOut = steeringPID.output;
// inside T5 interrupt, so don't need to DisableIntT5
telemFlashSample(&data);
}
static void cmdGetPIDTelemetry(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame){
unsigned int count;
unsigned long tic;
unsigned char *tic_char = (unsigned char*)&tic;
//unsigned long sampNum = 0;
int i;
unsigned short idx = 0;
MacPacket packet; Payload pld;
unsigned char* telem_ptr;
count = frame[0] + (frame[1] << 8);
swatchReset();
tic = swatchTic();
while(count){
pld = payCreateEmpty(36); // data length = 12
//*(long*)(pld->pld_data + idx) = tic;
pld->pld_data[2] = tic_char[0];
pld->pld_data[3] = tic_char[1];
pld->pld_data[4] = tic_char[2];
pld->pld_data[5] = tic_char[3];
idx += sizeof(tic);
telem_ptr = pidGetTelemetry();
//memcpy((pld->pld_data)+idx , telem_ptr, 4*sizeof(int));
for(i = 0; i < (4*sizeof(int)+6*sizeof(long)); ++i) {
pld->pld_data[i+6] = telem_ptr[i];
}
pld->pld_data[0] = status;
pld->pld_data[1] = CMD_GET_PID_TELEMETRY;
// radioSendPayload(macGetDestAddr(), pld);
// Enqueue the packet for broadcast
while(!radioEnqueueTxPacket(packet));
count--;
//delay_ms(2); // ~3ms delay
//delay_us(695);
delay_ms(10);
tic = swatchTic();
}
}
void attSetup(float ts) {
is_ready = 0;
is_running = 0;
sample_period = ts;
//measureXLScale(SCALE_CALIB_SAMPLES);
xlReadXYZ();
attZero();
attReset();
swatchReset();
swatchTic();
is_ready = 1;
}
void peHomingLoop(void) {
if(swatchToc() > 100000) {
swatchTic();
if (homingOn) {
peTrack();
if((++callCount) == CALL_FREQ) {
callCount = 0;
peSendFrame();
}
}
}
pidSetInput(0, motor_l, 1);
pidSetInput(1, motor_r, 1);
}
void peProcessCommand(unsigned char cmd) {
int i;
switch(cmd) {
case 'a':
peSendResponse('b');
motor_l = 0;
motor_r = 0;
homingOn = 0;
break;
case 'h':
peSendResponse('H');
homingOn = 1;
swatchTic();
break;
case 's': // sweep
for(i=0;i<8;i++) {
// Motors Off
peSendFrame();
// Motors On
delay_ms(1000);
}
break;
case 'k': // single temperature frame
peReceiveFrame();
peSendFrame();
break;
case 't':
peTrack();
peSendFrame();
break;
default:
break;
}
}
