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;
}
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 attReadSensorData(void) {
gyroReadXYZ(); // 255 us
xlReadXYZ(); // 250 us
// Wii camera no longer used
}
