void adcCalibOffsets(void) {
unsigned long lastUpdate;
float sumRate[3];
float dTemp, dTemp2, dTemp3;
int i;
delay(100);
#ifndef USE_DIGITAL_IMU
imuQuasiStatic(ADC_RATE_CALIB_SAMPLES);
#endif
sumRate[0] = sumRate[1] = sumRate[2] = 0.0;
lastUpdate = adcData.lastUpdate;
for (i = 0; i < ADC_RATE_CALIB_SAMPLES; i++) {
while (lastUpdate == adcData.lastUpdate)
;
lastUpdate = adcData.lastUpdate;
sumRate[0] += adcData.voltages[ADC_VOLTS_RATEX];
sumRate[1] += adcData.voltages[ADC_VOLTS_RATEY];
sumRate[2] += adcData.voltages[ADC_VOLTS_RATEZ];
}
dTemp = adcData.temperature - IMU_ROOM_TEMP;
dTemp2 = dTemp*dTemp;
dTemp3 = dTemp*dTemp2;
adcData.rateBiasX = -(sumRate[0] / ADC_RATE_CALIB_SAMPLES + p[IMU_GYO_BIAS1_X]*dTemp + p[IMU_GYO_BIAS2_X]*dTemp2 +
p[IMU_GYO_BIAS3_X]*dTemp3);
adcData.rateBiasY = -(sumRate[1] / ADC_RATE_CALIB_SAMPLES + p[IMU_GYO_BIAS1_Y]*dTemp + p[IMU_GYO_BIAS2_Y]*dTemp2 +
p[IMU_GYO_BIAS3_Y]*dTemp3);
adcData.rateBiasZ = -(sumRate[2] / ADC_RATE_CALIB_SAMPLES + p[IMU_GYO_BIAS1_Z]*dTemp + p[IMU_GYO_BIAS2_Z]*dTemp2 +
p[IMU_GYO_BIAS3_Z]*dTemp3);
adcData.rateX = 0.0f;
adcData.rateY = 0.0f;
adcData.rateZ = 0.0f;
}
void navUkfInitState(void) {
uint32_t lastUpdate;
float acc[3], mag[3];
float estAcc[3], estMag[3];
float m[3*3];
int i;
// vel
UKF_VELN = 0.0f;
UKF_VELE = 0.0f;
UKF_VELD = 0.0f;
// pos
UKF_POSN = 0.0f;
UKF_POSE = 0.0f;
UKF_POSD = navUkfPresToAlt(AQ_PRESSURE);
// acc bias
UKF_ACC_BIAS_X = 0.0f;
UKF_ACC_BIAS_Y = 0.0f;
UKF_ACC_BIAS_Z = 0.0f;
// gyo bias
UKF_GYO_BIAS_X = 0.0f;
UKF_GYO_BIAS_Y = 0.0f;
UKF_GYO_BIAS_Z = 0.0f;
// quat
UKF_Q1 = 1.0f;
UKF_Q2 = 0.0f;
UKF_Q3 = 0.0f;
UKF_Q4 = 0.0f;
UKF_PRES_ALT = navUkfPresToAlt(AQ_PRESSURE);
// wait for lack of movement
imuQuasiStatic(UKF_GYO_AVG_NUM);
// estimate initial orientation
i = 0;
do {
float rotError[3];
lastUpdate = IMU_LASTUPD;
while (lastUpdate == IMU_LASTUPD)
yield(1);
mag[0] = IMU_MAGX;
mag[1] = IMU_MAGY;
mag[2] = IMU_MAGZ;
acc[0] = IMU_ACCX;
acc[1] = IMU_ACCY;
acc[2] = IMU_ACCZ;
navUkfNormalizeVec3(acc, acc);
navUkfNormalizeVec3(mag, mag);
navUkfQuatToMatrix(m, &UKF_Q1, 1);
// rotate gravity to body frame of reference
navUkfRotateVecByRevMatrix(estAcc, navUkfData.v0a, m);
// rotate mags to body frame of reference
navUkfRotateVecByRevMatrix(estMag, navUkfData.v0m, m);
// measured error, starting with accel vector
rotError[0] = -(acc[2] * estAcc[1] - estAcc[2] * acc[1]) * 1.0f;
rotError[1] = -(acc[0] * estAcc[2] - estAcc[0] * acc[2]) * 1.0f;
rotError[2] = -(acc[1] * estAcc[0] - estAcc[1] * acc[0]) * 1.0f;
// add in mag vector
if (AQ_MAG_ENABLED && i < UKF_GYO_AVG_NUM*2) {
rotError[0] += -(mag[2] * estMag[1] - estMag[2] * mag[1]) * 0.50f;
rotError[1] += -(mag[0] * estMag[2] - estMag[0] * mag[2]) * 0.50f;
rotError[2] += -(mag[1] * estMag[0] - estMag[1] * mag[0]) * 0.50f;
}
navUkfRotateQuat(&UKF_Q1, &UKF_Q1, rotError, 0.1f);
i++;
} while (i <= UKF_GYO_AVG_NUM*5);
}
