diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
index 3764888244..1cd8ef6246 100644
--- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
@@ -67,6 +67,8 @@ sim.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
 
 sim.srcs   += $(SRC_BOOZ_SIM)/booz2_unsimulated_peripherals.c
 sim.srcs   += firmwares/rotorcraft/main.c
+sim.srcs   += mcu.c
+sim.srcs   += $(SRC_ARCH)/mcu_arch.c
 
 sim.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
 # -DTIME_LED=1
diff --git a/conf/flight_plans/basic_booz.xml b/conf/flight_plans/basic_booz.xml
index 434d3c9a5a..7719308239 100644
--- a/conf/flight_plans/basic_booz.xml
+++ b/conf/flight_plans/basic_booz.xml
@@ -1,6 +1,9 @@
 <!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
 
 <flight_plan alt="152" ground_alt="147" lat0="43 33 50.83" lon0="1 28 52.61" max_dist_from_home="400" name="Booz Test Enac" security_height="2">
+  <header>
+#include "autopilot.h"
+  </header>
   <waypoints>
     <waypoint name="HOME" x="0.0" y="0.0"/>
     <waypoint name="CLIMB" x="0.0" y="5.0"/>
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index 9c5b87f70c..01a7f58b02 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -42,6 +42,7 @@
 
 #include "booz/booz2_analog.h"
 #include "subsystems/sensors/baro.h"
+#include "baro_board.h"
 
 #include "firmwares/rotorcraft/battery.h"
 
diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c
index e990dc896d..126a35c476 100644
--- a/sw/airborne/subsystems/ins/hf_float.c
+++ b/sw/airborne/subsystems/ins/hf_float.c
@@ -259,7 +259,7 @@ void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) {
   printf("GPS_LAG_N: %d\n", GPS_LAG_N);
   printf("GPS_DT_N: %d\n", GPS_DT_N);
   printf("DT_HFILTER: %f\n", DT_HFILTER);
-  printf("GPS_LAG_TOL_N: %f\n", GPS_LAG_TOL_N);
+  printf("GPS_LAG_TOL_N: %i\n", GPS_LAG_TOL_N);
 #endif
 #else
   b2_hff_rb_last = &b2_hff_state;
diff --git a/sw/simulator/nps/nps_autopilot_booz.c b/sw/simulator/nps/nps_autopilot_booz.c
index 81c892fa77..80f3f0bbfd 100644
--- a/sw/simulator/nps/nps_autopilot_booz.c
+++ b/sw/simulator/nps/nps_autopilot_booz.c
@@ -5,7 +5,8 @@
 #include "nps_radio_control.h"
 #include "subsystems/radio_control.h"
 #include "subsystems/imu.h"
-#include "firmwares/rotorcraft/baro.h"
+#include "subsystems/sensors/baro.h"
+#include "baro_board.h"
 #include "firmwares/rotorcraft/battery.h"
 
 #include "actuators/supervision.h"
diff --git a/sw/simulator/nps/nps_fdm_jsbsim.c b/sw/simulator/nps/nps_fdm_jsbsim.c
index 5d5a726c9a..9c2e6f36f6 100644
--- a/sw/simulator/nps/nps_fdm_jsbsim.c
+++ b/sw/simulator/nps/nps_fdm_jsbsim.c
@@ -4,7 +4,6 @@
 #include <models/FGGroundReactions.h>
 #include <stdlib.h>
 #include "nps_fdm.h"
-#include "6dof.h"
 #include "generated/airframe.h"
 #include "math/pprz_geodetic.h"
 #include "math/pprz_geodetic_double.h"
diff --git a/sw/simulator/nps/nps_sensors_utils.c b/sw/simulator/nps/nps_sensors_utils.c
index 7252ee61ec..0911fed2eb 100644
--- a/sw/simulator/nps/nps_sensors_utils.c
+++ b/sw/simulator/nps/nps_sensors_utils.c
@@ -1,7 +1,6 @@
 #include "nps_sensors_utils.h"
 
 //#include <string.h>
-#include "6dof.h"
 #include "math/pprz_algebra.h"
 
 void UpdateSensorLatency(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading) {