diff --git a/sw/airborne/booz/booz2_autopilot.c b/sw/airborne/booz/booz2_autopilot.c
index b119a766c6..0d14f7b6eb 100644
--- a/sw/airborne/booz/booz2_autopilot.c
+++ b/sw/airborne/booz/booz2_autopilot.c
@@ -39,6 +39,7 @@ uint32_t booz2_autopilot_in_flight_counter;
 bool_t kill_throttle;
 
 bool_t booz2_autopilot_detect_ground;
+bool_t booz2_autopilot_detect_ground_once;
 
 #define BOOZ2_AUTOPILOT_MOTOR_ON_TIME     40
 #define BOOZ2_AUTOPILOT_IN_FLIGHT_TIME    40
@@ -54,6 +55,7 @@ void booz2_autopilot_init(void) {
   booz2_autopilot_in_flight_counter = 0;
   booz2_autopilot_mode_auto2 = BOOZ2_MODE_AUTO2;
   booz2_autopilot_detect_ground = FALSE;
+  booz2_autopilot_detect_ground_once = FALSE;
 }
 
 
diff --git a/sw/airborne/booz/booz2_autopilot.h b/sw/airborne/booz/booz2_autopilot.h
index 589b83cfc6..20e657b08e 100644
--- a/sw/airborne/booz/booz2_autopilot.h
+++ b/sw/airborne/booz/booz2_autopilot.h
@@ -56,6 +56,7 @@ extern void booz2_autopilot_on_rc_frame(void);
 extern void booz2_autopilot_set_mode(uint8_t new_autopilot_mode);
 
 extern bool_t booz2_autopilot_detect_ground;
+extern bool_t booz2_autopilot_detect_ground_once;
 
 #ifndef BOOZ2_MODE_MANUAL
 #define BOOZ2_MODE_MANUAL BOOZ2_AP_MODE_RATE_DIRECT
@@ -88,11 +89,13 @@ extern bool_t booz2_autopilot_detect_ground;
 
 #define TRESHOLD_GROUND_DETECT ACCEL_BFP_OF_REAL(15.)
 #define BoozDetectGroundEvent() {					\
-    if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE) {		\
-      if (booz_ins_ltp_accel.z < -TRESHOLD_GROUND_DETECT ||		\
-	  booz_ins_ltp_accel.z > TRESHOLD_GROUND_DETECT)		\
-	booz2_autopilot_detect_ground = TRUE;				\
-    }									\
+    if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE || booz2_autopilot_detect_ground_once) { \
+      if (booz_ins_ltp_accel.z < -TRESHOLD_GROUND_DETECT || \
+	        booz_ins_ltp_accel.z > TRESHOLD_GROUND_DETECT) { \
+        booz2_autopilot_detect_ground = TRUE; \
+        booz2_autopilot_detect_ground_once = FALSE; \
+      } \
+    } \
   }
 
 #endif /* BOOZ2_AUTOPILOT_H */
diff --git a/sw/airborne/booz/booz2_navigation.c b/sw/airborne/booz/booz2_navigation.c
index 54b4228599..d062f35218 100644
--- a/sw/airborne/booz/booz2_navigation.c
+++ b/sw/airborne/booz/booz2_navigation.c
@@ -137,7 +137,7 @@ void nav_circle(uint8_t wp_center, int32_t radius) {
     struct Int32Vect2 pos_diff;
     VECT2_DIFF(pos_diff, booz_ins_enu_pos,waypoints[wp_center]);
     // go back to half metric precision or values are too large
-    INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC-1);
+    //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
     // store last qdr
     int32_t last_qdr = nav_circle_qdr;
     // compute qdr
@@ -153,7 +153,7 @@ void nav_circle(uint8_t wp_center, int32_t radius) {
     int32_t abs_radius = abs(radius);
     // carrot_angle
     int32_t carrot_angle = (CARROT_DIST / abs_radius) << (INT32_TRIG_FRAC - INT32_POS_FRAC);
-    Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
+    Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_2);
     carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
     int32_t s_carrot, c_carrot;
     PPRZ_ITRIG_SIN(s_carrot, carrot_angle);	
@@ -182,6 +182,7 @@ void nav_route(uint8_t wp_start, uint8_t wp_end) {
   INT32_SQRT(leg_length,leg_length2);
   int32_t nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / leg_length;
   nav_leg_progress += Max((CARROT_DIST >> INT32_POS_FRAC), 0);
+  Bound(nav_leg_progress,0,leg_length);
   struct Int32Vect2 progress_pos;
   VECT2_SMUL(progress_pos, wp_diff, nav_leg_progress);
   VECT2_SDIV(progress_pos, progress_pos, leg_length);
@@ -287,5 +288,11 @@ void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) {
   }
 }
 
+bool_t nav_detect_ground(void) {
+  if (!booz2_autopilot_detect_ground) return FALSE;
+  booz2_autopilot_detect_ground = FALSE;
+  return TRUE;
+}
+
 void nav_home(void) {}
 
diff --git a/sw/airborne/booz/booz2_navigation.h b/sw/airborne/booz/booz2_navigation.h
index dcc863ec3b..53d1df6fcf 100644
--- a/sw/airborne/booz/booz2_navigation.h
+++ b/sw/airborne/booz/booz2_navigation.h
@@ -74,6 +74,7 @@ unit_t nav_reset_reference( void ) __attribute__ ((unused));
 unit_t nav_reset_alt( void ) __attribute__ ((unused));
 void nav_periodic_task(void);
 void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos);
+bool_t nav_detect_ground(void);
 
 void nav_home(void);
 
@@ -107,7 +108,7 @@ void nav_home(void);
 #define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; })
 #define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; })
 
-#define NavSetWaypointHere(_wp) { FALSE; }
+#define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], booz_ins_enu_pos); FALSE; })
 
 #define WaypointX(_wp)    POS_FLOAT_OF_BFP(waypoints[_wp].x)
 #define WaypointY(_wp)    POS_FLOAT_OF_BFP(waypoints[_wp].y)
@@ -189,8 +190,8 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
   nav_roll = ANGLE_BFP_OF_REAL(_roll); \
 }
 
-#define NAV_GROUND_DETECT ACCEL_BFP_OF_REAL(15.)
-#define NavDetectGround() ( booz_ins_enu_accel.z < -NAV_GROUND_DETECT || booz_ins_enu_accel.z > NAV_GROUND_DETECT )
+#define NavStartDetectGround() ({ booz2_autopilot_detect_ground_once = TRUE; FALSE; })
+#define NavDetectGround() nav_detect_ground()
 
 #define nav_IncreaseShift(x) {}
 
diff --git a/sw/airborne/booz/booz_radio_control.c b/sw/airborne/booz/booz_radio_control.c
index 29c90c0337..7a15b8215a 100644
--- a/sw/airborne/booz/booz_radio_control.c
+++ b/sw/airborne/booz/booz_radio_control.c
@@ -33,6 +33,7 @@ void radio_control_init(void) {
     radio_control.values[i] = 0;
   radio_control.status = RADIO_CONTROL_REALLY_LOST;
   radio_control.time_since_last_frame = RADIO_CONTROL_REALLY_LOST_TIME;
+  radio_control.radio_ok_cpt = 0;
   radio_control.frame_rate = 0;
   radio_control.frame_cpt = 0;
   radio_control_impl_init();
@@ -52,8 +53,10 @@ void radio_control_periodic(void) {
     radio_control.status = RADIO_CONTROL_REALLY_LOST;
   } 
   else {
-    if (radio_control.time_since_last_frame >= RADIO_CONTROL_LOST_TIME)
+    if (radio_control.time_since_last_frame >= RADIO_CONTROL_LOST_TIME) {
       radio_control.status = RADIO_CONTROL_LOST;
+      radio_control.radio_ok_cpt = RADIO_CONTROL_OK_CPT;
+    }
     radio_control.time_since_last_frame++;
   }
 
diff --git a/sw/airborne/booz/booz_radio_control.h b/sw/airborne/booz/booz_radio_control.h
index 2a82dd07b3..80ffb2d746 100644
--- a/sw/airborne/booz/booz_radio_control.h
+++ b/sw/airborne/booz/booz_radio_control.h
@@ -40,10 +40,13 @@ extern void radio_control_impl_init(void);
 /* timeouts - for now assumes 60Hz periodic */
 #define RADIO_CONTROL_LOST_TIME        30
 #define RADIO_CONTROL_REALLY_LOST_TIME 60
+/* number of valid frames before going back to OK */
+#define RADIO_CONTROL_OK_CPT      15
 
 struct RadioControl {
   uint8_t status;
   uint8_t time_since_last_frame;
+  uint8_t radio_ok_cpt;
   uint8_t frame_rate;
   uint8_t frame_cpt;
   pprz_t  values[RADIO_CONTROL_NB_CHANNEL];
diff --git a/sw/airborne/booz/guidance/booz2_guidance_h.c b/sw/airborne/booz/guidance/booz2_guidance_h.c
index 50bca29011..507ec9082f 100644
--- a/sw/airborne/booz/guidance/booz2_guidance_h.c
+++ b/sw/airborne/booz/guidance/booz2_guidance_h.c
@@ -138,8 +138,13 @@ void booz2_guidance_h_read_rc(bool_t  in_flight) {
     break;
   
   case BOOZ2_GUIDANCE_H_MODE_NAV:
-    BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight);
-    booz2_guidance_h_rc_sp.psi = 0;
+    if (radio_control.status == RADIO_CONTROL_OK) {
+      BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight);
+      booz2_guidance_h_rc_sp.psi = 0;
+    }
+    else {
+      INT_EULERS_ZERO(booz2_guidance_h_rc_sp);
+    }
     break;
   }
 
@@ -164,6 +169,8 @@ void booz2_guidance_h_run(bool_t  in_flight) {
     
   case BOOZ2_GUIDANCE_H_MODE_NAV:
     {
+      if (!in_flight) booz2_guidance_h_nav_enter();
+
       if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
 #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
         booz_stab_att_sp_euler.phi = nav_roll << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
diff --git a/sw/airborne/booz/guidance/booz2_guidance_v.c b/sw/airborne/booz/guidance/booz2_guidance_v.c
index 63b601d5f9..ae3f2b9513 100644
--- a/sw/airborne/booz/guidance/booz2_guidance_v.c
+++ b/sw/airborne/booz/guidance/booz2_guidance_v.c
@@ -209,8 +209,11 @@ void booz2_guidance_v_run(bool_t in_flight) {
         booz2_guidance_v_z_sp = -nav_flight_altitude; // For display only
         booz2_guidance_v_delta_t = nav_throttle;
       }
-      //booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t;
-    booz_stabilization_cmd[COMMAND_THRUST] = Min( booz2_guidance_v_rc_delta_t, booz2_guidance_v_delta_t);
+      /* use rc limitation if available */
+      if (radio_control.status == RADIO_CONTROL_OK)
+        booz_stabilization_cmd[COMMAND_THRUST] = Min( booz2_guidance_v_rc_delta_t, booz2_guidance_v_delta_t);
+      else
+        booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t;
       break;
     }
   }
diff --git a/sw/airborne/booz/radio_control/booz_radio_control_ppm.h b/sw/airborne/booz/radio_control/booz_radio_control_ppm.h
index 5cb3efe516..eee351553d 100644
--- a/sw/airborne/booz/radio_control/booz_radio_control_ppm.h
+++ b/sw/airborne/booz/radio_control/booz_radio_control_ppm.h
@@ -51,9 +51,12 @@ extern volatile bool_t booz_radio_control_ppm_frame_available;
     if (booz_radio_control_ppm_frame_available) {			\
       radio_control.frame_cpt++;					\
       radio_control.time_since_last_frame = 0;				\
-      radio_control.status = RADIO_CONTROL_OK;				\
-      NormalizePpm();							\
-      _received_frame_handler();					\
+      if (radio_control.radio_ok_cpt > 0) radio_control.radio_ok_cpt--; \
+      else { \
+        radio_control.status = RADIO_CONTROL_OK;				\
+        NormalizePpm();							\
+        _received_frame_handler();					\
+      } \
       booz_radio_control_ppm_frame_available = FALSE;			\
     }									\
   }