[rotorcraft] arming: only check if motors can be armed if ahrs_is_ali…

…gned
paparazzi · Nov 24, 2014 · 78b52b6 · 78b52b6
1 parent 63ba5a1
commit 78b52b6
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 13 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -527,9 +527,14 @@ void autopilot_on_rc_frame(void) {
     }
 #endif
 
-    /* an arming sequence is used to start/stop motors */
-    autopilot_arming_check_motors_on();
-    kill_throttle = ! autopilot_motors_on;
+    /* an arming sequence is used to start/stop motors.
+     * only allow switching motor if not in FAILSAFE or KILL mode and ahrs is aligned
+     */
+    if (autopilot_mode != AP_MODE_KILL && autopilot_mode != AP_MODE_FAILSAFE &&
+        ahrs_is_aligned()) {
+      autopilot_arming_check_motors_on();
+      kill_throttle = ! autopilot_motors_on;
+    }
 
     guidance_v_read_rc();
     guidance_h_read_rc(autopilot_in_flight);

diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h b/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h
@@ -69,14 +69,10 @@ static inline void autopilot_arming_set(bool_t motors_on) {
  * - if throttle was not down at startup, you need to put throttle down again first
  * - other sticks need to be centered to start motors
  * - need to be in manual mode to start the motors
- * - AHRS needs to be aligned
  */
 static inline void autopilot_arming_check_motors_on( void ) {
 
-  /* only allow switching motor if not in FAILSAFE or KILL mode */
-  if (autopilot_mode != AP_MODE_KILL && autopilot_mode != AP_MODE_FAILSAFE) {
-
-    switch(autopilot_arming_state) {
+  switch(autopilot_arming_state) {
     case STATE_UNINIT:
       autopilot_motors_on = FALSE;
       autopilot_arming_delay_counter = 0;
@@ -140,7 +136,6 @@ static inline void autopilot_arming_check_motors_on( void ) {
       break;
     default:
       break;
-    }
   }
 
 }

diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h b/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h
@@ -75,10 +75,8 @@ static inline void autopilot_arming_set(bool_t motors_on) {
  * The stick must return to a neutral position before starting/stoping again.
  */
 static inline void autopilot_arming_check_motors_on( void ) {
-  /* only allow switching motor if not in FAILSAFE or KILL mode */
-  if (autopilot_mode != AP_MODE_KILL && autopilot_mode != AP_MODE_FAILSAFE) {
 
-    switch(autopilot_check_motor_status) {
+  switch(autopilot_check_motor_status) {
     case STATUS_MOTORS_OFF:
       autopilot_motors_on = FALSE;
       autopilot_motors_on_counter = 0;
@@ -121,7 +119,6 @@ static inline void autopilot_arming_check_motors_on( void ) {
       break;
     default:
       break;
-    };
   }
 }