diff --git a/conf/autopilot/booz2_autopilot.makefile b/conf/autopilot/booz2_autopilot.makefile
index 4b128e50ed..a53e44ff00 100644
--- a/conf/autopilot/booz2_autopilot.makefile
+++ b/conf/autopilot/booz2_autopilot.makefile
@@ -122,7 +122,7 @@ ap.CFLAGS += -DHS_YAW
 
 ap.srcs += $(SRC_BOOZ)/booz2_fms.c
 
-#ap.CFLAGS += -DBOOZ2_FMS_TYPE=BOOZ2_FMS_TYPE_DATALINK -DFMS_DATALINK_USE_COMMANDS_RAW
+#ap.CFLAGS += -DBOOZ2_FMS_TYPE=BOOZ2_FMS_TYPE_DATALINK
 #ap.srcs += $(SRC_BOOZ)/booz2_fms_datalink.c
 
 ap.CFLAGS += -DBOOZ2_FMS_TYPE=BOOZ2_FMS_TYPE_TEST_SIGNAL
diff --git a/conf/autopilot/booz2_test_progs.makefile b/conf/autopilot/booz2_test_progs.makefile
index fbdd16c3d7..b3e5724431 100644
--- a/conf/autopilot/booz2_test_progs.makefile
+++ b/conf/autopilot/booz2_test_progs.makefile
@@ -298,8 +298,8 @@ test_mc.ARCH = arm7tdmi
 test_mc.TARGET = test_mc
 test_mc.TARGETDIR = test_mc
 
-test_mc.CFLAGS += -DCONFIG=$(BOARD_CFG) -I$(BOOZ_PRIV) -I$(BOOZ_PRIV_ARCH) -I$(BOOZ_ARCH)
-test_mc.srcs += $(BOOZ_PRIV_TEST)/booz2_test_mc.c
+test_mc.CFLAGS += -DCONFIG=$(BOARD_CFG) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
+test_mc.srcs += $(SRC_BOOZ_TEST)/booz2_test_mc.c
 test_mc.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIME_LED=1
 test_mc.CFLAGS += -DLED
 test_mc.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
@@ -311,7 +311,7 @@ test_mc.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVIC
 test_mc.srcs += downlink.c pprz_transport.c
 
 test_mc.CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC
-test_mc.srcs += $(BOOZ_PRIV_ARCH)/actuators_buss_twi_blmc_hw.c actuators.c
+test_mc.srcs += $(SRC_BOOZ_ARCH)/actuators_buss_twi_blmc_hw.c actuators.c
 test_mc.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
 test_mc.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
 
diff --git a/sw/airborne/booz/booz2_fms_datalink.h b/sw/airborne/booz/booz2_fms_datalink.h
index 26e7bd4b20..4aec2a9088 100644
--- a/sw/airborne/booz/booz2_fms_datalink.h
+++ b/sw/airborne/booz/booz2_fms_datalink.h
@@ -24,18 +24,6 @@
 extern void booz_fms_impl_init(void);
 extern void booz_fms_impl_periodic(void);
 
-#if defined FMS_DATALINK_USE_COMMANDS_RAW
-#define BOOZ_FMS_PARSE_DATALINK(_l, _cmds) {				\
-    if (_l >= 2) {							\
-      booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE;		\
-      booz_fms_input.h_sp.attitude.phi   = BOOZ2_JOYSTICK_SP_PHI_COEF   * _cmds[0]; \
-      booz_fms_input.h_sp.attitude.theta = BOOZ2_JOYSTICK_SP_THETA_COEF * _cmds[1]; \
-      booz_fms_input.h_sp.attitude.psi   = 0;				\
-      booz_fms_last_msg = 0;						\
-      booz_fms_timeout = FALSE;						\
-    }									\
-  }
-#else
 
 #ifdef BOOZ_FMS_PHI_THETA_MAX
 #define BOOZ_FMS_LIMIT_ATTITUDE(_fms_att) { \
@@ -80,6 +68,5 @@ extern void booz_fms_impl_periodic(void);
       break;								\
     }									\
   }
-#endif /* FMS_USE_COMMANDS_RAW */
 
 #endif /* BOOZ2_FMS_DATALINK_H */
diff --git a/sw/airborne/booz/booz2_guidance_v.c b/sw/airborne/booz/booz2_guidance_v.c
index fe2d160573..8c508b2a39 100644
--- a/sw/airborne/booz/booz2_guidance_v.c
+++ b/sw/airborne/booz/booz2_guidance_v.c
@@ -1,7 +1,7 @@
 #include "booz2_guidance_v.h"
 
 #define B2_GUIDANCE_V_C
-#define B2_GUIDANCE_V_USE_REF
+//#define B2_GUIDANCE_V_USE_REF
 #include "booz2_guidance_v_ref.h"
 
 #include "radio_control.h"
@@ -105,8 +105,8 @@ static inline void run_hover_loop(bool_t in_flight) {
 #else
   b2_gv_set_ref(booz2_guidance_v_z_sp, 0, 0);
 #endif
-  int64_t ref_z = b2_gv_z_ref>>(B2_GV_Z_REF_FRAC - B2_GV_Z_SP_FRAC);
-  int32_t err_z =  booz_ins_position.z - (int32_t)ref_z;
+  int64_t ref_z  = b2_gv_z_ref>>(B2_GV_Z_REF_FRAC - B2_GV_Z_SP_FRAC);
+  int32_t err_z  =  booz_ins_position.z - (int32_t)ref_z;
   int32_t ref_zd =  b2_gv_zd_ref<<(ISPEED_RES - B2_GV_ZD_REF_FRAC);
   int32_t err_zd =  booz_ins_speed_earth.z - ref_zd;