diff --git a/sw/airborne/booz/booz2_navigation.c b/sw/airborne/booz/booz2_navigation.c
index d51cc418e8..b42fd22f8e 100644
--- a/sw/airborne/booz/booz2_navigation.c
+++ b/sw/airborne/booz/booz2_navigation.c
@@ -55,7 +55,10 @@ int32_t nav_heading, nav_course;
 
 uint8_t vertical_mode;
 uint32_t nav_throttle;
-int32_t nav_climb, nav_altitude;
+int32_t nav_climb, nav_altitude, nav_flight_altitude;
+float flight_altitude;
+
+static inline void nav_set_altitude( void );
 
 #define CLOSE_TO_WAYPOINT (15 << 8)
 #define ARRIVED_AT_WAYPOINT (10 << 8)
@@ -66,6 +69,8 @@ void booz2_nav_init(void) {
   nav_stage = 0;
   ground_alt = (int32_t)(GROUND_ALT * 100); // cm
   nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
+  nav_flight_altitude = nav_altitude;
+  flight_altitude = SECURITY_HEIGHT;
   INT32_VECT3_COPY( booz2_navigation_target, waypoints[WP_HOME]);
   INT32_VECT3_COPY( booz2_navigation_carrot, waypoints[WP_HOME]);
 
@@ -102,6 +107,8 @@ void booz2_nav_run(void) {
     VECT2_SDIV(path_to_carrot, dist_to_waypoint, path_to_carrot);
     VECT2_SUM(booz2_navigation_carrot, path_to_carrot, booz_ins_enu_pos);
   }
+
+  nav_set_altitude();
 }
 
 //#include "stdio.h"
@@ -157,6 +164,14 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx) {
 
 static int32_t previous_ground_alt;
 
+static inline void nav_set_altitude( void ) {
+  static int32_t last_nav_alt = 0;
+  if (nav_altitude != last_nav_alt) {
+    nav_flight_altitude = nav_altitude;
+    last_nav_alt = nav_altitude;
+  }
+}
+
 /** Reset the geographic reference to the current GPS fix */
 unit_t nav_reset_reference( void ) {
   booz_ins_ltp_initialised = FALSE;
diff --git a/sw/airborne/booz/booz2_navigation.h b/sw/airborne/booz/booz2_navigation.h
index 0e0f3d85d3..637ebb2550 100644
--- a/sw/airborne/booz/booz2_navigation.h
+++ b/sw/airborne/booz/booz2_navigation.h
@@ -55,7 +55,8 @@ extern int32_t nav_heading, nav_course;
 
 extern uint8_t vertical_mode;
 extern uint32_t nav_throttle;
-extern int32_t nav_climb, nav_altitude;
+extern int32_t nav_climb, nav_altitude, nav_flight_altitude;
+extern float flight_altitude;
 #define VERTICAL_MODE_MANUAL      0
 #define VERTICAL_MODE_CLIMB       1
 #define VERTICAL_MODE_ALT         2
@@ -143,7 +144,7 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
 /** Set the climb control to auto-throttle with the specified pitch
     pre-command */
 #define NavVerticalAutoThrottleMode(_pitch) { \
-  nav_pitch = _pitch; \
+  nav_pitch = ANGLE_BFP_OF_REAL(_pitch); \
 }
 
 /** Set the climb control to auto-pitch with the specified throttle
@@ -173,7 +174,7 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
 
 #define NavAttitude(_roll) { \
   horizontal_mode = HORIZONTAL_MODE_ATTITUDE; \
-  nav_roll = _roll; \
+  nav_roll = ANGLE_BFP_OF_REAL(_roll); \
 }
 
 #define nav_IncreaseShift(x) {}
@@ -183,4 +184,10 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
 #define booz2_navigation_SetNavHeading(x) { \
   nav_heading = ANGLE_BFP_OF_REAL(x); \
 }
+
+#define booz2_navigation_SetFlightAltitude(x) { \
+  flight_altitude = x; \
+  nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude); \
+}
+
 #endif /* BOOZ2_NAVIGATION_H */
diff --git a/sw/airborne/booz/guidance/booz2_guidance_v.c b/sw/airborne/booz/guidance/booz2_guidance_v.c
index 8bacb93462..1b5ca19cc6 100644
--- a/sw/airborne/booz/guidance/booz2_guidance_v.c
+++ b/sw/airborne/booz/guidance/booz2_guidance_v.c
@@ -193,22 +193,20 @@ void booz2_guidance_v_run(bool_t in_flight) {
   case BOOZ2_GUIDANCE_V_MODE_NAV:
     {
       if (vertical_mode == VERTICAL_MODE_ALT) {
-        booz2_guidance_v_z_sp = -nav_altitude;
+        booz2_guidance_v_z_sp = -nav_flight_altitude;
         b2_gv_update_ref_from_z_sp(booz2_guidance_v_z_sp);
         run_hover_loop(in_flight);
       }
       else if (vertical_mode == VERTICAL_MODE_CLIMB) {
         booz2_guidance_v_zd_sp = -nav_climb;
         b2_gv_update_ref_from_zd_sp(booz2_guidance_v_zd_sp);
-        nav_altitude = -booz2_guidance_v_z_sp;
+        nav_flight_altitude = -booz2_guidance_v_z_sp;
         run_hover_loop(in_flight);
       }
       else if (vertical_mode == VERTICAL_MODE_MANUAL) {
         booz2_guidance_v_delta_t = nav_throttle;
       }
-      // saturate max authority with RC stick
-      booz_stabilization_cmd[COMMAND_THRUST] = Min( booz2_guidance_v_rc_delta_t, booz2_guidance_v_delta_t);
-      //booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_rc_delta_t;
+      booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t;
       break;
     }
   }