Plane: re-work AFS for new AP_AdvancedFailsafe API

ArduPlane/ArduPlane.cpp

@@ -291,7 +291,7 @@ void Plane::update_logging2(void)
 void Plane::afs_fs_check(void)
 {
     // perform AFS failsafe checks
-    afs.check(AFS_MODE_PLANE(control_mode), failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms);
+    afs.check(failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms);
 }
 
 

ArduPlane/Attitude.cpp

@@ -854,6 +854,14 @@ uint16_t Plane::throttle_min(void) const
 *****************************************/
 void Plane::set_servos(void)
 {
+    // this is to allow the failsafe module to deliberately crash 
+    // the plane. Only used in extreme circumstances to meet the
+    // OBC rules
+    if (afs.should_crash_vehicle()) {
+        afs.terminate_vehicle();
+        return;
+    }
+
     int16_t last_throttle = channel_throttle->get_radio_out();
 
     // do any transition updates for quadplane
@@ -1219,11 +1227,6 @@ void Plane::set_servos(void)
         }
     }
 
-    // this is to allow the failsafe module to deliberately crash 
-    // the plane. Only used in extreme circumstances to meet the
-    // OBC rules
-    afs.check_crash_plane();
-
 #if HIL_SUPPORT
     if (g.hil_mode == 1) {
         // get the servos to the GCS immediately for HIL

ArduPlane/Plane.h

@@ -126,6 +126,26 @@ protected:
     bool ins_checks(bool report);
 };
 
+
+/*
+  a plane specific AP_AdvancedFailsafe class
+ */
+class AP_AdvancedFailsafe_Plane : public AP_AdvancedFailsafe
+{
+public:
+    AP_AdvancedFailsafe_Plane(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps, const RCMapper &_rcmap);
+
+    // called to set all outputs to termination state
+    void terminate_vehicle(void);
+    
+protected:
+    // setup failsafe values for if FMU firmware stops running
+    void setup_IO_failsafe(void);
+
+    // return the AFS mapped control mode
+    enum control_mode afs_mode(void);
+};
+
 /*
   main APM:Plane class
  */
@@ -137,6 +157,7 @@ public:
     friend class AP_Arming_Plane;
     friend class QuadPlane;
     friend class AP_Tuning_Plane;
+    friend class AP_AdvancedFailsafe_Plane;
 
     Plane(void);
 
@@ -632,7 +653,7 @@ private:
     AP_ADSB adsb {ahrs};
 
     // Outback Challenge Failsafe Support
-    AP_AdvancedFailsafe afs {mission, barometer, gps, rcmap};
+    AP_AdvancedFailsafe_Plane afs {mission, barometer, gps, rcmap};
 
     /*
       meta data to support counting the number of circles in a loiter

ArduPlane/afs_plane.cpp

@@ -0,0 +1,78 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+  plane specific AP_AdvancedFailsafe class
+ */
+
+#include "Plane.h"
+
+// Constructor
+AP_AdvancedFailsafe_Plane::AP_AdvancedFailsafe_Plane(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps, const RCMapper &_rcmap) :
+    AP_AdvancedFailsafe(_mission, _baro, _gps, _rcmap)
+{}
+
+
+/*
+  setup radio_out values for all channels to termination values
+ */
+void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
+{
+    // we are terminating. Setup primary output channels radio_out values
+    RC_Channel *ch_roll     = RC_Channel::rc_channel(rcmap.roll()-1);
+    RC_Channel *ch_pitch    = RC_Channel::rc_channel(rcmap.pitch()-1);
+    RC_Channel *ch_yaw      = RC_Channel::rc_channel(rcmap.yaw()-1);
+    RC_Channel *ch_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
+
+    ch_roll->set_radio_out(ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
+    ch_pitch->set_radio_out(ch_pitch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
+    ch_yaw->set_radio_out(ch_yaw->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
+    ch_throttle->set_radio_out(ch_throttle->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
+
+    // and all aux channels
+    RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_flap_auto, RC_Channel::RC_CHANNEL_LIMIT_MAX);
+    RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_flap, RC_Channel::RC_CHANNEL_LIMIT_MAX);
+    RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_aileron, RC_Channel::RC_CHANNEL_LIMIT_MIN);
+    RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_rudder, RC_Channel::RC_CHANNEL_LIMIT_MAX);
+    RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_elevator, RC_Channel::RC_CHANNEL_LIMIT_MAX);
+    RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_elevator_with_input, RC_Channel::RC_CHANNEL_LIMIT_MAX);
+    RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_manual, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+    RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_none, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+}
+
+void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void)
+{
+    const RC_Channel *ch_roll     = RC_Channel::rc_channel(rcmap.roll()-1);
+    const RC_Channel *ch_pitch    = RC_Channel::rc_channel(rcmap.pitch()-1);
+    const RC_Channel *ch_yaw      = RC_Channel::rc_channel(rcmap.yaw()-1);
+    const RC_Channel *ch_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
+
+    // setup primary channel output values
+    hal.rcout->set_failsafe_pwm(1U<<(rcmap.roll()-1),     ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
+    hal.rcout->set_failsafe_pwm(1U<<(rcmap.pitch()-1),    ch_pitch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
+    hal.rcout->set_failsafe_pwm(1U<<(rcmap.yaw()-1),      ch_yaw->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
+    hal.rcout->set_failsafe_pwm(1U<<(rcmap.throttle()-1), ch_throttle->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
+
+    // and all aux channels
+    RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_flap_auto, RC_Channel::RC_CHANNEL_LIMIT_MAX);
+    RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_flap, RC_Channel::RC_CHANNEL_LIMIT_MAX);
+    RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_aileron, RC_Channel::RC_CHANNEL_LIMIT_MIN);
+    RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_rudder, RC_Channel::RC_CHANNEL_LIMIT_MAX);
+    RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_elevator, RC_Channel::RC_CHANNEL_LIMIT_MAX);
+    RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_elevator_with_input, RC_Channel::RC_CHANNEL_LIMIT_MAX);
+    RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_manual, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+    RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_none, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+}
+
+/*
+  return an AFS_MODE for current control mode
+ */
+AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Plane::afs_mode(void)
+{
+    if (plane.auto_throttle_mode) {
+        return AP_AdvancedFailsafe::AFS_AUTO;
+    }
+    if (plane.control_mode == MANUAL) {
+        return AP_AdvancedFailsafe::AFS_MANUAL;
+    }
+    return AP_AdvancedFailsafe::AFS_STABILIZED;
+}

ArduPlane/failsafe.cpp

@@ -82,7 +82,10 @@ void Plane::failsafe_check(void)
         // this is to allow the failsafe module to deliberately crash 
         // the plane. Only used in extreme circumstances to meet the
         // OBC rules
-        afs.check_crash_plane();
+        if (afs.should_crash_vehicle()) {
+            afs.terminate_vehicle();
+            return;
+        }
 
         if (!demoing_servos) {
             channel_roll->output();