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;
}
static void cmdSpecialTelemetry(unsigned char status, unsigned char length, unsigned char *frame) {
//unsigned int count;
unsigned long count; //count changed to long to accomodate 64Mbit flash
//count = (unsigned long)frame[0] + ((unsigned long)frame[1] << 8)
// + ((unsigned long)frame[2] << 16) + ((unsigned long)frame[3] << 24);
count = *((unsigned long*) (frame));
if (count != 0) {
swatchReset();
telemSetSamplesToSave(count);
}
}
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;
}
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();
}
}
