From 96f1d7a229d9ceae81cc051792827229558488a7 Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Mon, 13 Oct 2014 19:31:44 +0200
Subject: [PATCH] [fixedwing] pass imu measurements to ahrs functions to test
 the refactoring

---
 sw/airborne/firmwares/fixedwing/main_ap.c | 20 ++++++++++++--------
 1 file changed, 12 insertions(+), 8 deletions(-)

diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 10f872717bc..36bb189347d 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -199,6 +199,7 @@ void init_ap( void ) {
 
 #if USE_AHRS
   ahrs_init();
+  DefaultAhrsRegister();
 #endif
 
 #if USE_AHRS && USE_IMU
@@ -599,7 +600,7 @@ void sensors_task( void ) {
   //FIXME: this is just a kludge
 #if USE_AHRS && defined SITL && !USE_NPS
   // dt is not really used in ahrs_sim
-  ahrs_propagate(1./PERIODIC_FREQUENCY);
+  ahrs_propagate(&imu.gyro, 1./PERIODIC_FREQUENCY);
 #endif
 
 #if USE_GPS
@@ -738,8 +739,8 @@ PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
 #endif
 
   imu_scale_accel(&imu);
-  if (ahrs.status != AHRS_UNINIT) {
-    ahrs_update_accel(dt);
+  if (ahrs.status == AHRS_RUNNING) {
+    ahrs_update_accel(&imu.accel, dt);
   }
 }
 
@@ -765,15 +766,18 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
 
 #if USE_AHRS_ALIGNER
   // Run aligner on raw data as it also makes averages.
-  if (ahrs.status == AHRS_UNINIT) {
+  if (ahrs.status == AHRS_REGISTERED) {
     ahrs_aligner_run();
-    if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
-      ahrs_align();
+    if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
+      if (ahrs_align(&ahrs_aligner.lp_gyro, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag)) {
+        ahrs.status = AHRS_RUNNING;
+      }
+    }
     return;
   }
 #endif
 
-  ahrs_propagate(dt);
+  ahrs_propagate(&imu.gyro_prev, dt);
 
 #if defined SITL && USE_NPS
   if (nps_bypass_ahrs) sim_overwrite_ahrs();
@@ -805,7 +809,7 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
 
   imu_scale_mag(&imu);
   if (ahrs.status == AHRS_RUNNING) {
-    ahrs_update_mag(dt);
+    ahrs_update_mag(&imu.mag, dt);
   }
 #endif
 }