[checks] Add preflight checks (#2951)

---------

Co-authored-by: Christophe De Wagter <dewagter@gmail.com>
Co-authored-by: Gautier Hattenberger <gautier.hattenberger@enac.fr>

conf/airframes/examples/cube_orange.xml

@@ -73,6 +73,7 @@
       <define name="RADIO_KILL_SWITCH" value="0"/>
     </target>
 
+    <module name="preflight_checks"/>
     <module name="telemetry"     type="transparent">
       <configure name="MODEM_BAUD" value="B115200"/>
       <configure name="MODEM_PORT" value="usb_serial"/>

conf/airframes/tudelft/bebop_indi.xml

@@ -41,6 +41,8 @@
     <module name="logger_file">
       <define name="LOGGER_FILE_PATH" value="/data/ftp/internal_000"/>
     </module>
+
+    <module name="preflight_checks"/>
   </firmware>
 
 

conf/airframes/tudelft/nederdrone4.xml

@@ -82,6 +82,7 @@
       <define name="INDI_SCHEDULING_TRIM_ELEVATOR" value="840"/>
       <define name="INDI_SCHEDULING_PREF_FLAPS_FACTOR" value="1.0"/>
     </module>
+    <module name="preflight_checks"/>
 
     <!--module name="follow_me">
       <define name="FOLLOW_ME_DISTANCE" value="60"/>

conf/airframes/tudelft/rot_wing_25kg.xml

@@ -121,7 +121,9 @@
         <module name="rot_wing_automation"/>
         <module name="ground_detect_sensor"/>
         <module name="rotwing_state"/>
-        <module name="preflight_checks"/>
+        <module name="preflight_checks">
+            <define name="SDLOG_PREFLIGHT_ERROR" value="TRUE"/>
+        </module>
         <module name="agl_dist"/>
         <module name="approach_moving_target"/>
     </firmware>

conf/airframes/tudelft/rot_wing_v3b.xml

@@ -122,6 +122,7 @@
         <module name="rot_wing_automation"/>
         <module name="ground_detect_sensor"/>
         <module name="rotwing_state"/>
+        <module name="preflight_checks"/>
         <module name="agl_dist"/>
         <module name="approach_moving_target"/>
 

conf/modules/electrical.xml

@@ -24,7 +24,9 @@
   <periodic fun="electrical_periodic()" freq="10"/>
   <makefile target="fbw|sim|nps" firmware="fixedwing">
     <file name="electrical.c"/>
-    <test firmware="fixedwing"/>
+    <test firmware="fixedwing">
+      <define name="ELECTRICAL_PERIODIC_FREQ" value="10"/>
+    </test>
   </makefile>
   <makefile target="ap" firmware="fixedwing" cond="ifeq (,$(findstring $(SEPARATE_FBW),0 FALSE))">
     <file name="electrical.c"/>

conf/modules/logger_sd_chibios.xml

@@ -13,6 +13,7 @@
     <define name="SDLOG_START_DELAY" value="30" unit="s" description="Set the delay in seconds before starting the logger. This delay can be used to get plug USB cable and get data without starting a new log. Default: 30s"/>
     <define name="SDLOG_AUTO_FLUSH_PERIOD" value="10" unit="s" description="Data flush period. Shorter period may decrease performances. Default: 10s"/>
     <define name="SDLOG_CONTIGUOUS_STORAGE_MEM" value="50" unit="Mo" description="Try to reserve a given contiguous mass storage memory. Default: 50Mo"/>
+    <define name="SDLOG_PREFLIGHT_ERROR" value="FALSE" description="If set to TRUE, the autopilot will not arm if the SDLogger is not running. Default: FALSE"/>
   </doc>
    <settings>
     <dl_settings>

conf/modules/preflight_checks.xml

@@ -0,0 +1,33 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="preflight_checks" dir="checks" task="core">
+  <doc>
+    <description>
+      Preform preflight checks before arming the motors and periodically while not armed for status information.
+    </description>
+    <section name="PREFLIGHT_CHECK" prefix="PREFLIGHT_CHECK_">
+      <define name="MAX_MSGBUF" value="512" description="Maximum combined message size for storing the errors"/>
+      <define name="SEPERATOR" value=";" description="Seperating character for storing the errors"/>
+      <define name="INFO_TIMEOUT" value="5" description="Only send messages down every xx amount of seconds"/>
+    </section>
+  </doc>
+  <settings>
+    <dl_settings>
+      <dl_settings name="Checks">
+        <dl_setting var="preflight_bypass" min="0" max="1" step="1" values="FALSE|TRUE"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+  <header>
+    <file name="preflight_checks.h"/>
+  </header>
+  <makefile>
+    <define name="PREFLIGHT_CHECKS" value="true"/>
+    <file name="preflight_checks.c"/>
+    <test>
+      <define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
+      <define name="DOWNLINK_DEVICE" value="uart0"/>
+      <define name="USE_UART0"/>
+    </test>
+  </makefile>
+</module>

conf/settings/fixedwing_basic.xml

@@ -20,7 +20,7 @@
     <dl_settings NAME="mode">
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="autopilot.mode" module="autopilot" values="MANUAL|AUTO1|AUTO2|HOME|NOGPS|FAILSAFE"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="autopilot.launch"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="autopilot.kill_throttle"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="autopilot.kill_throttle"  module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
     </dl_settings>
 
     <dl_settings NAME="mcu">

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,52 @@ 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 ?
+ */
+bool autopilot_set_motors_on(bool motors_on)
+{
+  // Prevent unnessary preflight checks
+  if (autopilot.motors_on == motors_on) {
+    return true;
+  }
+
+#if PREFLIGHT_CHECKS
+  // When we fail the preflight checks abort
+  if (motors_on && !preflight_check()) {
+    // Bypass the preflight checks even if they fail but still preform them
+    if (!preflight_bypass) {
+      return false;
+    }
+  }
+#endif
+  autopilot_force_motors_on(motors_on);
+  return true;
+}
+
+/** turn motors on/off during arming, not done automatically
+ * prevents takeoff with preflight checks
+ */
+bool autopilot_arming_motors_on(bool motors_on)
+{
+  // Prevent unnessary preflight checks
+  if (autopilot.motors_on == motors_on) {
+    return true;
+  }
+
+#if PREFLIGHT_CHECKS
+  // When we fail the preflight checks abort
+  if (motors_on && !preflight_check()) {
+    // Bypass the preflight checks even if they fail but still preform them
+    if (!preflight_bypass) {
+      return false;
+    }
+  }
+#endif
+  autopilot.motors_on = motors_on;
+  return true;
+}
+
 /** get motors status
  */
 bool autopilot_get_motors_on(void)

sw/airborne/autopilot.h

@@ -122,12 +122,29 @@ 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
+ * @return true The preflight checks are successful and motors are started/stopped
+ * @return false The preflight checks are failed
  */
-extern void autopilot_set_motors_on(bool motors_on);
+extern bool autopilot_set_motors_on(bool motors_on);
+
+/**
+ * Start or stop the motors during arming
+ * May not happen when preflight checks are failing
+*/
+extern bool autopilot_arming_motors_on(bool motors_on);
 
 /** Get motor status
  *

sw/airborne/boards/ardrone/actuators.c

@@ -193,7 +193,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,34 +80,36 @@ static inline void autopilot_arming_check_motors_on(void)
 {
   switch (autopilot_arming_state) {
     case STATE_UNINIT:
-      autopilot.motors_on = false;
+      autopilot_arming_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_arming_motors_on(false);
       if (kill_switch_is_on()) {
         autopilot_arming_state = STATE_STARTABLE;
       }
       break;
     case STATE_STARTABLE:
-      autopilot.motors_on = false;
+      autopilot_arming_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;
       }
       else if (THROTTLE_STICK_DOWN() && rc_attitude_sticks_centered() &&
                (autopilot_get_mode() == MODE_MANUAL || autopilot_unarmed_in_auto)) {
-        autopilot_arming_state = STATE_MOTORS_ON;
+        /* start motors if preflight is successful */
+        if(autopilot_arming_motors_on(true))
+          autopilot_arming_state = STATE_MOTORS_ON;
       }
       break;
     case STATE_MOTORS_ON:
       if (kill_switch_is_on()) {
         /* kill motors, go to startable state */
-        autopilot.motors_on = false;
+        autopilot_arming_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 +119,7 @@ static inline void autopilot_arming_check_motors_on(void)
           autopilot_unarmed_in_auto = false;
         }
       } else {
-        autopilot.motors_on = true;
+        autopilot_arming_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_arming_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_arming_motors_on(false);
         autopilot_arming_delay_counter = 0;
         if (THROTTLE_STICK_DOWN()) {
           autopilot_arming_state = STATE_MOTORS_OFF_READY;
@@ -117,33 +117,37 @@ static inline void autopilot_arming_check_motors_on(void)
         }
         break;
       case STATE_MOTORS_OFF_READY:
-        autopilot.motors_on = false;
+        autopilot_arming_motors_on(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_arming_motors_on(false);
         autopilot_arming_delay_counter++;
         if (!autopilot_arming_check_valid()) {
           autopilot_arming_state = STATE_MOTORS_OFF_READY;
         } else if (autopilot_arming_delay_counter >= AUTOPILOT_ARMING_DELAY) {
-          autopilot_arming_state = STATE_MOTORS_ON;
+          if(autopilot_arming_motors_on(true)) {
+            autopilot_arming_state = STATE_MOTORS_ON;
+          } else {
+            autopilot_arming_state = AP_ARMING_STATUS_ARMING;
+          }
         } else {
           autopilot.arming_status = AP_ARMING_STATUS_ARMING;
         }
         break;
       case STATE_MOTORS_ON:
-        autopilot.motors_on = true;
+        autopilot_arming_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_arming_motors_on(true);
         autopilot.arming_status = AP_ARMING_STATUS_DISARMING;
         autopilot_arming_delay_counter--;
         if (!THROTTLE_STICK_DOWN()) {