[checks] Add preflight checks

conf/airframes/tudelft/nederdrone4.xml

@@ -83,6 +83,8 @@
       <define name="RADIO_KILL_SWITCH" value="0"/>
     </target>
 
+    <module name="preflight_checks"/>
+
     <!--module name="follow_me">
       <define name="FOLLOW_ME_DISTANCE" value="60"/>
       <define name="FOLLOW_ME_HEIGHT" value="40"/>
@@ -432,4 +434,4 @@
     <define name="BAT_NB_CELLS" value="6"/>
   </section>
 
-</airframe>
+</airframe>

conf/modules/preflight_checks.xml

@@ -0,0 +1,19 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="preflight_checks" dir="checks" task="core">
+  <doc>
+    <description>
+      Preform preflight checks before arming the motors.
+    </description>
+  </doc>
+  <header>
+    <file name="preflight_checks.h"/>
+  </header>
+  <!--init fun="electrical_init()"/-->
+  <!--periodic fun="electrical_periodic()" freq="10"/-->
+  <makefile>
+    <define name="PREFLIGHT_CHECKS" value="true"/>
+    <file name="preflight_checks.c"/>
+  </makefile>
+</module>
+

sw/airborne/autopilot.c

@@ -39,6 +39,7 @@
 #include "modules/core/commands.h"
 #include "modules/datalink/telemetry.h"
 #include "modules/core/abi.h"
+#include "modules/checks/preflight_checks.h"
 
 #include "modules/core/settings.h"
 #include "generated/settings.h"
@@ -226,10 +227,9 @@ void autopilot_reset_flight_time(void)
   autopilot.launch = false;
 }
 
-/** turn motors on/off, eventually depending of the current mode
- *  set kill_throttle accordingly FIXME is it true for FW firmware ?
+/** Force the motors on/off skipping preflight checks
  */
-void autopilot_set_motors_on(bool motors_on)
+void autopilot_force_motors_on(bool motors_on)
 {
 #if USE_GENERATED_AUTOPILOT
   autopilot_generated_set_motors_on(motors_on);
@@ -239,6 +239,20 @@ void autopilot_set_motors_on(bool motors_on)
   autopilot.kill_throttle = ! autopilot.motors_on;
 }
 
+/** turn motors on/off, eventually depending of the current mode
+ *  set kill_throttle accordingly FIXME is it true for FW firmware ?
+ */
+void autopilot_set_motors_on(bool motors_on)
+{
+#if PREFLIGHT_CHECKS
+  // When we fail the preflight checks abort
+  if(motors_on && !preflight_check()) {
+    return;
+  }
+#endif
+  autopilot_force_motors_on(motors_on);
+}
+
 /** get motors status
  */
 bool autopilot_get_motors_on(void)

sw/airborne/autopilot.h

@@ -122,8 +122,17 @@ extern uint8_t autopilot_get_mode(void);
 extern void autopilot_reset_flight_time(void);
 #define autopilot_ResetFlightTimeAndLaunch(_) autopilot_reset_flight_time()
 
+/**
+ * @brief Force start/stop the motors
+ * WARNING This will skip he preflight checks
+ * 
+ * @param motors_on Wheter the motors should be forced on/off
+ */
+extern void autopilot_force_motors_on(bool motors_on);
+
 /** Start or stop motors
  *  May have no effect if motors has auto-start based on throttle setpoint
+ *  or when the preflight checks are failing.
  *
  * @param[in] motors_on true to start motors, false to stop
  */

sw/airborne/boards/ardrone/actuators.c

@@ -192,7 +192,7 @@ void actuators_ardrone_motor_status(void)
     if (autopilot_get_motors_on()) {
       if (last_motor_on) {
         // Tell paparazzi that one motor has stalled
-        autopilot_set_motors_on(FALSE);
+        autopilot_set_motors_on(false);
       } else {
         // Toggle Flipflop reset so motors can be re-enabled
         reset_flipflop_counter = 20;

sw/airborne/boards/bebop/actuators.c

@@ -87,7 +87,7 @@ void actuators_bebop_commit(void)
   // When detected a suicide
   actuators_bebop.i2c_trans.buf[10] = actuators_bebop.i2c_trans.buf[10] & 0x7;
   if (actuators_bebop.i2c_trans.buf[11] == 2 && actuators_bebop.i2c_trans.buf[10] != 1) {
-    autopilot_set_motors_on(FALSE);
+    autopilot_set_motors_on(false);
   }
 
   // Start the motors

sw/airborne/firmwares/rotorcraft/autopilot_arming_common.h

@@ -45,8 +45,5 @@
 #define AP_ARMING_STATUS_PITCH_NOT_CENTERED   12
 #define AP_ARMING_STATUS_ROLL_NOT_CENTERED    13
 #define AP_ARMING_STATUS_YAW_NOT_CENTERED     14
-#define AP_ARMING_STATUS_AHRS_NOT_ALLIGNED    15
-#define AP_ARMING_STATUS_OUT_OF_GEOFENCE      16
-#define AP_ARMING_STATUS_LOW_BATTERY          17
 
 #endif /* AUTOPILOT_ARMING_COMMON_H */

sw/airborne/firmwares/rotorcraft/autopilot_arming_switch.h

@@ -80,21 +80,21 @@ static inline void autopilot_arming_check_motors_on(void)
 {
   switch (autopilot_arming_state) {
     case STATE_UNINIT:
-      autopilot.motors_on = false;
+      autopilot_set_motors_on(false);
       if (kill_switch_is_on()) {
         autopilot_arming_state = STATE_STARTABLE;
       } else {
         autopilot_arming_state = STATE_WAITING;
       }
       break;
     case STATE_WAITING:
-      autopilot.motors_on = false;
+      autopilot_set_motors_on(false);
       if (kill_switch_is_on()) {
         autopilot_arming_state = STATE_STARTABLE;
       }
       break;
     case STATE_STARTABLE:
-      autopilot.motors_on = false;
+      autopilot_set_motors_on(false);
       /* don't allow to start if in KILL mode or kill switch is on */
       if (autopilot_get_mode() == AP_MODE_KILL || kill_switch_is_on()) {
         break;
@@ -107,7 +107,7 @@ static inline void autopilot_arming_check_motors_on(void)
     case STATE_MOTORS_ON:
       if (kill_switch_is_on()) {
         /* kill motors, go to startable state */
-        autopilot.motors_on = false;
+        autopilot_set_motors_on(false);
         autopilot_arming_state = STATE_STARTABLE;
         /* if turned off in an AUTO mode, remember it so it can be turned on again in AUTO */
         if (autopilot_get_mode() != MODE_MANUAL && autopilot_get_mode() != AP_MODE_KILL &&
@@ -117,7 +117,7 @@ static inline void autopilot_arming_check_motors_on(void)
           autopilot_unarmed_in_auto = false;
         }
       } else {
-        autopilot.motors_on = true;
+        autopilot_set_motors_on(true);
       }
       break;
     default:

sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h

@@ -103,12 +103,12 @@ static inline void autopilot_arming_check_motors_on(void)
 
     switch (autopilot_arming_state) {
       case STATE_UNINIT:
-        autopilot.motors_on = false;
+        autopilot_set_motors_on(false);
         autopilot_arming_delay_counter = 0;
         autopilot_arming_state = STATE_WAITING;
         break;
       case STATE_WAITING: // after startup wait until throttle is down before attempting to arm
-        autopilot.motors_on = false;
+        autopilot_set_motors_on(false);
         autopilot_arming_delay_counter = 0;
         if (THROTTLE_STICK_DOWN()) {
           autopilot_arming_state = STATE_MOTORS_OFF_READY;
@@ -117,14 +117,14 @@ static inline void autopilot_arming_check_motors_on(void)
         }
         break;
       case STATE_MOTORS_OFF_READY:
-        autopilot.motors_on = false;
+        autopilot_set_motors_on(false, false);
         autopilot_arming_delay_counter = 0;
         if (autopilot_arming_check_valid()) {
           autopilot_arming_state = STATE_ARMING;
         }
         break;
       case STATE_ARMING:
-        autopilot.motors_on = false;
+        autopilot_set_motors_on(false);
         autopilot_arming_delay_counter++;
         if (!autopilot_arming_check_valid()) {
           autopilot_arming_state = STATE_MOTORS_OFF_READY;
@@ -135,15 +135,15 @@ static inline void autopilot_arming_check_motors_on(void)
         }
         break;
       case STATE_MOTORS_ON:
-        autopilot.motors_on = true;
+        autopilot_set_motors_on(true);
         autopilot.arming_status = AP_ARMING_STATUS_ARMED;
         autopilot_arming_delay_counter = AUTOPILOT_ARMING_DELAY;
         if (THROTTLE_STICK_DOWN()) {
           autopilot_arming_state = STATE_UNARMING;
         }
         break;
       case STATE_UNARMING:
-        autopilot.motors_on = true;
+        autopilot_set_motors_on(true);
         autopilot.arming_status = AP_ARMING_STATUS_DISARMING;
         autopilot_arming_delay_counter--;
         if (!THROTTLE_STICK_DOWN()) {

sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h

@@ -136,13 +136,13 @@ static inline void autopilot_arming_check_motors_on(void)
           autopilot_check_motor_status = STATUS_MOTORS_ON;
         } else {
           autopilot_check_motor_status = STATUS_MOTORS_AUTOMATICALLY_OFF;
-          autopilot.motors_on = false;
+          autopilot_set_motors_on(false);
         }
         break;
       case STATUS_MOTORS_AUTOMATICALLY_OFF: // Motors were disarmed externally
         //(possibly due to crash)
         //wait extra delay before enabling the normal arming state machine
-        autopilot.motors_on = false;
+        autopilot_set_motors_on(false);
         autopilot_motors_on_counter = 0;
         if (autopilot_arming_check_valid(YAW_MUST_BE_CENTERED)) {
           autopilot_check_motor_status = STATUS_MOTORS_AUTOMATICALLY_OFF_SAFETY_WAIT;
@@ -157,15 +157,15 @@ static inline void autopilot_arming_check_motors_on(void)
         }
         break;
       case STATUS_MOTORS_OFF:
-        autopilot.motors_on = false;
+        autopilot_set_motors_on(false);
         autopilot_motors_on_counter = 0;
         autopilot.arming_status = AP_ARMING_STATUS_WAITING;
         if (autopilot_arming_check_valid(YAW_MUST_BE_PUSHED)) { // stick pushed
           autopilot_check_motor_status = STATUS_M_OFF_STICK_PUSHED;
         }
         break;
       case STATUS_M_OFF_STICK_PUSHED:
-        autopilot.motors_on = false;
+        autopilot_set_motors_on(false);
         autopilot_motors_on_counter++;
         if (autopilot_motors_on_counter >= MOTOR_ARMING_DELAY) {
           autopilot_check_motor_status = STATUS_START_MOTORS;
@@ -176,7 +176,7 @@ static inline void autopilot_arming_check_motors_on(void)
         }
         break;
       case STATUS_START_MOTORS:
-        autopilot.motors_on = true;
+        autopilot_set_motors_on(true);
         autopilot_motors_on_counter = MOTOR_ARMING_DELAY;
         autopilot_set_in_flight(false);   // stop fc from starting control (integration and yaw) till arm process is complete
         if (YAW_STICK_CENTERED()) { // wait until stick released
@@ -185,14 +185,14 @@ static inline void autopilot_arming_check_motors_on(void)
         break;
       case STATUS_MOTORS_ON:
         autopilot.arming_status = AP_ARMING_STATUS_ARMED;
-        autopilot.motors_on = true;
+        autopilot_set_motors_on(true);
         autopilot_motors_on_counter = MOTOR_ARMING_DELAY;
         if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) { // stick pushed
           autopilot_check_motor_status = STATUS_M_ON_STICK_PUSHED;
         }
         break;
       case STATUS_M_ON_STICK_PUSHED:
-        autopilot.motors_on = true;
+        autopilot_set_motors_on(true);
         autopilot_motors_on_counter--;
         if (autopilot_motors_on_counter == 0) {
           autopilot_check_motor_status = STATUS_STOP_MOTORS;
@@ -203,7 +203,7 @@ static inline void autopilot_arming_check_motors_on(void)
         }
         break;
       case STATUS_STOP_MOTORS:
-        autopilot.motors_on = false;
+        autopilot_set_motors_on(false);
         autopilot_motors_on_counter = 0;
         if (autopilot_arming_check_valid(YAW_MUST_BE_CENTERED)) { // wait till release disarm stick before allowing to re-arm
           autopilot_check_motor_status = STATUS_MOTORS_OFF;
@@ -215,7 +215,7 @@ static inline void autopilot_arming_check_motors_on(void)
   } else {
     autopilot.arming_status = AP_ARMING_STATUS_KILLED;
     if (kill_switch_is_on()) {
-      autopilot.motors_on = false;
+      autopilot_set_motors_on(false);
     }
   }
 }

sw/airborne/firmwares/rotorcraft/autopilot_generated.c

@@ -88,7 +88,7 @@ void autopilot_generated_set_mode(uint8_t new_autopilot_mode)
 
 void autopilot_generated_set_motors_on(bool motors_on)
 {
-  if (ap_ahrs_is_aligned() && motors_on
+  if (motors_on
 #ifdef AP_MODE_KILL
       && autopilot.mode != AP_MODE_KILL
 #endif
@@ -107,14 +107,9 @@ void autopilot_generated_on_rc_frame(void)
   // FIXME what to do here ?
 
   /* an arming sequence is used to start/stop motors.
-   * only allow arming if ahrs is aligned
    */
-  if (ap_ahrs_is_aligned()) {
-    autopilot_arming_check_motors_on();
-    autopilot.kill_throttle = ! autopilot.motors_on;
-  } else {
-    autopilot.arming_status = AP_ARMING_STATUS_AHRS_NOT_ALLIGNED;
-  }
+  autopilot_arming_check_motors_on();
+  autopilot.kill_throttle = ! autopilot.motors_on;
 
   /* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */
 //  if (autopilot.mode != AP_MODE_FAILSAFE && autopilot.mode != AP_MODE_HOME) {

sw/airborne/firmwares/rotorcraft/autopilot_static.c

@@ -192,10 +192,10 @@ void autopilot_static_SetModeHandler(float mode)
 void autopilot_static_set_mode(uint8_t new_autopilot_mode)
 {
 
-  /* force startup mode (default is kill) as long as AHRS is not aligned */
-  if (!ap_ahrs_is_aligned()) {
-    new_autopilot_mode = MODE_STARTUP;
-  }
+  // /* force startup mode (default is kill) as long as preflight fail */
+  // if (!peflight_check()) {
+  //   new_autopilot_mode = MODE_STARTUP;
+  // }
 
   if (new_autopilot_mode != autopilot.mode) {
     /* horizontal mode */
@@ -317,7 +317,7 @@ void autopilot_static_set_mode(uint8_t new_autopilot_mode)
 
 void autopilot_static_set_motors_on(bool motors_on)
 {
-  if (autopilot.mode != AP_MODE_KILL && ap_ahrs_is_aligned() && motors_on) {
+  if (autopilot.mode != AP_MODE_KILL && motors_on) {
     autopilot.motors_on = true;
   } else {
     autopilot.motors_on = false;
@@ -362,14 +362,9 @@ void autopilot_static_on_rc_frame(void)
   }
 
   /* an arming sequence is used to start/stop motors.
-   * only allow arming if ahrs is aligned
    */
-  if (ap_ahrs_is_aligned()) {
-    autopilot_arming_check_motors_on();
-    autopilot.kill_throttle = ! autopilot.motors_on;
-  } else {
-    autopilot.arming_status = AP_ARMING_STATUS_AHRS_NOT_ALLIGNED;
-  }
+  autopilot_arming_check_motors_on();
+  autopilot.kill_throttle = ! autopilot.motors_on;
 
   /* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */
   if (autopilot.mode != AP_MODE_FAILSAFE && autopilot.mode != AP_MODE_HOME) {

sw/airborne/firmwares/rotorcraft/autopilot_utils.c

@@ -36,20 +36,6 @@
 PRINT_CONFIG_VAR(FAILSAFE_DESCENT_SPEED)
 
 
-// Utility functions
-#ifndef AUTOPILOT_DISABLE_AHRS_KILL
-bool ap_ahrs_is_aligned(void)
-{
-  return stateIsAttitudeValid();
-}
-#else
-PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")
-bool ap_ahrs_is_aligned(void)
-{
-  return true;
-}
-#endif
-
 #if defined RADIO_MODE_2x3
 
 #define THRESHOLD_1d3_PPRZ (MAX_PPRZ / 3)

sw/airborne/firmwares/rotorcraft/autopilot_utils.h

@@ -37,7 +37,6 @@
 #define FAILSAFE_DESCENT_SPEED 1.5
 #endif
 
-extern bool ap_ahrs_is_aligned(void);
 #if defined RADIO_MODE_2x3
 extern uint8_t ap_mode_of_3x2way_switch(void);
 #else