diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
index b6963dac02..6cc818a313 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -230,6 +230,15 @@ static inline int ahrs_is_aligned(void) {
 }
 #endif
 
+/** Set motors ON or OFF and change the status of the check_motors state machine
+ */
+void autopilot_set_motors_on(bool_t motors_on) {
+  autopilot_motors_on = motors_on;
+  kill_throttle = ! autopilot_motors_on;
+  if (autopilot_motors_on) autopilot_check_motor_status = STATUS_MOTORS_ON;
+  else autopilot_check_motor_status = STATUS_MOTORS_OFF;
+}
+
 /**
  * State machine to check if motors should be turned ON or OFF
  * The motors start/stop when pushing the yaw stick without throttle during a given time
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h
index 9c611a7316..b2eda139d3 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.h
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.h
@@ -60,6 +60,7 @@ extern void autopilot_init(void);
 extern void autopilot_periodic(void);
 extern void autopilot_on_rc_frame(void);
 extern void autopilot_set_mode(uint8_t new_autopilot_mode);
+extern void autopilot_set_motors_on(bool_t motors_on);
 
 extern bool_t autopilot_detect_ground;
 extern bool_t autopilot_detect_ground_once;
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h
index 742f1b9c10..5ce687f4cf 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.h
+++ b/sw/airborne/firmwares/rotorcraft/navigation.h
@@ -76,8 +76,8 @@ bool_t nav_detect_ground(void);
 
 void nav_home(void);
 
-#define NavKillThrottle() ({ if (autopilot_mode == AP_MODE_NAV) { kill_throttle = 1; autopilot_motors_on = 0; } FALSE; })
-#define NavResurrect() ({ if (autopilot_mode == AP_MODE_NAV) { kill_throttle = 0; autopilot_motors_on = 1; } FALSE; })
+#define NavKillThrottle() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); } FALSE; })
+#define NavResurrect() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); } FALSE; })
 
 
 #define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; })