sim overwrite ahrs before vertical filter

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();
   }
 }

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);
 

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 */