[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
+/** Force the motors on/off skipping preflight checks
- *  set kill_throttle accordingly FIXME is it true for FW firmware ?
  */
-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,7 +80,7 @@ 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 {
@@ -88,26 +88,28 @@ static inline void autopilot_arming_check_motors_on(void)
       }
       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,25 +117,29 @@ 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()) {
@@ -143,7 +147,7 @@ static inline void autopilot_arming_check_motors_on(void)
         }
         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()) {

sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h

@@ -134,15 +134,16 @@ static inline void autopilot_arming_check_motors_on(void)
         // If the vehicle was killed accidentally, allow rapid re-arm
         if ( (get_sys_time_float() - motor_kill_time) < MOTOR_RE_ARM_TIME) {
           autopilot_check_motor_status = STATUS_MOTORS_ON;
+          autopilot.motors_on = true; // Bypass preflight-checks and force the motors on
         } else {
           autopilot_check_motor_status = STATUS_MOTORS_AUTOMATICALLY_OFF;
-          autopilot.motors_on = false;
+          autopilot_arming_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_arming_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,7 +158,7 @@ static inline void autopilot_arming_check_motors_on(void)
         }
         break;
       case STATUS_MOTORS_OFF:
-        autopilot.motors_on = false;
+        autopilot_arming_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
@@ -165,7 +166,7 @@ static inline void autopilot_arming_check_motors_on(void)
         }
         break;
       case STATUS_M_OFF_STICK_PUSHED:
-        autopilot.motors_on = false;
+        autopilot_arming_motors_on(false);
         autopilot_motors_on_counter++;
         if (autopilot_motors_on_counter >= MOTOR_ARMING_DELAY) {
           autopilot_check_motor_status = STATUS_START_MOTORS;
@@ -176,23 +177,27 @@ static inline void autopilot_arming_check_motors_on(void)
         }
         break;
       case STATUS_START_MOTORS:
-        autopilot.motors_on = true;
+        autopilot_arming_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
-          autopilot_check_motor_status = STATUS_MOTORS_ON;
+          // Verify if motors are really on else go back to STATUS_MOTORS_OFF
+          if(autopilot_get_motors_on())
+            autopilot_check_motor_status = STATUS_MOTORS_ON;
+          else
+             autopilot_check_motor_status = STATUS_MOTORS_OFF;
         }
         break;
       case STATUS_MOTORS_ON:
         autopilot.arming_status = AP_ARMING_STATUS_ARMED;
-        autopilot.motors_on = true;
+        autopilot_arming_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_arming_motors_on(true);
         autopilot_motors_on_counter--;
         if (autopilot_motors_on_counter == 0) {
           autopilot_check_motor_status = STATUS_STOP_MOTORS;
@@ -203,7 +208,7 @@ static inline void autopilot_arming_check_motors_on(void)
         }
         break;
       case STATUS_STOP_MOTORS:
-        autopilot.motors_on = false;
+        autopilot_arming_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 +220,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_arming_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_arming_check_motors_on();
+  autopilot.kill_throttle = ! autopilot.motors_on;
-    autopilot.kill_throttle = ! autopilot.motors_on;
-  } else {
-    autopilot.arming_status = AP_ARMING_STATUS_AHRS_NOT_ALLIGNED;
-  }
 
   /* 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

@@ -191,12 +191,6 @@ 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;
-  }
-
   if (new_autopilot_mode != autopilot.mode) {
     /* horizontal mode */
     switch (new_autopilot_mode) {
@@ -317,7 +311,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 +356,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_arming_check_motors_on();
+  autopilot.kill_throttle = ! autopilot.motors_on;
-    autopilot.kill_throttle = ! autopilot.motors_on;
-  } else {
-    autopilot.arming_status = AP_ARMING_STATUS_AHRS_NOT_ALLIGNED;
-  }
 
   /* 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

sw/airborne/firmwares/rotorcraft/navigation.h

@@ -231,11 +231,11 @@ extern bool nav_detect_ground(void);
 /* switching motors on/off */
 static inline void NavKillThrottle(void)
 {
-  if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); }
+  if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(false); }
 }
 static inline void NavResurrect(void)
 {
-  if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); }
+  if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(true); }
 }
 
 

sw/airborne/firmwares/rover/autopilot_generated.c

@@ -84,7 +84,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

sw/airborne/firmwares/rover/autopilot_utils.c

@@ -34,21 +34,6 @@
 /** Display descent speed in failsafe mode if needed */
 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
-
 #define THRESHOLD_1_PPRZ (MIN_PPRZ / 2)
 #define THRESHOLD_2_PPRZ (MAX_PPRZ / 2)
 

sw/airborne/firmwares/rover/autopilot_utils.h

@@ -31,7 +31,6 @@
 #include "std.h"
 #include "modules/core/commands.h"
 
-extern bool ap_ahrs_is_aligned(void);
 extern uint8_t ap_mode_of_3way_switch(void);
 
 extern void set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight, bool motors_on);

sw/airborne/firmwares/rover/navigation.h

@@ -190,11 +190,11 @@ extern void nav_set_failsafe(void);
 /* switching motors on/off */
 static inline void NavKillThrottle(void)
 {
-  if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); }
+  if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(false); }
 }
 static inline void NavResurrect(void)
 {
-  if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); }
+  if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(true); }
 }
 
 

sw/airborne/modules/checks/preflight_checks.c

@@ -0,0 +1,233 @@
+/*
+ * Copyright (C) 2022 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file "modules/checks/preflight_checks.c"
+ * @author Freek van Tienen <freek.v.tienen@gmail.com>
+ * Adds preflight checks for takeoff
+ */
+
+#include "preflight_checks.h"
+#include "modules/datalink/telemetry.h"
+#include <stdio.h>
+
+/** Maximum combined message size for storing the errors */
+#ifndef PREFLIGHT_CHECK_MAX_MSGBUF
+#define PREFLIGHT_CHECK_MAX_MSGBUF 512
+#endif
+
+/** Seperating character for storing the errors */
+#ifndef PREFLIGHT_CHECK_SEPERATOR
+#define PREFLIGHT_CHECK_SEPERATOR ';'
+#endif
+
+/** Only send messages down every xx amount of seconds */
+#ifndef PREFLIGHT_CHECK_INFO_TIMEOUT
+#define PREFLIGHT_CHECK_INFO_TIMEOUT 5
+#endif
+
+static struct preflight_check_t *preflight_head = NULL;
+bool preflight_bypass = FALSE;
+
+/**
+ * @brief Register a preflight check and add it to the linked list
+ *
+ * @param check The check to add containing a linked list
+ * @param func The function to register for the check
+ */
+void preflight_check_register(struct preflight_check_t *check, preflight_check_f func)
+{
+  // Prepend the preflight check
+  struct preflight_check_t *next = preflight_head;
+  preflight_head = check;
+  check->func = func;
+  check->next = next;
+}
+
+/**
+ * @brief Perform all the preflight checks
+ *
+ * @return true When all preflight checks are successful
+ * @return false When one or more preflight checks fail
+ */
+bool preflight_check(void)
+{
+  static float last_info_time = 0;
+  char error_msg[PREFLIGHT_CHECK_MAX_MSGBUF];
+  struct preflight_result_t result = {
+    .message = error_msg,
+    .max_len = PREFLIGHT_CHECK_MAX_MSGBUF,
+    .fail_cnt = 0,
+    .warning_cnt = 0,
+    .success_cnt = 0
+  };
+
+  // Go through all the checks
+  struct preflight_check_t *check = preflight_head;
+  while (check != NULL) {
+    // Peform the check and register errors
+    check->func(&result);
+    check = check->next;
+  }
+
+  // We failed a check or have a warning
+  if (result.fail_cnt > 0 || result.warning_cnt > 0) {
+    // Only send every xx amount of seconds
+    if ((get_sys_time_float() - last_info_time) > PREFLIGHT_CHECK_INFO_TIMEOUT) {
+      // Record the total
+      int rc = 0;
+      if (result.fail_cnt > 0) {
+        rc = snprintf(result.message, result.max_len, "Preflight fail [fail:%d warn:%d tot:%d]", result.fail_cnt,
+                      result.warning_cnt, (result.fail_cnt + result.warning_cnt + result.success_cnt));
+      } else {
+        rc = snprintf(result.message, result.max_len, "Preflight success with warnings [%d/%d]", result.warning_cnt,
+                      (result.fail_cnt + result.warning_cnt + result.success_cnt));
+      }
+      if (rc > 0) {
+        result.max_len -= rc;
+      }
+
+      // Send the errors seperatly
+      uint8_t last_sendi = 0;
+      for (uint8_t i = 0; i <= PREFLIGHT_CHECK_MAX_MSGBUF - result.max_len; i++) {
+        if (error_msg[i] == PREFLIGHT_CHECK_SEPERATOR || i == (PREFLIGHT_CHECK_MAX_MSGBUF - result.max_len)) {
+          DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, i - last_sendi, &error_msg[last_sendi]);
+          last_sendi = i + 1;
+        }
+      }
+
+      // Update the time
+      last_info_time = get_sys_time_float();
+    }
+
+    // Only if we fail a check
+    if (result.fail_cnt > 0) {
+      return false;
+    } else {
+      return true;
+    }
+  }
+
+  // Send success down
+  int rc = snprintf(error_msg, PREFLIGHT_CHECK_MAX_MSGBUF, "Preflight success [%d]", result.success_cnt);
+  if (rc > 0) {
+    DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, rc, error_msg);
+  }
+
+  // Return success if we didn't fail a preflight check
+  return true;
+}
+
+/**
+ * @brief Register a preflight error used inside the preflight checking functions
+ *
+ * @param result Where the error gets registered
+ * @param fmt A formatted string describing the error used in a vsnprintf
+ * @param ... The arguments for the vsnprintf
+ */
+void preflight_error(struct preflight_result_t *result, const char *fmt, ...)
+{
+  // Record the error count
+  result->fail_cnt++;
+
+  // No more space in the message
+  if (result->max_len <= 0) {
+    return;
+  }
+
+  // Add the error
+  va_list args;
+  va_start(args, fmt);
+  int rc = vsnprintf(result->message, result->max_len, fmt, args);
+  va_end(args);
+
+  // Remove the length from the buffer if it was successfull
+  if (rc > 0) {
+    result->max_len -= rc;
+    result->message += rc;
+
+    // Add seperator if it fits
+    if (result->max_len > 0) {
+      result->message[0] = PREFLIGHT_CHECK_SEPERATOR;
+      result->max_len--;
+      result->message++;
+
+      // Add the '\0' character
+      if (result->max_len > 0) {
+        result->message[0] = 0;
+      }
+    }
+  }
+}
+
+/**
+ * @brief Register a preflight error used inside the preflight checking functions
+ *
+ * @param result Where the error gets registered
+ * @param fmt A formatted string describing the error used in a vsnprintf
+ * @param ... The arguments for the vsnprintf
+ */
+void preflight_warning(struct preflight_result_t *result, const char *fmt, ...)
+{
+  // Record the warning count
+  result->warning_cnt++;
+
+  // No more space in the message
+  if (result->max_len <= 0) {
+    return;
+  }
+
+  // Add the warning to the error string
+  va_list args;
+  va_start(args, fmt);
+  int rc = vsnprintf(result->message, result->max_len, fmt, args);
+  va_end(args);
+
+  // Remove the length from the buffer if it was successfull
+  if (rc > 0) {
+    result->max_len -= rc;
+    result->message += rc;
+
+    // Add seperator if it fits
+    if (result->max_len > 0) {
+      result->message[0] = PREFLIGHT_CHECK_SEPERATOR;
+      result->max_len--;
+      result->message++;
+
+      // Add the '\0' character
+      if (result->max_len > 0) {
+        result->message[0] = 0;
+      }
+    }
+  }
+}
+
+/**
+ * @brief Register a preflight success used inside the preflight checking functions
+ *
+ * @param result
+ * @param __attribute__
+ * @param ...
+ */
+void preflight_success(struct preflight_result_t *result, const char *fmt __attribute__((unused)), ...)
+{
+  // Record the success count
+  result->success_cnt++;
+}

sw/airborne/modules/checks/preflight_checks.h

@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2022 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file "modules/checks/preflight_checks.h"
+ * @author Freek van Tienen <freek.v.tienen@gmail.com>
+ * Adds preflight checks for takeoff
+ */
+
+#ifndef PREFLIGHT_CHECKS_H
+#define PREFLIGHT_CHECKS_H
+
+#include "std.h"
+#include <stdarg.h>
+
+struct preflight_result_t {
+  char *message;
+  uint16_t max_len;
+  uint16_t fail_cnt;
+  uint16_t warning_cnt;
+  uint16_t success_cnt;
+};
+
+typedef void (*preflight_check_f)(struct preflight_result_t *result);
+
+struct preflight_check_t {
+  preflight_check_f func;
+  struct preflight_check_t *next;
+};
+
+extern bool preflight_bypass;
+extern void preflight_check_register(struct preflight_check_t *check, preflight_check_f func);
+extern bool preflight_check(void);
+extern void preflight_error(struct preflight_result_t *result, const char *fmt, ...);
+extern void preflight_warning(struct preflight_result_t *result, const char *fmt, ...);
+extern void preflight_success(struct preflight_result_t *result, const char *fmt, ...);
+
+#endif /* PREFLIGHT_CHECKS_H */

sw/airborne/modules/energy/electrical.c

@@ -33,6 +33,7 @@
 #include "autopilot.h"
 
 #include "generated/airframe.h"
+#include "generated/modules.h"
 #include BOARD_CONFIG
 
 #ifdef MILLIAMP_PER_PERCENT
@@ -52,17 +53,39 @@
 #define BAT_CHECKER_DELAY 5
 #endif
 
-#define ELECTRICAL_PERIODIC_FREQ 10
-static float period_to_hour = 1 / 3600.f / ELECTRICAL_PERIODIC_FREQ;
-
 #ifndef MIN_BAT_LEVEL
 #define MIN_BAT_LEVEL 3
 #endif
 
+#ifndef TAKEOFF_BAT_LEVEL
+#define TAKEOFF_BAT_LEVEL LOW_BAT_LEVEL
+#endif
+PRINT_CONFIG_VAR(TAKEOFF_BAT_LEVEL)
+
 PRINT_CONFIG_VAR(LOW_BAT_LEVEL)
 PRINT_CONFIG_VAR(CRITIC_BAT_LEVEL)
 PRINT_CONFIG_VAR(MIN_BAT_LEVEL)
 
+#ifndef VoltageOfAdc
+#define VoltageOfAdc(adc) DefaultVoltageOfAdc(adc)
+#endif
+#ifndef MilliAmpereOfAdc
+#define MilliAmpereOfAdc(adc) DefaultMilliAmpereOfAdc(adc)
+#endif
+
+#ifndef CURRENT_ESTIMATION_NONLINEARITY
+#define CURRENT_ESTIMATION_NONLINEARITY 1.2
+#endif
+PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY)
+
+#if defined MILLIAMP_AT_FULL_THROTTLE && !defined MILLIAMP_AT_IDLE_THROTTLE
+  PRINT_CONFIG_MSG("Assuming 0 mA at idle throttle")
+  #define MILLIAMP_AT_IDLE_THROTTLE 0
+#endif
+
+PRINT_CONFIG_VAR(MILLIAMP_AT_IDLE_THROTTLE)
+
+/* Main external structure */
 struct Electrical electrical;
 
 #if defined ADC_CHANNEL_VSUPPLY || (defined ADC_CHANNEL_CURRENT && !defined SITL) || defined MILLIAMP_AT_FULL_THROTTLE
@@ -82,23 +105,19 @@ static struct {
 } electrical_priv;
 #endif
 
-#ifndef VoltageOfAdc
+#ifdef PREFLIGHT_CHECKS
-#define VoltageOfAdc(adc) DefaultVoltageOfAdc(adc)
+/* Preflight checks */
-#endif
+#include "modules/checks/preflight_checks.h"
-#ifndef MilliAmpereOfAdc
+static struct preflight_check_t electrical_pfc;
-#define MilliAmpereOfAdc(adc) DefaultMilliAmpereOfAdc(adc)
-#endif
 
-#ifndef CURRENT_ESTIMATION_NONLINEARITY
+static void electrical_preflight(struct preflight_result_t *result) {
-#define CURRENT_ESTIMATION_NONLINEARITY 1.2
+  if(electrical.vsupply < TAKEOFF_BAT_LEVEL) {
-#endif
+    preflight_error(result, "Battery level %.2fV below minimum takeoff level %.2fV", electrical.vsupply, TAKEOFF_BAT_LEVEL);
-
+  } else {
-#if defined MILLIAMP_AT_FULL_THROTTLE && !defined MILLIAMP_AT_IDLE_THROTTLE
+    preflight_success(result, "Battery level %.2fV above takeoff level %.2fV", electrical.vsupply, TAKEOFF_BAT_LEVEL);
-  PRINT_CONFIG_MSG("Assuming 0 mA at idle throttle")
+  }
-  #define MILLIAMP_AT_IDLE_THROTTLE 0
+}
-#endif
+#endif // PREFLIGHT_CHECKS
-
-PRINT_CONFIG_VAR(MILLIAMP_AT_IDLE_THROTTLE)
 
 void electrical_init(void)
 {
@@ -124,9 +143,13 @@ void electrical_init(void)
   adc_buf_channel(ADC_CHANNEL_CURRENT2, &electrical_priv.current2_adc_buf, DEFAULT_AV_NB_SAMPLE);
 #endif
 #elif defined MILLIAMP_AT_FULL_THROTTLE
-  PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY)
   electrical_priv.nonlin_factor = CURRENT_ESTIMATION_NONLINEARITY;
 #endif
+
+  /* Register preflight checks */
+#if PREFLIGHT_CHECKS
+  preflight_check_register(&electrical_pfc, electrical_preflight);
+#endif
 }
 
 void electrical_periodic(void)
@@ -188,6 +211,7 @@ void electrical_periodic(void)
 #endif
 #endif /* ADC_CHANNEL_CURRENT */
 
+  static const float period_to_hour = 1 / 3600.f / ELECTRICAL_PERIODIC_FREQ;
   float consumed_since_last = electrical.current * period_to_hour;
 
   electrical.charge += consumed_since_last;

sw/airborne/modules/gps/gps.c

@@ -77,6 +77,20 @@ static uint8_t current_gps_id = GpsId(PRIMARY_GPS);
 
 uint8_t multi_gps_mode;
 
+#if PREFLIGHT_CHECKS
+/* Preflight checks */
+#include "modules/checks/preflight_checks.h"
+static struct preflight_check_t gps_pfc;
+
+static void gps_preflight(struct preflight_result_t *result) {
+  if(!gps_fix_valid()) {
+    preflight_error(result, "No valid GPS fix");
+  } else {
+    preflight_success(result, "GPS fix ok");
+  }
+}
+#endif // PREFLIGHT_CHECKS
+
 
 #if PERIODIC_TELEMETRY
 #include "modules/datalink/telemetry.h"
@@ -353,6 +367,11 @@ void gps_init(void)
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_RXMRTCM, send_gps_rxmrtcm);
 #endif
 
+  /* Register preflight checks */
+#if PREFLIGHT_CHECKS
+  preflight_check_register(&gps_pfc, gps_preflight);
+#endif
+
   // Initializing counter variables to count the number of Rtcm msgs in the input stream(for each msg type)
   rtcm_man.Cnt105 = 0;
   rtcm_man.Cnt177 = 0;

sw/airborne/modules/loggers/sdlog_chibios.c

@@ -106,6 +106,24 @@ static enum {
 static char chibios_sdlog_filenames[68];
 static char NO_FILE_NAME[] = "none";
 
+#if PREFLIGHT_CHECKS
+/* Preflight checks */
+#include "modules/checks/preflight_checks.h"
+static struct preflight_check_t sdlog_pfc;
+
+static void sdlog_preflight(struct preflight_result_t *result) {
+  if(chibios_sdlog_status != SDLOG_RUNNING) {
+#ifdef SDLOG_PREFLIGHT_ERROR
+    preflight_error(result, "SDLogger is not running [%d: %d]", chibios_sdlog_status, sdLogGetStorageStatus());
+#else
+    preflight_warning(result, "SDLogger is not running [%d: %d]", chibios_sdlog_status, sdLogGetStorageStatus());
+#endif
+  } else {
+    preflight_success(result, "SDLogger running");
+  }
+}
+#endif // PREFLIGHT_CHECKS
+
 #if PERIODIC_TELEMETRY
 #include "modules/datalink/telemetry.h"
 static void send_sdlog_status(struct transport_tx *trans, struct link_device *dev)
@@ -184,6 +202,10 @@ void sdlog_chibios_init(void)
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_LOGGER_STATUS, send_sdlog_status);
 #endif
 
+#if PREFLIGHT_CHECKS
+  preflight_check_register(&sdlog_pfc, sdlog_preflight);
+#endif
+
   // Start polling on USB
   usbStorageStartPolling();
 

sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c

@@ -159,6 +159,20 @@ static struct i2c_transaction ms45xx_trans;
 static Butterworth2LowPass ms45xx_filter;
 #endif
 
+#if PREFLIGHT_CHECKS
+/* Preflight checks */
+#include "modules/checks/preflight_checks.h"
+static struct preflight_check_t ms45xx_i2c_pfc;
+
+static void ms45xx_preflight(struct preflight_result_t *result) {
+  if(ms45xx.offset_set) {
+    preflight_success(result, "Airspeed sensor succesfully nulled (MS45XX)");
+  } else {
+    preflight_error(result, "Airspeed sensor not nulled (MS45XX)");
+  }
+}
+#endif // PREFLIGHT_CHECKS
+
 static void ms45xx_downlink(struct transport_tx *trans, struct link_device *dev)
 {
   uint8_t dev_id = MS45XX_SENDER_ID;
@@ -181,6 +195,7 @@ void ms45xx_i2c_init(void)
   ms45xx.pressure_type = MS45XX_PRESSURE_TYPE;
   ms45xx.pressure_scale = MS45XX_PRESSURE_SCALE;
   ms45xx.pressure_offset = MS45XX_PRESSURE_OFFSET;
+  ms45xx.offset_set = false;
 
   ms45xx_trans.status = I2CTransDone;
   // setup low pass filter with time constant and 100Hz sampling freq
@@ -192,6 +207,11 @@ void ms45xx_i2c_init(void)
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_RAW, ms45xx_downlink);
 #endif
+
+  /* Register preflight checks */
+#if PREFLIGHT_CHECKS
+  preflight_check_register(&ms45xx_i2c_pfc, ms45xx_preflight);
+#endif
 }
 
 void ms45xx_i2c_periodic(void)
@@ -256,6 +276,7 @@ void ms45xx_i2c_event(void)
           autoset_offset = 0.f;
           autoset_nb = 0;
           ms45xx.autoset_offset = false;
+          ms45xx.offset_set = true;
         }
       }
 

sw/airborne/modules/sensors/airspeed_ms45xx_i2c.h

@@ -37,6 +37,7 @@ struct AirspeedMs45xx {
   float pressure_scale;        ///< Scaling factor from raw measurement to Pascal
   float pressure_offset;       ///< Offset in Pascal
   bool autoset_offset;         ///< Set offset value from current filtered value
+  bool offset_set;             ///< Offset is set once
 };
 
 extern struct AirspeedMs45xx ms45xx;

sw/airborne/modules/sonar/agl_dist.c

@@ -50,9 +50,23 @@ float agl_measurement_time;
 #endif
 
 abi_event agl_ev;
-
 static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
 
+
+#if PREFLIGHT_CHECKS && defined(AGL_DIST_MIN_DISTANCE_CHECK) && defined(AGL_DIST_MAX_DISTANCE_CHECK)
+/* Preflight checks */
+#include "modules/checks/preflight_checks.h"
+static struct preflight_check_t agl_dist_pfc;
+
+static void agl_dist_preflight(struct preflight_result_t *result) {
+  if(agl_dist_valid && agl_dist_value > AGL_DIST_MIN_DISTANCE_CHECK && agl_dist_value < AGL_DIST_MAX_DISTANCE_CHECK) {
+    preflight_success(result, "AGL within limits %.2f < %.2f < %.2f", AGL_DIST_MIN_DISTANCE_CHECK, agl_dist_value, AGL_DIST_MAX_DISTANCE_CHECK);
+  } else {
+    preflight_error(result, "AGL outside limits %.2f < %.2f < %.2f", AGL_DIST_MIN_DISTANCE_CHECK, agl_dist_value, AGL_DIST_MAX_DISTANCE_CHECK);
+  }
+}
+#endif // PREFLIGHT_CHECKS && defined(AGL_DIST_MIN_DISTANCE_CHECK) && defined(AGL_DIST_MAX_DISTANCE_CHECK)
+
 void agl_dist_init(void)
 {
   agl_dist_valid = false;
@@ -62,6 +76,11 @@ void agl_dist_init(void)
 
   // Bind to AGL message
   AbiBindMsgAGL(AGL_DIST_ID, &agl_ev, agl_cb);
+
+  /* Register preflight checks */
+#if PREFLIGHT_CHECKS && defined(AGL_DIST_MIN_DISTANCE_CHECK) && defined(AGL_DIST_MAX_DISTANCE_CHECK)
+  preflight_check_register(&agl_dist_pfc, agl_dist_preflight);
+#endif // PREFLIGHT_CHECKS && defined(AGL_DIST_MIN_DISTANCE_CHECK) && defined(AGL_DIST_MAX_DISTANCE_CHECK)
 }
 
 static void agl_cb(uint8_t __attribute__((unused)) sender_id, uint32_t __attribute__((unused)) stamp, float distance)

sw/airborne/state.c

@@ -35,6 +35,20 @@
 
 struct State state;
 
+#if PREFLIGHT_CHECKS && !defined(AUTOPILOT_DISABLE_AHRS_KILL)
+/* Preflight checks */
+#include "modules/checks/preflight_checks.h"
+static struct preflight_check_t state_pfc;
+
+static void state_preflight(struct preflight_result_t *result) {
+  if(!stateIsAttitudeValid()) {
+    preflight_error(result, "State attitude is invalid");
+  } else {
+    preflight_success(result, "State attitude is valid");
+  }
+}
+#endif // PREFLIGHT_CHECKS
+
 /**
  * @addtogroup state_interface
  * @{
@@ -54,6 +68,11 @@ void stateInit(void)
 
   /* setting to zero forces recomputation of zone using lla when utm uninitialised*/
   state.utm_origin_f.zone = 0;
+
+  /* Register preflight checks */
+#if PREFLIGHT_CHECKS && !defined(AUTOPILOT_DISABLE_AHRS_KILL)
+  preflight_check_register(&state_pfc, state_preflight);
+#endif
 }