void gyroRunCalib(unsigned int count){
unsigned int i;
long x, y, z;
x = 0;
y = 0;
z = 0;
// throw away first 200 data. sometimes they are bad at the beginning.
for (i = 0; i < 200; ++i) {
gyroReadXYZ();
delay_us(100);
}
for (i = 0; i < count; i++) {
gyroReadXYZ();
x += GyroData.int_data[1];
y += GyroData.int_data[2];
z += GyroData.int_data[3];
delay_us(100);
}
GyroOffset.fdata[0] = 1.0*x/count;
GyroOffset.fdata[1] = 1.0*y/count;
GyroOffset.fdata[2] = 1.0*z/count;
}
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 gyroRunCalib(unsigned int count){
unsigned int i;
long x_acc, y_acc, z_acc;
x_acc = 0;
y_acc = 0;
z_acc = 0;
CRITICAL_SECTION_START
for (i = 0; i < count; ++i) {
gyroReadXYZ();
x_acc += GyroData.int_data[1];
y_acc += GyroData.int_data[2];
z_acc += GyroData.int_data[3];
delay_ms(1); // Sample at around 1kHz
}
CRITICAL_SECTION_END
offsets[0] = x_acc/count;
offsets[1] = y_acc/count;
offsets[2] = z_acc/count;
GyroOffset.fdata[0] = 1.0*x_acc/count;
GyroOffset.fdata[1] = 1.0*y_acc/count;
GyroOffset.fdata[2] = 1.0*z_acc/count;
}
static void imuISRHandler() {
LED_GREEN = 1;
int gyroData[3];
int xlData[3];
#if defined (__IMAGEPROC24)
/////// Get Gyro data and calc average via filter
gyroReadXYZ(); //bad design of gyro module; todo: humhu
gyroGetIntXYZ(gyroData);
#elif defined (__IMAGEPROC25)
mpuBeginUpdate();
mpuGetGyro(gyroData);
mpuGetXl(xlData);
#endif
lastGyro[0] = gyroData[0];
lastGyro[1] = gyroData[1];
lastGyro[2] = gyroData[2];
lastXL[0] = xlData[0];
lastXL[1] = xlData[1];
lastXL[2] = xlData[2];
int i;
for (i = 0; i < 3; i++) {
//Threshold:
if (ABS(lastGyro[i]) < GYRO_DRIFT_THRESH) {
lastGyro[0] = lastGyro[i] >> 1; //fast divide by 2
}
}
void gyroRunCalib(unsigned int count) {
unsigned int i;
long x_acc, y_acc, z_acc;
x_acc = 0;
y_acc = 0;
z_acc = 0;
CRITICAL_SECTION_START
// throw away first 200 data. Sometimes they are bad at the beginning.
for (i = 0; i < 200; ++i) {
gyroReadXYZ();
delay_us(100);
}
for (i = 0; i < count; i++) {
gyroReadXYZ();
x_acc += GyroData.int_data[1];
y_acc += GyroData.int_data[2];
z_acc += GyroData.int_data[3];
delay_ms(1); // Sample at around 1kHz
}
CRITICAL_SECTION_END
offsets[0] = x_acc/count;
offsets[1] = y_acc/count;
offsets[2] = z_acc/count;
GyroOffset.fdata[0] = 1.0*x_acc/count;
GyroOffset.fdata[1] = 1.0*y_acc/count;
GyroOffset.fdata[2] = 1.0*z_acc/count;
}
void gyroGetXYZ(unsigned char *data) {
gyroReadXYZ();
gyroDumpData(data);
}
void attReadSensorData(void) {
gyroReadXYZ(); // 255 us
xlReadXYZ(); // 250 us
// Wii camera no longer used
}
