Copter: added optional AdvancedFailsafe for copter and heli

ArduCopter/ArduCopter.cpp

@@ -128,6 +128,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK(accel_cal_update,      10,    100),
 #if ADSB_ENABLED == ENABLED
     SCHED_TASK(avoidance_adsb_update, 10,    100),
+#endif
+#if ADVANCED_FAILSAFE == ENABLED
+    SCHED_TASK(afs_fs_check,          10,    100),
 #endif
     SCHED_TASK(terrain_update,        10,    100),
 #if EPM_ENABLED == ENABLED

ArduCopter/Copter.h

@@ -114,6 +114,10 @@
 #include <AP_Frsky_Telem/AP_Frsky_Telem.h>
 #endif
 
+#if ADVANCED_FAILSAFE == ENABLED
+#include "afs_copter.h"
+#endif
+
 // Local modules
 #include "Parameters.h"
 #include "avoidance_adsb.h"
@@ -122,12 +126,17 @@
 #include <SITL/SITL.h>
 #endif
 
+
 class Copter : public AP_HAL::HAL::Callbacks {
 public:
     friend class GCS_MAVLINK_Copter;
     friend class AP_Rally_Copter;
     friend class Parameters;
+    friend class ParametersG2;
     friend class AP_Avoidance_Copter;
+#if ADVANCED_FAILSAFE == ENABLED
+    friend class AP_AdvancedFailsafe_Copter;
+#endif
 
     Copter(void);
 
@@ -592,6 +601,9 @@ private:
     // true if we are out of time in our event timeslice
     bool gcs_out_of_time;
 
+    // last valid RC input time
+    uint32_t last_radio_update_ms;
+    
     // Top-level logic
     // setup the var_info table
     AP_Param param_loader;
@@ -818,6 +830,9 @@ private:
     void autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max);
     void autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max);
     void avoidance_adsb_update(void);
+#if ADVANCED_FAILSAFE == ENABLED
+    void afs_fs_check(void);
+#endif
     bool brake_init(bool ignore_checks);
     void brake_run();
     void brake_timeout_to_loiter_ms(uint32_t timeout_ms);

ArduCopter/Parameters.cpp

@@ -995,10 +995,26 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("GND_EFFECT_COMP", 5, ParametersG2, gndeffect_comp_enabled, 0),
 
+#if ADVANCED_FAILSAFE == ENABLED
+    // @Group: AFS_
+    // @Path: ../libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp
+    AP_SUBGROUPINFO(afs, "AFS_", 6, ParametersG2, AP_AdvancedFailsafe),
+#endif
+    
     AP_GROUPEND
 };
 
 
+/*
+  constructor for g2 object
+ */
+ParametersG2::ParametersG2(void)
+#if ADVANCED_FAILSAFE == ENABLE
+    : afs(copter.mission, copter.barometer, copter.gps, copter.rcmap)
+#endif
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
 
 /*
   This is a conversion table from old parameter values to new

ArduCopter/Parameters.h

@@ -540,7 +540,7 @@ public:
  */
 class ParametersG2 {
 public:
-    ParametersG2(void) { AP_Param::setup_object_defaults(this, var_info); }
+    ParametersG2(void);
 
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
@@ -557,6 +557,11 @@ public:
 
     // ground effect compensation enable/disable
     AP_Int8 gndeffect_comp_enabled;
+
+#if ADVANCED_FAILSAFE == ENABLED
+    // advanced failsafe library
+    AP_AdvancedFailsafe_Copter afs;
+#endif
 };
 
 extern const AP_Param::Info        var_info[];

ArduCopter/afs_copter.cpp

@@ -0,0 +1,74 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+  copter specific AP_AdvancedFailsafe class
+ */
+
+#include "Copter.h"
+
+#if ADVANCED_FAILSAFE == ENABLED
+
+// Constructor
+AP_AdvancedFailsafe_Copter::AP_AdvancedFailsafe_Copter(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_Copter::terminate_vehicle(void)
+{
+    // stop motors
+    copter.motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
+    copter.motors.output();
+
+    // disarm as well
+    copter.init_disarm_motors();
+    
+    // and set all aux channels
+    RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_heli_rsc, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+    RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_heli_tail_rsc, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+    RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_engine_run_enable, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+    RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_ignition, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+    RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_none, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+    RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_manual, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+
+    RC_Channel_aux::output_ch_all();
+}
+
+void AP_AdvancedFailsafe_Copter::setup_IO_failsafe(void)
+{
+    // setup failsafe for all aux channels
+    RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_heli_rsc, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+    RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_heli_tail_rsc, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+    RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_engine_run_enable, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+    RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_ignition, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+    RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_none, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+    RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_manual, RC_Channel::RC_CHANNEL_LIMIT_TRIM);
+
+#if FRAME_CONFIG != HELI_FRAME
+    // setup AP_Motors outputs for failsafe
+    uint16_t mask = copter.motors.get_motor_mask();
+    hal.rcout->set_failsafe_pwm(mask, copter.motors.get_pwm_output_min());
+#endif
+}
+
+/*
+  return an AFS_MODE for current control mode
+ */
+AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void)
+{
+    switch (copter.control_mode) {
+    case AUTO:
+    case GUIDED:
+    case RTL:
+    case LAND:
+        return AP_AdvancedFailsafe::AFS_AUTO;
+    default:
+        break;
+    }
+    return AP_AdvancedFailsafe::AFS_STABILIZED;
+}
+
+#endif // ADVANCED_FAILSAFE

ArduCopter/afs_copter.h

@@ -0,0 +1,45 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program 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 3 of the License, or
+   (at your option) any later version.
+
+   This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+/*
+  advanced failsafe support for copter
+ */
+
+#if ADVANCED_FAILSAFE == ENABLED
+#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
+
+/*
+  a plane specific AP_AdvancedFailsafe class
+ */
+class AP_AdvancedFailsafe_Copter : public AP_AdvancedFailsafe
+{
+public:
+    AP_AdvancedFailsafe_Copter(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);
+};
+
+#endif // ADVANCED_FAILSAFE
+

ArduCopter/config.h

@@ -668,3 +668,7 @@
 #ifndef FRSKY_TELEM_ENABLED
   #  define FRSKY_TELEM_ENABLED          ENABLED
 #endif
+
+#ifndef ADVANCED_FAILSAFE
+# define ADVANCED_FAILSAFE DISABLE
+#endif

ArduCopter/failsafe.cpp

@@ -71,3 +71,15 @@ void Copter::failsafe_check()
         }
     }
 }
+
+
+#if ADVANCED_FAILSAFE == ENABLED
+/*
+  check for AFS failsafe check
+*/
+void Copter::afs_fs_check(void)
+{
+    // perform AFS failsafe checks
+    g2.afs.check(failsafe.last_heartbeat_ms, fence.get_breaches() != 0, last_radio_update_ms);
+}
+#endif

ArduCopter/motors.cpp

@@ -267,6 +267,16 @@ void Copter::init_disarm_motors()
 // motors_output - send output to motors library which will adjust and send to ESCs and servos
 void Copter::motors_output()
 {
+#if ADVANCED_FAILSAFE == ENABLED
+    // this is to allow the failsafe module to deliberately crash 
+    // the vehicle. Only used in extreme circumstances to meet the
+    // OBC rules
+    if (g2.afs.should_crash_vehicle()) {
+        g2.afs.terminate_vehicle();
+        return;
+    }
+#endif
+
     // Update arming delay state
     if (ap.in_arming_delay && (!motors.armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) {
         ap.in_arming_delay = false;

ArduCopter/radio.cpp

@@ -97,7 +97,6 @@ void Copter::enable_motor_output()
 
 void Copter::read_radio()
 {
-    static uint32_t last_update_ms = 0;
     uint32_t tnow_ms = millis();
 
     if (hal.rcin->new_input()) {
@@ -118,11 +117,11 @@ void Copter::read_radio()
         // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters)
         radio_passthrough_to_motors();
 
-        float dt = (tnow_ms - last_update_ms)*1.0e-3f;
+        float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f;
         rc_throttle_control_in_filter.apply(g.rc_3.get_control_in(), dt);
-        last_update_ms = tnow_ms;
+        last_radio_update_ms = tnow_ms;
     }else{
-        uint32_t elapsed = tnow_ms - last_update_ms;
+        uint32_t elapsed = tnow_ms - last_radio_update_ms;
         // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE
         if (((!failsafe.rc_override_active && (elapsed >= FS_RADIO_TIMEOUT_MS)) || (failsafe.rc_override_active && (elapsed >= FS_RADIO_RC_OVERRIDE_TIMEOUT_MS))) &&
             (g.failsafe_throttle && (ap.rc_receiver_present||motors.armed()) && !failsafe.radio)) {

ArduCopter/wscript

@@ -29,6 +29,7 @@ def build(bld):
             'AP_Relay',
             'AP_ServoRelayEvents',
             'AP_Avoidance',
+            'AP_AdvancedFailsafe'
         ],
     )