[checks] Add preflight checks

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/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/preflight_checks.xml

@@ -0,0 +1,28 @@
+<!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>
+  </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/userconf/tudelft/conf.xml

@@ -73,7 +73,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/delft_basic.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
-   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml [modules/geo_mag.xml] modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
+   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/electrical.xml [modules/geo_mag.xml] modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/stabilization_indi_simple.xml"
    gui_color="#ffffcccaccca"
   />
   <aircraft

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,23 @@ 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)
+{
+#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;
+}
+
 /** get motors status
  */
 bool autopilot_get_motors_on(void)

sw/airborne/autopilot.h

@@ -122,12 +122,23 @@ 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);
 
 /** 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,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);
         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) {