[fixedwing] pass imu measurements to ahrs functions to test the refac…

…toring
paparazzi · Oct 13, 2014 · 96f1d7a · 96f1d7a
1 parent 4374148
commit 96f1d7a
Showing 1 changed file with 12 additions and 8 deletions.
diff --git 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
 }