From e7424a456f73410b3ba60d35442d6ecdb0017fab Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 19 Jul 2017 13:34:04 +0900
Subject: [PATCH] Rover: update advanced failsafe

---
 APMrover2/afs_rover.cpp | 26 ++------------------------
 APMrover2/afs_rover.h   |  8 ++++----
 2 files changed, 6 insertions(+), 28 deletions(-)

diff --git a/APMrover2/afs_rover.cpp b/APMrover2/afs_rover.cpp
index 3e24251375..e8f3763079 100644
--- a/APMrover2/afs_rover.cpp
+++ b/APMrover2/afs_rover.cpp
@@ -17,28 +17,11 @@ AP_AdvancedFailsafe_Rover::AP_AdvancedFailsafe_Rover(AP_Mission &_mission, AP_Ba
  */
 void AP_AdvancedFailsafe_Rover::terminate_vehicle(void)
 {
-    // stop motors
-    SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
-    SRV_Channels::set_output_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
-    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);
+    rover.set_mode(rover.mode_hold);
 }
 
 /*
@@ -46,13 +29,8 @@ void AP_AdvancedFailsafe_Rover::setup_IO_failsafe(void)
  */
 AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Rover::afs_mode(void)
 {
-    switch (rover.control_mode) {
-    case AUTO:
-    case GUIDED:
-    case RTL:
+    if (rover.control_mode->is_autopilot_mode()) {
         return AP_AdvancedFailsafe::AFS_AUTO;
-    default:
-        break;
     }
     return AP_AdvancedFailsafe::AFS_STABILIZED;
 }
diff --git a/APMrover2/afs_rover.h b/APMrover2/afs_rover.h
index 60bc96ab63..fdc77c6140 100644
--- a/APMrover2/afs_rover.h
+++ b/APMrover2/afs_rover.h
@@ -30,14 +30,14 @@ 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);
+    void terminate_vehicle(void) override;
 
 protected:
-    // setup failsafe values for if FMU firmware stops running
-    void setup_IO_failsafe(void);
+    // setup failsafe values - this is handled by motors library
+    void setup_IO_failsafe(void) override {}
 
     // return the AFS mapped control mode
-    enum control_mode afs_mode(void);
+    enum control_mode afs_mode(void) override;
 };
 
 #endif  // ADVANCED_FAILSAFE