From d66331500be82a98a4553d0822fdd9d5765e7d3a Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Tue, 18 Aug 2009 12:42:56 +0000
Subject: [PATCH] sim overwrite ahrs before vertical filter

---
 sw/airborne/booz/booz2_main.c         | 9 +++++++++
 sw/simulator/nps/nps_autopilot_booz.c | 4 +---
 sw/simulator/nps/nps_autopilot_booz.h | 1 +
 3 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/sw/airborne/booz/booz2_main.c b/sw/airborne/booz/booz2_main.c
index 686e62f706..744be3ade5 100644
--- a/sw/airborne/booz/booz2_main.c
+++ b/sw/airborne/booz/booz2_main.c
@@ -53,6 +53,10 @@
 
 #include "booz2_main.h"
 
+#ifdef SITL
+#include "nps_autopilot_booz.h"
+#endif
+
 static inline void on_gyro_accel_event( void );
 static inline void on_baro_event( void );
 static inline void on_gps_event( void );
@@ -180,6 +184,11 @@ static inline void on_gyro_accel_event( void ) {
     RunOnceEvery(50, booz_ahrs_update_accel());
 #endif
     //    booz2_filter_attitude_update();
+#ifdef SITL
+    if (nps_bypass_ahrs) {
+        sim_overwrite_ahrs();
+    }
+#endif
     booz_ins_propagate();
   }
 }
diff --git a/sw/simulator/nps/nps_autopilot_booz.c b/sw/simulator/nps/nps_autopilot_booz.c
index 0968111e86..6d4cf76243 100644
--- a/sw/simulator/nps/nps_autopilot_booz.c
+++ b/sw/simulator/nps/nps_autopilot_booz.c
@@ -13,8 +13,6 @@
 struct NpsAutopilot autopilot;
 bool_t nps_bypass_ahrs;
 
-static void sim_overwrite_ahrs(void);
-
 
 void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) {
 
@@ -99,7 +97,7 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
 #include "nps_fdm.h"
 #include "math/pprz_algebra_int.h"
 #include "booz_ahrs.h"
-static void sim_overwrite_ahrs(void) {
+void sim_overwrite_ahrs(void) {
 
   //  printf("%f\n", fdm.ltpprz_to_body_eulers.phi);
 
diff --git a/sw/simulator/nps/nps_autopilot_booz.h b/sw/simulator/nps/nps_autopilot_booz.h
index c412efa324..6ffa5465b8 100644
--- a/sw/simulator/nps/nps_autopilot_booz.h
+++ b/sw/simulator/nps/nps_autopilot_booz.h
@@ -5,5 +5,6 @@
 
 
 extern bool_t nps_bypass_ahrs;
+extern void sim_overwrite_ahrs(void);
 
 #endif /* NPS_AUTOPILOT_BOOZ_H */