[ahrs] add possibility for fixed dt again

sw/airborne/firmwares/rotorcraft/main.c

@@ -325,19 +325,6 @@ static inline void on_gyro_event( void ) {
   // current timestamp
   uint32_t now_ts = get_sys_time_usec();
 
-#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
-PRINT_CONFIG_MSG("Calculating dt for INS propagation.")
-  // timestamp in usec when last callback was received
-  static uint32_t last_ts = 0;
-  // dt between this and last callback in seconds
-  float dt = (float)(now_ts - last_ts) / 1e6;
-  last_ts = now_ts;
-#else
-PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for INS propagation.")
-PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
-  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
-#endif
-
   imu_scale_gyro(&imu);
 
   AbiSendMsgIMU_GYRO_INT32(1, &now_ts, &imu.gyro_prev);
@@ -356,6 +343,18 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
   if (nps_bypass_ahrs) sim_overwrite_ahrs();
 #endif
 
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for INS propagation.")
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
+  // dt between this and last callback in seconds
+  float dt = (float)(now_ts - last_ts) / 1e6;
+  last_ts = now_ts;
+#else
+PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for INS propagation.")
+PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
+  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
+#endif
   ins_propagate(dt);
 
 #ifdef USE_VEHICLE_INTERFACE

sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c

@@ -106,34 +106,66 @@ static abi_event aligner_ev;
 static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
                     const struct Int32Rates* gyro)
 {
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl propagation.")
+  /* timestamp in usec when last callback was received */
   static uint32_t last_stamp = 0;
+
   if (last_stamp > 0 && ahrs_fc.status == AHRS_FC_RUNNING) {
     float dt = (float)(*stamp - last_stamp) * 1e-6;
     ahrs_fc_propagate((struct Int32Rates*)gyro, dt);
   }
   last_stamp = *stamp;
+#else
+PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS float_cmpl propagation.")
+PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
+  if (ahrs_fc.status == AHRS_FC_RUNNING) {
+    const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
+    ahrs_fc_propagate((struct Int32Rates*)gyro, dt);
+  }
+#endif
 }
 
 static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
                      const struct Int32Vect3* accel)
 {
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl accel update.")
   static uint32_t last_stamp = 0;
   if (last_stamp > 0 && ahrs_fc.status == AHRS_FC_RUNNING) {
     float dt = (float)(*stamp - last_stamp) * 1e-6;
     ahrs_fc_update_accel((struct Int32Vect3*)accel, dt);
   }
   last_stamp = *stamp;
+#else
+PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS float_cmpl accel update.")
+PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
+  if (ahrs_fc.status == AHRS_FC_RUNNING) {
+    const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
+    ahrs_fc_update_accel((struct Int32Vect3*)accel, dt);
+  }
+#endif
 }
 
 static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
                    const struct Int32Vect3* mag)
 {
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl mag update.")
   static uint32_t last_stamp = 0;
   if (last_stamp > 0 && ahrs_fc.status == AHRS_FC_RUNNING) {
     float dt = (float)(*stamp - last_stamp) * 1e-6;
     ahrs_fc_update_mag((struct Int32Vect3*)mag, dt);
   }
   last_stamp = *stamp;
+#else
+PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS float_cmpl mag update.")
+PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
+  if (ahrs_fc.status == AHRS_FC_RUNNING) {
+    const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
+    ahrs_fc_update_mag((struct Int32Vect3*)mag, dt);
+  }
+#endif
 }
 
 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,

sw/airborne/subsystems/ahrs/ahrs_float_dcm.c

@@ -108,12 +108,23 @@ static abi_event aligner_ev;
 static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
                     const struct Int32Rates* gyro)
 {
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.")
+  /* timestamp in usec when last callback was received */
   static uint32_t last_stamp = 0;
   if (last_stamp > 0 && ahrs_dcm.status == AHRS_DCM_RUNNING) {
     float dt = (float)(*stamp - last_stamp) * 1e-6;
     ahrs_dcm_propagate((struct Int32Rates*)gyro, dt);
   }
   last_stamp = *stamp;
+#else
+PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS dcm propagation.")
+PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
+  if (ahrs_dcm.status == AHRS_DCM_RUNNING) {
+    const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
+    ahrs_dcm_propagate((struct Int32Rates*)gyro, dt);
+  }
+#endif
 }
 
 static void accel_cb(uint8_t sender_id __attribute__((unused)),

sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c

@@ -85,12 +85,24 @@ static abi_event aligner_ev;
 static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
                     const struct Int32Rates* gyro)
 {
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.")
+  /* timestamp in usec when last callback was received */
   static uint32_t last_stamp = 0;
+
   if (last_stamp > 0 && ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
     float dt = (float)(*stamp - last_stamp) * 1e-6;
     ahrs_mlkf_propagate((struct Int32Rates*)gyro, dt);
   }
   last_stamp = *stamp;
+#else
+PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_MLKF propagation.")
+PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
+  if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
+    const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
+    ahrs_mlkf_propagate((struct Int32Rates*)gyro, dt);
+  }
+#endif
 }
 
 static void accel_cb(uint8_t sender_id __attribute__((unused)),

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c

@@ -87,23 +87,43 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* s
 static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
                      const struct Int32Vect3* accel)
 {
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_euler propagation.")
   static uint32_t last_stamp = 0;
   if (last_stamp > 0 && ahrs_ice.status == AHRS_ICE_RUNNING) {
     float dt = (float)(*stamp - last_stamp) * 1e-6;
     ahrs_ice_update_accel((struct Int32Vect3*)accel, dt);
   }
   last_stamp = *stamp;
+#else
+PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS int_cmpl_euler propagation.")
+PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
+  if (ahrs_ice.status == AHRS_ICE_RUNNING) {
+    const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
+    ahrs_ice_propagate((struct Int32Rates*)gyro, dt);
+  }
+#endif
 }
 
 static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
                    const struct Int32Vect3* mag)
 {
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_euler accel update.")
   static uint32_t last_stamp = 0;
   if (last_stamp > 0 && ahrs_ice.status == AHRS_ICE_RUNNING) {
     float dt = (float)(*stamp - last_stamp) * 1e-6;
     ahrs_ice_update_mag((struct Int32Vect3*)mag, dt);
   }
   last_stamp = *stamp;
+#else
+PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.")
+PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
+  if (ahrs_ice.status == AHRS_ICE_RUNNING) {
+    const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
+    ahrs_ice_update_accel((struct Int32Rates*)accel, dt);
+  }
+#endif
 }
 
 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c

@@ -124,35 +124,66 @@ static abi_event aligner_ev;
 static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
                     const struct Int32Rates* gyro)
 {
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat propagation.")
+  /* timestamp in usec when last callback was received */
   static uint32_t last_stamp = 0;
   if (last_stamp > 0 && ahrs_icq.status == AHRS_ICQ_RUNNING) {
     float dt = (float)(*stamp - last_stamp) * 1e-6;
     ahrs_icq_propagate((struct Int32Rates*)gyro, dt);
   }
   last_stamp = *stamp;
+#else
+PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS int_cmpl_quat propagation.")
+PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
+  if (ahrs_icq.status == AHRS_ICQ_RUNNING) {
+    const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
+    ahrs_icq_propagate((struct Int32Rates*)gyro, dt);
+  }
+#endif
 }
 
 static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
                      const struct Int32Vect3* accel)
 {
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat accel update.")
   static uint32_t last_stamp = 0;
   if (last_stamp > 0 && ahrs_icq.status == AHRS_ICQ_RUNNING) {
     float dt = (float)(*stamp - last_stamp) * 1e-6;
     ahrs_icq_update_accel((struct Int32Vect3*)accel, dt);
   }
   last_stamp = *stamp;
+#else
+PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.")
+PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
+  if (ahrs_icq.status == AHRS_ICQ_RUNNING) {
+    const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
+    ahrs_icq_update_accel((struct Int32Rates*)accel, dt);
+  }
+#endif
 }
 
 static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
                    const struct Int32Vect3* mag)
 {
 #if USE_MAGNETOMETER
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat mag update.")
   static uint32_t last_stamp = 0;
   if (last_stamp > 0 && ahrs_icq.status == AHRS_ICQ_RUNNING) {
     float dt = (float)(*stamp - last_stamp) * 1e-6;
     ahrs_icq_update_mag((struct Int32Vect3*)mag, dt);
   }
   last_stamp = *stamp;
+#else
+PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS int_cmpl_quat mag update.")
+PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
+  if (ahrs_icq.status == AHRS_ICQ_RUNNING) {
+    const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
+    ahrs_icq_update_mag((struct Int32Vect3*)mag, dt);
+  }
+#endif
 #endif
 }