Merge pull request paparazzi#818 from paparazzi/propagate_dt

Pass dt to ahrs/ins propagation and updates instead of using the defines in the implementations.

The fixedwing ins_alt_float now uses the real dt for alt_kalman update from baro or gps.

For the AHRS: If the frequencies are not defines or USE_AUTO_AHRS_FREQ is TRUE, the dt is calculated from the system time.
Should be cleaner like this and less prone to misconfiguration, possible drawback however is that you get some more jitter on the DT.

The complementary filters now don't use the AHRS_x_FREQUENCY defines for calculation of the gains anymore and instead count the number of propagations since last update.

conf/firmwares/subsystems/rotorcraft/ins.makefile

@@ -9,6 +9,3 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c
 #  vertical filter float version
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c
 
-ifdef INS_PROPAGATE_FREQUENCY
-$(TARGET).CFLAGS += -DINS_PROPAGATE_FREQUENCY=$(INS_PROPAGATE_FREQUENCY)
-endif

conf/firmwares/subsystems/rotorcraft/ins_extended.makefile

@@ -9,7 +9,3 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c
 #  vertical filter float version
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_extended_float.c
 $(TARGET).CFLAGS += -DUSE_VFF_EXTENDED
-
-ifdef INS_PROPAGATE_FREQUENCY
-$(TARGET).CFLAGS += -DINS_PROPAGATE_FREQUENCY=$(INS_PROPAGATE_FREQUENCY)
-endif

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -130,19 +130,12 @@ PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
 
 #if USE_AHRS && USE_IMU
 
-#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 PERIODIC_FREQUENCY
-#endif
-PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
-
+#ifdef AHRS_PROPAGATE_FREQUENCY
 #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
 #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
 INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ",AHRS_PROPAGATE_FREQUENCY)
 #endif
+#endif
 
 static inline void on_gyro_event( void );
 static inline void on_accel_event( void );
@@ -605,7 +598,8 @@ void sensors_task( void ) {
 
   //FIXME: this is just a kludge
 #if USE_AHRS && defined SITL && !USE_NPS
-  ahrs_propagate();
+  // dt is not really used in ahrs_sim
+  ahrs_propagate(1./PERIODIC_FREQUENCY);
 #endif
 
 #if USE_GPS
@@ -728,67 +722,58 @@ 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)
+PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.")
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
+  // current timestamp
+  uint32_t now_ts = get_sys_time_usec();
+  // 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_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(dt);
+  }
 }
 
 static inline void on_gyro_event( void ) {
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.")
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
+  // current timestamp
+  uint32_t now_ts = get_sys_time_usec();
+  // 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 AHRS/INS propagation.")
+PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
+  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
+#endif
 
   ahrs_timeout_counter = 0;
 
+  ImuScaleGyro(imu);
+
 #if USE_AHRS_ALIGNER
   // Run aligner on raw data as it also makes averages.
   if (ahrs.status == AHRS_UNINIT) {
-    ImuScaleGyro(imu);
-    ImuScaleAccel(imu);
     ahrs_aligner_run();
     if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
       ahrs_align();
     return;
   }
 #endif
 
-#if PERIODIC_FREQUENCY == 60
-  ImuScaleGyro(imu);
-  ImuScaleAccel(imu);
-
-  ahrs_propagate();
-  ahrs_update_accel();
-
-#else //PERIODIC_FREQUENCY
-  static uint8_t _reduced_propagation_rate = 0;
-  static uint8_t _reduced_correction_rate = 0;
-  static struct Int32Vect3 acc_avg;
-  static struct Int32Rates gyr_avg;
-
-  RATES_ADD(gyr_avg, imu.gyro_unscaled);
-  VECT3_ADD(acc_avg, imu.accel_unscaled);
-
-  _reduced_propagation_rate++;
-  if (_reduced_propagation_rate < (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY))
-  {
-    return;
-  }
-  else
-  {
-    _reduced_propagation_rate = 0;
-
-    RATES_SDIV(imu.gyro_unscaled, gyr_avg, (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY) );
-    INT_RATES_ZERO(gyr_avg);
-
-    ImuScaleGyro(imu);
-
-    ahrs_propagate();
-
-    _reduced_correction_rate++;
-    if (_reduced_correction_rate >= (AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY))
-    {
-      _reduced_correction_rate = 0;
-      VECT3_SDIV(imu.accel_unscaled, acc_avg, (PERIODIC_FREQUENCY / AHRS_CORRECT_FREQUENCY) );
-      INT_VECT3_ZERO(acc_avg);
-      ImuScaleAccel(imu);
-      ahrs_update_accel();
-    }
-  }
-#endif //PERIODIC_FREQUENCY
+  ahrs_propagate(dt);
 
 #if defined SITL && USE_NPS
   if (nps_bypass_ahrs) sim_overwrite_ahrs();
@@ -803,14 +788,28 @@ static inline void on_gyro_event( void ) {
 static inline void on_mag_event(void)
 {
 #if USE_MAGNETOMETER
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.")
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
+  // current timestamp
+  uint32_t now_ts = get_sys_time_usec();
+  // 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_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 (ahrs.status == AHRS_RUNNING) {
-    ahrs_update_mag();
+    ahrs_update_mag(dt);
   }
 #endif
 }
 
 #endif // USE_IMU
 
 #endif // USE_AHRS
-

sw/airborne/firmwares/rotorcraft/main.c

@@ -307,14 +307,43 @@ 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 in usec when last callback was received
+  static uint32_t last_ts = 0;
+  // current timestamp
+  uint32_t now_ts = get_sys_time_usec();
+  // dt between this and last callback
+  float dt = (float)(now_ts - last_ts) / 1e6;
+  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);
   }
 }
 
 static inline void on_gyro_event( void ) {
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.")
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
+  // current timestamp
+  uint32_t now_ts = get_sys_time_usec();
+  // 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 AHRS/INS propagation.")
+PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
+  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
+#endif
 
   ImuScaleGyro(imu);
 
@@ -324,11 +353,11 @@ static inline void on_gyro_event( void ) {
       ahrs_align();
   }
   else {
-    ahrs_propagate();
+    ahrs_propagate(dt);
 #ifdef SITL
     if (nps_bypass_ahrs) sim_overwrite_ahrs();
 #endif
-    ins_propagate();
+    ins_propagate(dt);
   }
 #ifdef USE_VEHICLE_INTERFACE
   vi_notify_imu_available();
@@ -348,10 +377,25 @@ static inline void on_mag_event(void) {
   ImuScaleMag(imu);
 
 #if USE_MAGNETOMETER
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
+PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.")
+  // timestamp in usec when last callback was received
+  static uint32_t last_ts = 0;
+  // current timestamp
+  uint32_t now_ts = get_sys_time_usec();
+  // 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_MAG_CORRECT_FREQUENCY for AHRS mag update.")
+PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
+  const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
+#endif
+
   if (ahrs.status == AHRS_RUNNING) {
-    ahrs_update_mag();
+    ahrs_update_mag(dt);
   }
-#endif
+#endif // USE_MAGNETOMETER
 
 #ifdef USE_VEHICLE_INTERFACE
   vi_notify_mag_available();

sw/airborne/mcu_periph/sys_time.h

@@ -118,7 +118,7 @@ static inline bool_t sys_time_check_and_ack_timer(tid_t id) {
 
 /**
  * Get the time in seconds since startup.
- * @return current system time as float
+ * @return current system time as float with sys_time.resolution
  */
 static inline float get_sys_time_float(void) {
   return (float)(sys_time.nb_sec + (float)(sys_time.nb_sec_rem) / sys_time.cpu_ticks_per_sec);

sw/airborne/modules/ins/ahrs_chimu_spi.c

@@ -131,8 +131,8 @@ void ahrs_update_gps(void)
   // Downlink Send
 }
 
-void ahrs_propagate(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

@@ -32,10 +32,10 @@ struct Ahrs ahrs;
 #define WEAK __attribute__((weak))
 // weak functions, used if not explicitly provided by implementation
 
-void WEAK ahrs_propagate(void) {}
+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

@@ -62,20 +62,23 @@ extern void ahrs_align(void);
 /** Propagation. Usually integrates the gyro rates to angles.
  *  Reads the global #imu data struct.
  *  Does nothing if not implemented by specific AHRS algorithm.
+ *  @param dt time difference since last propagation in seconds
  */
-extern void ahrs_propagate(void);
+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_ardrone2.c

@@ -100,7 +100,7 @@ static void dump(const void *_b, size_t s) {
 }
 #endif
 
-void ahrs_propagate(void) {
+void ahrs_propagate(float dt __attribute__((unused))) {
   int l;
 
   //Recieve the main packet