From 0061caba1466848d35c86b1b315517fc9d86fea9 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Wed, 17 Sep 2014 18:33:42 +0200
Subject: [PATCH] [fix] typo in invariant filter and auto dt in mag module

---
 sw/airborne/modules/sensors/mag_hmc58xx.c     | 20 ++++++++++++++++++-
 .../subsystems/ins/ins_float_invariant.c      |  2 +-
 2 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/sw/airborne/modules/sensors/mag_hmc58xx.c b/sw/airborne/modules/sensors/mag_hmc58xx.c
index ee104c13234..6c5e31e7470 100644
--- 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
index 57f5658a426..5f36c681411 100644
--- 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;