APMRover2: add advance failsafe

APMrover2/APMrover2.cpp

@@ -77,6 +77,9 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK(button_update,           5,    100),
     SCHED_TASK(stats_update,            1,    100),
     SCHED_TASK(crash_check,            10,   1000),
+#if ADVANCED_FAILSAFE == ENABLED
+    SCHED_TASK(afs_fs_check,           10,    100),
+#endif
 };
 
 /*

APMrover2/Parameters.cpp

@@ -536,12 +536,21 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Group: RC
     // @Path: ../libraries/RC_Channel/RC_Channel.cpp
     AP_SUBGROUPINFO(rc_channels, "RC", 4, ParametersG2, RC_Channels),
-    
+
+#if ADVANCED_FAILSAFE == ENABLED
+    // @Group: AFS_
+    // @Path: ../libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp
+    AP_SUBGROUPINFO(afs, "AFS_", 5, ParametersG2, AP_AdvancedFailsafe),
+#endif
+
     AP_GROUPEND
 };
 
 
 ParametersG2::ParametersG2(void)
+#if ADVANCED_FAILSAFE == ENABLED
+    : afs(rover.mission, rover.barometer, rover.gps, rover.rcmap)
+#endif
 {
     AP_Param::setup_object_defaults(this, var_info);
 }

APMrover2/Parameters.h

@@ -313,6 +313,12 @@ public:
     
     // control over servo output ranges
     SRV_Channels servo_channels;
+
+#if ADVANCED_FAILSAFE == ENABLED
+    // advanced failsafe library
+    AP_AdvancedFailsafe_Rover afs;
+#endif
+
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.h

@@ -79,6 +79,9 @@
 
 // Local modules
 #include "defines.h"
+#if ADVANCED_FAILSAFE == ENABLED
+#include "afs_rover.h"
+#endif
 #include "Parameters.h"
 #include "GCS_Mavlink.h"
 
@@ -94,6 +97,9 @@ public:
     friend class Parameters;
     friend class ParametersG2;
     friend class AP_Arming_Rover;
+#if ADVANCED_FAILSAFE == ENABLED
+    friend class AP_AdvancedFailsafe_Rover;
+#endif
 
     Rover(void);
 
@@ -554,6 +560,9 @@ private:
     void set_loiter_active(const AP_Mission::Mission_Command& cmd);
     void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
     void crash_check();
+#if ADVANCED_FAILSAFE == ENABLED
+    void afs_fs_check(void);
+#endif
 
 public:
     bool print_log_menu(void);

APMrover2/afs_rover.cpp

@@ -0,0 +1,60 @@
+/*
+  Rover specific AP_AdvancedFailsafe class
+ */
+
+#include "Rover.h"
+
+#if ADVANCED_FAILSAFE == ENABLED
+
+// Constructor
+AP_AdvancedFailsafe_Rover::AP_AdvancedFailsafe_Rover(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_Rover::terminate_vehicle(void)
+{
+    // stop motors
+    SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, 0);
+    SRV_Channels::set_output_pwm(SRV_Channel::k_steering, 0);
+    rover.lateral_acceleration = 0;
+
+    // disarm as well
+    rover.disarm_motors();
+
+    // Set to HOLD mode
+    rover.set_mode(HOLD);
+    // and set all aux channels
+    SRV_Channels::set_output_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
+    SRV_Channels::set_output_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
+
+    SRV_Channels::output_ch_all();
+}
+
+void AP_AdvancedFailsafe_Rover::setup_IO_failsafe(void)
+{
+    // setup failsafe for all aux channels
+    SRV_Channels::set_failsafe_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
+    SRV_Channels::set_failsafe_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
+}
+
+/*
+  Return an AFS_MODE for current control mode
+ */
+AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Rover::afs_mode(void)
+{
+    switch (rover.control_mode) {
+    case AUTO:
+    case GUIDED:
+    case RTL:
+        return AP_AdvancedFailsafe::AFS_AUTO;
+    default:
+        break;
+    }
+    return AP_AdvancedFailsafe::AFS_STABILIZED;
+}
+
+#endif  // ADVANCED_FAILSAFE

APMrover2/afs_rover.h

@@ -0,0 +1,43 @@
+/*
+   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 rover
+ */
+
+#if ADVANCED_FAILSAFE == ENABLED
+#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
+
+/*
+  a rover specific AP_AdvancedFailsafe class
+ */
+class AP_AdvancedFailsafe_Rover : public AP_AdvancedFailsafe
+{
+public:
+    AP_AdvancedFailsafe_Rover(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

APMrover2/config.h

@@ -255,3 +255,7 @@
 #ifndef RESET_SWITCH_CHAN_PWM
   #define RESET_SWITCH_CHAN_PWM    1750
 #endif
+
+#ifndef ADVANCED_FAILSAFE
+  #define ADVANCED_FAILSAFE DISABLED
+#endif

APMrover2/failsafe.cpp

@@ -50,3 +50,13 @@ void Rover::failsafe_check()
     }
 }
 
+#if ADVANCED_FAILSAFE == ENABLED
+/*
+   check for AFS failsafe check
+ */
+void Rover::afs_fs_check(void)
+{
+    // perform AFS failsafe checks
+    g2.afs.check(rover.last_heartbeat_ms, false, failsafe.last_valid_rc_ms);  // Rover don't have fence
+}
+#endif

APMrover2/wscript

@@ -21,6 +21,7 @@ def build(bld):
             'PID',
             'AP_Stats',
             'AP_Beacon',
+            'AP_AdvancedFailsafe',
         ],
     )