[fix] typo in invariant filter and auto dt in mag module

paparazzi · Sep 17, 2014 · 0061cab · 0061cab
1 parent 9e3510e
commit 0061cab
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 2 deletions.
diff --git a/sw/airborne/modules/sensors/mag_hmc58xx.c b/sw/airborne/modules/sensors/mag_hmc58xx.c
@@ -57,7 +57,18 @@ void mag_hmc58xx_module_periodic(void) {
 }
 
 void mag_hmc58xx_module_event(void) {
+#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;
+#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
+
   hmc58xx_event(&mag_hmc58xx);
+
 #if MODULE_HMC58XX_UPDATE_AHRS
   if (mag_hmc58xx.data_available) {
     // set channel order
@@ -72,7 +83,14 @@ void mag_hmc58xx_module_event(void) {
     ImuScaleMag(imu);
     // update ahrs
     if (ahrs.status == AHRS_RUNNING) {
-      ahrs_update_mag();
+#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
+      // 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;
+#endif
+      ahrs_update_mag(dt);
     }
     mag_hmc58xx.data_available = FALSE;
   }

diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c
@@ -526,7 +526,7 @@ 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(float dt __attribute__((unused)) {
+void ahrs_update_mag(float dt __attribute__((unused))) {
   static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
   static int32_t last_mx = 0;