[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

@@ -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
 }

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
 

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))) {
 }

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) {}

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.

sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c

@@ -57,20 +57,6 @@
 #warning "Using magnetometer _and_ GPS course to update heading. Probably better to <configure name="USE_MAGNETOMETER" value="0"/> 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);
 }
 

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;

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

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);

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);