From e3abe48436179d888475f13b4b1a7f71b6adbe56 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 8 Sep 2014 18:21:31 +0200 Subject: [PATCH] [ahrs] also pass dt to update_accel and update_mag Not so sure passing dt to the update functions is the best way to go... Is only used to update the gains of the cmpl filters right now. But it makes it possible to properly compute the gains and avoids having a dependency on sys_time in the AHRS. --- sw/airborne/firmwares/fixedwing/main_ap.c | 28 ++++- sw/airborne/firmwares/rotorcraft/main.c | 34 +++++- sw/airborne/modules/ins/ahrs_chimu_spi.c | 2 +- sw/airborne/subsystems/ahrs.c | 4 +- sw/airborne/subsystems/ahrs.h | 6 +- sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c | 79 ++++++------- sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h | 4 + sw/airborne/subsystems/ahrs/ahrs_float_dcm.c | 4 +- sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c | 4 +- .../subsystems/ahrs/ahrs_int_cmpl_euler.c | 4 +- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 106 +++++++++--------- .../subsystems/ahrs/ahrs_int_cmpl_quat.h | 4 + .../subsystems/ins/ins_float_invariant.c | 4 +- 13 files changed, 173 insertions(+), 110 deletions(-) diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 125f4f34451..725df196cbd 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -720,9 +720,21 @@ static inline void on_gps_solution( void ) { #if USE_AHRS #if USE_IMU static inline void on_accel_event( void ) { +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY) + // timestamp when last callback was received + static float last_ts = 0.f; + // current timestamp + float now_ts = get_sys_time_float(); + // dt between this and last callback + float dt = now_ts - last_ts; + last_ts = now_ts; +#else + const float dt = 1./AHRS_CORRECT_FREQUENCY; +#endif + ImuScaleAccel(imu); if (ahrs.status != AHRS_UNINIT) { - ahrs_update_accel(); + ahrs_update_accel(dt); } } @@ -771,9 +783,21 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) static inline void on_mag_event(void) { #if USE_MAGNETOMETER +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) + // timestamp when last callback was received + static float last_ts = 0.f; + // current timestamp + float now_ts = get_sys_time_float(); + // dt between this and last callback + float dt = now_ts - last_ts; + last_ts = now_ts; +#else + const float dt = 1./AHRS_MAG_CORRECT_FREQUENCY; +#endif + ImuScaleMag(imu); if (ahrs.status == AHRS_RUNNING) { - ahrs_update_mag(); + ahrs_update_mag(dt); } #endif } diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 4a2a9f63170..3b431937e8a 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -311,10 +311,25 @@ STATIC_INLINE void main_event( void ) { } static inline void on_accel_event( void ) { +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY) +PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.") + // timestamp when last callback was received + static float last_ts = 0.f; + // current timestamp + float now_ts = get_sys_time_float(); + // dt between this and last callback + float dt = now_ts - last_ts; + last_ts = now_ts; +#else +PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS accel update.") +PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) + const float dt = (1./AHRS_CORRECT_FREQUENCY); +#endif + ImuScaleAccel(imu); if (ahrs.status != AHRS_UNINIT) { - ahrs_update_accel(); + ahrs_update_accel(dt); } } @@ -363,11 +378,26 @@ static inline void on_gps_event(void) { } static inline void on_mag_event(void) { +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) +PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.") + // timestamp when last callback was received + static float last_ts = 0.f; + // current timestamp + float now_ts = get_sys_time_float(); + // dt between this and last callback + float dt = now_ts - last_ts; + last_ts = now_ts; +#else +PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.") +PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) + const float dt = (1./AHRS_MAG_CORRECT_FREQUENCY); +#endif + ImuScaleMag(imu); #if USE_MAGNETOMETER if (ahrs.status == AHRS_RUNNING) { - ahrs_update_mag(); + ahrs_update_mag(dt); } #endif diff --git a/sw/airborne/modules/ins/ahrs_chimu_spi.c b/sw/airborne/modules/ins/ahrs_chimu_spi.c index 79927ae9487..f27352bb0a5 100644 --- a/sw/airborne/modules/ins/ahrs_chimu_spi.c +++ b/sw/airborne/modules/ins/ahrs_chimu_spi.c @@ -134,5 +134,5 @@ void ahrs_update_gps(void) void ahrs_propagate(float dt __attribute__((unused))) { } -void ahrs_update_accel(void) { +void ahrs_update_accel(float dt __attribute__((unused))) { } diff --git a/sw/airborne/subsystems/ahrs.c b/sw/airborne/subsystems/ahrs.c index de557cefd14..c6695618618 100644 --- a/sw/airborne/subsystems/ahrs.c +++ b/sw/airborne/subsystems/ahrs.c @@ -34,8 +34,8 @@ struct Ahrs ahrs; void WEAK ahrs_propagate(float dt __attribute__((unused))) {} -void WEAK ahrs_update_accel(void) {} +void WEAK ahrs_update_accel(float dt __attribute__((unused))) {} -void WEAK ahrs_update_mag(void) {} +void WEAK ahrs_update_mag(float dt __attribute__((unused))) {} void WEAK ahrs_update_gps(void) {} diff --git a/sw/airborne/subsystems/ahrs.h b/sw/airborne/subsystems/ahrs.h index 7571e3829d4..0df80faec28 100644 --- a/sw/airborne/subsystems/ahrs.h +++ b/sw/airborne/subsystems/ahrs.h @@ -69,14 +69,16 @@ extern void ahrs_propagate(float dt); /** Update AHRS state with accerleration measurements. * Reads the global #imu data struct. * Does nothing if not implemented by specific AHRS algorithm. + * @param dt time difference since last update in seconds */ -extern void ahrs_update_accel(void); +extern void ahrs_update_accel(float dt); /** Update AHRS state with magnetometer measurements. * Reads the global #imu data struct. * Does nothing if not implemented by specific AHRS algorithm. + * @param dt time difference since last update in seconds */ -extern void ahrs_update_mag(void); +extern void ahrs_update_mag(float dt); /** Update AHRS state with GPS measurements. * Reads the global #gps data struct. diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index 0360977d54d..ef0b5c7af12 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -57,20 +57,6 @@ #warning "Using magnetometer _and_ GPS course to update heading. Probably better to if you want to use GPS course." #endif -#ifndef AHRS_PROPAGATE_FREQUENCY -#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY -#endif -PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) - -#ifndef AHRS_CORRECT_FREQUENCY -#define AHRS_CORRECT_FREQUENCY AHRS_PROPAGATE_FREQUENCY -#endif -PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) - -#ifndef AHRS_MAG_CORRECT_FREQUENCY -#define AHRS_MAG_CORRECT_FREQUENCY 50 -#endif -PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) /* * default gains for correcting attitude and bias from accel/mag @@ -95,8 +81,8 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) #endif -void ahrs_update_mag_full(void); -void ahrs_update_mag_2d(void); +void ahrs_update_mag_full(float dt); +void ahrs_update_mag_2d(float dt); void ahrs_update_mag_2d_dumb(void); static inline void compute_body_orientation_and_rates(void); @@ -183,6 +169,9 @@ void ahrs_init(void) { VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z); + ahrs_impl.accel_cnt = 0; + ahrs_impl.mag_cnt = 0; + #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att); register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); @@ -248,9 +237,15 @@ void ahrs_propagate(float dt) { #endif compute_body_orientation_and_rates(); + // increase accel and mag propagation counters + ahrs_impl.accel_cnt++; + ahrs_impl.mag_cnt++; } -void ahrs_update_accel(void) { +void ahrs_update_accel(float dt) { + // check if we had at least one propagation since last update + if (ahrs_impl.accel_cnt == 0) + return; /* last column of roation matrix = ltp z-axis in imu-frame */ struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2), @@ -313,34 +308,42 @@ void ahrs_update_accel(void) { } /* Complementary filter proportional gain. - * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY + * Kp = 2 * zeta * omega * weight * ahrs_impl.accel_cnt + * with ahrs_impl.accel_cnt beeing the number of propagations since last update */ const float gravity_rate_update_gain = -2 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega * - ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / (AHRS_CORRECT_FREQUENCY * 9.81); + ahrs_impl.weight * ahrs_impl.accel_cnt / 9.81; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain); + // reset accel propagation counter + ahrs_impl.accel_cnt = 0; + /* Complementary filter integral gain * Correct the gyro bias. - * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY + * Ki = (omega*weight)^2 * dt */ const float gravity_bias_update_gain = ahrs_impl.accel_omega * ahrs_impl.accel_omega * - ahrs_impl.weight * ahrs_impl.weight / (AHRS_CORRECT_FREQUENCY * 9.81); + ahrs_impl.weight * ahrs_impl.weight * dt / 9.81; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain); /* FIXME: saturate bias */ - } -void ahrs_update_mag(void) { +void ahrs_update_mag(float dt) { + // check if we had at least one propagation since last update + if (ahrs_impl.mag_cnt == 0) + return; #if AHRS_MAG_UPDATE_ALL_AXES - ahrs_update_mag_full(); + ahrs_update_mag_full(dt); #else - ahrs_update_mag_2d(); + ahrs_update_mag_2d(dt); #endif + // reset mag propagation counter + ahrs_impl.mag_cnt = 0; } -void ahrs_update_mag_full(void) { +void ahrs_update_mag_full(float dt) { struct FloatVect3 expected_imu; FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_impl.ltp_to_imu_rmat, ahrs_impl.mag_h); @@ -355,24 +358,23 @@ void ahrs_update_mag_full(void) { // DISPLAY_FLOAT_VECT3("# residual", residual); /* Complementary filter proportional gain. - * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + * Kp = 2 * zeta * omega * weight * ahrs_impl.mag_cnt + * with ahrs_impl.mag_cnt beeing the number of propagations since last update */ - const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * - AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY; + const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * ahrs_impl.mag_cnt; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); /* Complementary filter integral gain * Correct the gyro bias. - * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY + * Ki = (omega*weight)^2 * dt */ - const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) / - AHRS_MAG_CORRECT_FREQUENCY; + const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) * dt; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain); } -void ahrs_update_mag_2d(void) { +void ahrs_update_mag_2d(float dt) { struct FloatVect2 expected_ltp; VECT2_COPY(expected_ltp, ahrs_impl.mag_h); @@ -401,18 +403,17 @@ void ahrs_update_mag_2d(void) { /* Complementary filter proportional gain. - * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + * Kp = 2 * zeta * omega * weight * ahrs_impl.mag_cnt + * with ahrs_impl.mag_cnt beeing the number of propagations since last update */ - const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * - AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY; + const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * ahrs_impl.mag_cnt; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); /* Complementary filter integral gain * Correct the gyro bias. - * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY + * Ki = (omega*weight)^2 * dt */ - const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) / - AHRS_MAG_CORRECT_FREQUENCY; + const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) * dt; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain); } diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h index 3d756b3d03d..b1d65ee32db 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h @@ -56,6 +56,10 @@ struct AhrsFloatCmpl { bool_t heading_aligned; struct FloatVect3 mag_h; + + /* internal counters for the gains */ + uint16_t accel_cnt; ///< number of propagations since last accel update + uint16_t mag_cnt; ///< number of propagations since last mag update }; extern struct AhrsFloatCmpl ahrs_impl; diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index e282c7a9411..7415468e067 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -202,7 +202,7 @@ void ahrs_update_gps(void) } -void ahrs_update_accel(void) +void ahrs_update_accel(float dt __attribute__((unused))) { ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); @@ -233,7 +233,7 @@ PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses GPS acceleration.") } -void ahrs_update_mag(void) +void ahrs_update_mag(float dt __attribute__((unused))) { #if USE_MAGNETOMETER #warning MAGNETOMETER FEEDBACK NOT TESTED YET diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c index 4c6d7d1dc99..d3d9edea7a8 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c @@ -124,7 +124,7 @@ void ahrs_propagate(float dt) { set_body_state_from_quat(); } -void ahrs_update_accel(void) { +void ahrs_update_accel(float dt __attribute__((unused))) { struct FloatVect3 imu_g; ACCELS_FLOAT_OF_BFP(imu_g, imu.accel); const float alpha = 0.92; @@ -138,7 +138,7 @@ void ahrs_update_accel(void) { } -void ahrs_update_mag(void) { +void ahrs_update_mag(float dt __attribute__((unused))) { struct FloatVect3 imu_h; MAGS_FLOAT_OF_BFP(imu_h, imu.mag); update_state(&ahrs_impl.mag_h, &imu_h, &ahrs_impl.mag_noise); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c index 85d2513ac57..ebf457c8c31 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c @@ -239,7 +239,7 @@ void ahrs_propagate(float dt __attribute__((unused))) { } -void ahrs_update_accel(void) { +void ahrs_update_accel(float dt __attribute__((unused))) { #if USE_NOISE_CUT || USE_NOISE_FILTER static struct Int32Vect3 last_accel = { 0, 0, 0 }; @@ -260,7 +260,7 @@ void ahrs_update_accel(void) { } -void ahrs_update_mag(void) { +void ahrs_update_mag(float dt __attribute__((unused))) { get_psi_measurement_from_mag(&ahrs_impl.measurement.psi, ahrs_impl.ltp_to_imu_euler.phi, ahrs_impl.ltp_to_imu_euler.theta, imu.mag); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 2a5f9f2c5e0..37f59a95919 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -63,28 +63,6 @@ PRINT_CONFIG_MSG("LOW PASS FILTER ON GYRO RATES") #error "AHRS_USE_GPS_HEADING needs USE_GPS to be TRUE" #endif -#ifndef AHRS_PROPAGATE_FREQUENCY -#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY -#endif -PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) -#if AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY -#warning PERIODIC_FREQUENCY should be >= AHRS_PROPAGATE_FREQUENCY -#endif - -#ifndef AHRS_CORRECT_FREQUENCY -#define AHRS_CORRECT_FREQUENCY AHRS_PROPAGATE_FREQUENCY -#endif -PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) -#if AHRS_CORRECT_FREQUENCY > PERIODIC_FREQUENCY -#warning PERIODIC_FREQUENCY should be >= AHRS_CORRECT_FREQUENCY -#endif - -#ifndef AHRS_MAG_CORRECT_FREQUENCY -#define AHRS_MAG_CORRECT_FREQUENCY 50 -#endif -#if USE_MAGNETOMETER -PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) -#endif /* * default gains for correcting attitude and bias from accel/mag @@ -130,8 +108,8 @@ PRINT_CONFIG_VAR(AHRS_MAG_ZETA) struct AhrsIntCmplQuat ahrs_impl; static inline void set_body_state_from_quat(void); -static inline void UNUSED ahrs_update_mag_full(void); -static inline void ahrs_update_mag_2d(void); +static inline void UNUSED ahrs_update_mag_full(float dt); +static inline void ahrs_update_mag_2d(float dt); #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" @@ -213,6 +191,9 @@ void ahrs_init(void) { VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); + ahrs_impl.accel_cnt = 0; + ahrs_impl.mag_cnt = 0; + #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat); register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler); @@ -275,32 +256,36 @@ void ahrs_propagate(float dt) { set_body_state_from_quat(); + // increase accel and mag propagation counters + ahrs_impl.accel_cnt++; + ahrs_impl.mag_cnt++; } void ahrs_set_accel_gains(void) { - /* Complementary filter proportionnal gain - * Kp = 2 * omega * zeta * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY + /* Complementary filter proportionnal gain (without frequency correction) + * Kp = 2 * omega * zeta * * accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain) * accel_inv_kp = 4096 * 9.81 / Kp */ ahrs_impl.accel_inv_kp = 4096 * 9.81 / - (2 * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * - AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY); + (2 * ahrs_impl.accel_omega * ahrs_impl.accel_zeta); /* Complementary filter integral gain - * Ki = omega^2 / AHRS_CORRECT_FREQUENCY + * Ki = omega^2 * * accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain) * accel_inv_ki = 2^5 / 2^16 * 9.81 / Ki * accel_inv_ki = 9.81 / 2048 / Ki */ - ahrs_impl.accel_inv_ki = AHRS_CORRECT_FREQUENCY * 9.81 / 2048 / - (ahrs_impl.accel_omega * ahrs_impl.accel_omega); + ahrs_impl.accel_inv_ki = 9.81 / 2048 / (ahrs_impl.accel_omega * ahrs_impl.accel_omega); } -void ahrs_update_accel(void) { +void ahrs_update_accel(float dt) { + // check if we had at least one propagation since last update + if (ahrs_impl.accel_cnt == 0) + return; // c2 = ltp z-axis in imu-frame struct Int32RMat ltp_to_imu_rmat; @@ -373,7 +358,9 @@ void ahrs_update_accel(void) { } /* Complementary filter proportional gain. - * Kp = 2 * zeta * omega * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY + * Kp = 2 * zeta * omega + * final Kp with frequency correction = Kp * ahrs_impl.accel_cnt + * with ahrs_impl.accel_cnt beeing the number of propagations since last update * * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * rate_correction FRAC: RATE_FRAC = 12 @@ -384,19 +371,23 @@ void ahrs_update_accel(void) { * accel_inv_kp = 4096 * 9.81 / Kp * * inv_rate_scale = 1 / (weight * Kp * FRAC_conversion / cross_product_gain) - * inv_rate_scale = inv_kp / weight + * inv_rate_scale = 1 / Kp / weight + * inv_rate_scale = accel_inv_kp / accel_cnt / weight */ - int32_t inv_rate_scale = (int32_t)(ahrs_impl.accel_inv_kp / ahrs_impl.weight); + int32_t inv_rate_scale = (int32_t)(ahrs_impl.accel_inv_kp / ahrs_impl.accel_cnt + / ahrs_impl.weight); Bound(inv_rate_scale, 8192, 4194304); ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale; ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale; ahrs_impl.rate_correction.r -= residual.z / inv_rate_scale; + // reset accel propagation counter + ahrs_impl.accel_cnt = 0; /* Complementary filter integral gain * Correct the gyro bias. - * Ki = omega^2 / AHRS_CORRECT_FREQUENCY + * Ki = omega^2 * dt * * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * high_rez_bias = RATE_FRAC+28 = 40 @@ -411,7 +402,7 @@ void ahrs_update_accel(void) { */ int32_t inv_bias_gain = (int32_t)(ahrs_impl.accel_inv_ki / - (ahrs_impl.weight * ahrs_impl.weight)); + (dt * ahrs_impl.weight * ahrs_impl.weight)); Bound(inv_bias_gain, 8, 65536) ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5; @@ -422,25 +413,28 @@ void ahrs_update_accel(void) { } -void ahrs_update_mag(void) { +void ahrs_update_mag(float dt) { + // check if we had at least one propagation since last update + if (ahrs_impl.mag_cnt == 0) + return; #if AHRS_MAG_UPDATE_ALL_AXES - ahrs_update_mag_full(); + ahrs_update_mag_full(dt); #else - ahrs_update_mag_2d(); + ahrs_update_mag_2d(dt); #endif + // reset mag propagation counter + ahrs_impl.mag_cnt = 0; } void ahrs_set_mag_gains(void) { /* Complementary filter proportionnal gain = 2*omega*zeta */ - ahrs_impl.mag_kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * - AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY; + ahrs_impl.mag_kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega; /* Complementary filter integral gain = omega^2 */ - ahrs_impl.mag_ki = ahrs_impl.mag_omega * ahrs_impl.mag_omega / - AHRS_MAG_CORRECT_FREQUENCY; + ahrs_impl.mag_ki = ahrs_impl.mag_omega * ahrs_impl.mag_omega; } -static inline void ahrs_update_mag_full(void) { +static inline void ahrs_update_mag_full(float dt) { struct Int32RMat ltp_to_imu_rmat; INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); @@ -452,7 +446,9 @@ static inline void ahrs_update_mag_full(void) { INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu); /* Complementary filter proportionnal gain. - * Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + * Kp = 2 * mag_zeta * mag_omega + * final Kp with frequency correction = Kp * ahrs_impl.mag_cnt + * with ahrs_impl.mag_cnt beeing the number of propagations since last update * * residual FRAC: 2 * MAG_FRAC = 22 * rate_correction FRAC: RATE_FRAC = 12 @@ -462,7 +458,7 @@ static inline void ahrs_update_mag_full(void) { * inv_rate_gain = 1024 / Kp */ - const int32_t inv_rate_gain = (int32_t)(1024.0 / ahrs_impl.mag_kp); + const int32_t inv_rate_gain = (int32_t)(1024.0 / (ahrs_impl.mag_kp * ahrs_impl.mag_cnt)); ahrs_impl.rate_correction.p += residual.x / inv_rate_gain; ahrs_impl.rate_correction.q += residual.y / inv_rate_gain; @@ -470,7 +466,7 @@ static inline void ahrs_update_mag_full(void) { /* Complementary filter integral gain * Correct the gyro bias. - * Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY + * Ki = omega^2 * dt * * residual FRAC: 2* MAG_FRAC = 22 * high_rez_bias FRAC: RATE_FRAC+28 = 40 @@ -478,7 +474,7 @@ static inline void ahrs_update_mag_full(void) { * * bias_gain = Ki * FRAC_conversion = Ki * 2^18 */ - const int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * (1<<18)); + const int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * dt * (1<<18)); ahrs_impl.high_rez_bias.p -= residual.x * bias_gain; ahrs_impl.high_rez_bias.q -= residual.y * bias_gain; @@ -490,7 +486,7 @@ static inline void ahrs_update_mag_full(void) { } -static inline void ahrs_update_mag_2d(void) { +static inline void ahrs_update_mag_2d(float dt) { struct Int32Vect2 expected_ltp = {ahrs_impl.mag_h.x, ahrs_impl.mag_h.y}; /* normalize expected ltp in 2D (x,y) */ @@ -517,7 +513,9 @@ static inline void ahrs_update_mag_2d(void) { INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp); /* Complementary filter proportionnal gain. - * Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + * Kp = 2 * mag_zeta * mag_omega + * final Kp with frequency correction = Kp * ahrs_impl.mag_cnt + * with ahrs_impl.mag_cnt beeing the number of propagations since last update * * residual_imu FRAC = residual_ltp FRAC = 17 * rate_correction FRAC: RATE_FRAC = 12 @@ -526,7 +524,7 @@ static inline void ahrs_update_mag_2d(void) { * inv_rate_gain = 1 / Kp / FRAC_conversion * inv_rate_gain = 32 / Kp */ - int32_t inv_rate_gain = (int32_t)(32.0 / ahrs_impl.mag_kp); + int32_t inv_rate_gain = (int32_t)(32.0 / (ahrs_impl.mag_kp * ahrs_impl.mag_cnt)); ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain); ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain); @@ -534,7 +532,7 @@ static inline void ahrs_update_mag_2d(void) { /* Complementary filter integral gain * Correct the gyro bias. - * Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY + * Ki = omega^2 * dt * * residual_imu FRAC = residual_ltp FRAC = 17 * high_rez_bias FRAC: RATE_FRAC+28 = 40 @@ -542,7 +540,7 @@ static inline void ahrs_update_mag_2d(void) { * * bias_gain = Ki * FRAC_conversion = Ki * 2^23 */ - int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * (1 << 23)); + int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * dt * (1 << 23)); ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain); ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index f8682b9b7f2..e47201b8975 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -86,6 +86,10 @@ struct AhrsIntCmplQuat { * only update through #ahrs_int_cmpl_quat_SetMagZeta */ float mag_zeta; + + /* internal counters for the gains */ + uint16_t accel_cnt; ///< number of propagations since last accel update + uint16_t mag_cnt; ///< number of propagations since last mag update }; extern struct AhrsIntCmplQuat ahrs_impl; diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index 28cccd3bf1d..613e95ac482 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -510,13 +510,13 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres } } -void ahrs_update_accel(void) { +void ahrs_update_accel(float dt __attribute__((unused))) { } // assume mag is dead when values are not moving anymore #define MAG_FROZEN_COUNT 30 -void ahrs_update_mag(void) { +void ahrs_update_mag(float dt __attribute__((unused)) { static uint32_t mag_frozen_count = MAG_FROZEN_COUNT; static int32_t last_mx = 0;