void ahrs_update_accel(void) {
// c2 = ltp z-axis in imu-frame
struct Int32Vect3 c2 = { RMAT_ELMT(ahrs.ltp_to_imu_rmat, 0,2),
RMAT_ELMT(ahrs.ltp_to_imu_rmat, 1,2),
RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)
};
struct Int32Vect3 residual;
if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
/*
* centrifugal acceleration in body frame
* a_c_body = omega x (omega x r)
* (omega x r) = tangential velocity in body frame
* a_c_body = omega x vel_tangential_body
* assumption: tangential velocity only along body x-axis
*/
// FIXME: check overflows !
const struct Int32Vect3 vel_tangential_body = {(ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC), 0.0, 0.0};
struct Int32Vect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs.body_rate, vel_tangential_body);
INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC);
/* convert centrifucal acceleration from body to imu frame */
struct Int32Vect3 acc_c_imu;
INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
struct Int32Vect3 corrected_gravity;
INT32_VECT3_DIFF(corrected_gravity, imu.accel, acc_c_imu);
/* compute the residual of gravity vector in imu frame */
INT32_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
} else {
void ahrs_update_accel(void) {
struct Int32Vect3 c2 = { RMAT_ELMT(ahrs.ltp_to_imu_rmat, 0,2),
RMAT_ELMT(ahrs.ltp_to_imu_rmat, 1,2),
RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)};
struct Int32Vect3 residual;
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
// FIXME: check overflow ?
const struct Int32Vect3 Xdd_imu = {
0,
((ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC) * ahrs.imu_rate.r)
>>(INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC),
-((ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC) * ahrs.imu_rate.q)
>>(INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC)
};
struct Int32Vect3 corrected_gravity;
VECT3_DIFF(corrected_gravity, imu.accel, Xdd_imu);
INT32_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
#else
INT32_VECT3_CROSS_PRODUCT(residual, imu.accel, c2);
#endif
// residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
// rate_correction FRAC = RATE_FRAC = 12
// 2^12 / 2^24 * 5e-2 = 1/81920
ahrs_impl.rate_correction.p += -residual.x/82000;
ahrs_impl.rate_correction.q += -residual.y/82000;
ahrs_impl.rate_correction.r += -residual.z/82000;
// residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
// high_rez_bias = RATE_FRAC+28 = 40
// 2^40 / 2^24 * 5e-6 = 1/3.05
// ahrs_impl.high_rez_bias.p += residual.x*3;
// ahrs_impl.high_rez_bias.q += residual.y*3;
// ahrs_impl.high_rez_bias.r += residual.z*3;
ahrs_impl.high_rez_bias.p += residual.x;
ahrs_impl.high_rez_bias.q += residual.y;
ahrs_impl.high_rez_bias.r += residual.z;
/* */
INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
}
static inline void ahrs_update_mag_full(void) {
const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
MAG_BFP_OF_REAL(AHRS_H_Y),
MAG_BFP_OF_REAL(AHRS_H_Z)};
struct Int32Vect3 expected_imu;
INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp);
struct Int32Vect3 residual;
INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
ahrs_impl.rate_correction.p += residual.x/32/16;
ahrs_impl.rate_correction.q += residual.y/32/16;
ahrs_impl.rate_correction.r += residual.z/32/16;
ahrs_impl.high_rez_bias.p += -residual.x/32*1024;
ahrs_impl.high_rez_bias.q += -residual.y/32*1024;
ahrs_impl.high_rez_bias.r += -residual.z/32*1024;
INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
}
