diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp
index 6265e07289..d97bab6bcc 100644
--- a/APMrover2/APMrover2.cpp
+++ b/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
};
/*
diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp
index 2f3f0d3982..fe62279318 100644
--- a/APMrover2/Parameters.cpp
+++ b/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);
}
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index 42742db513..3e2964e5f1 100644
--- a/APMrover2/Parameters.h
+++ b/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[];
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 386ac83281..62c1ff9063 100644
--- a/APMrover2/Rover.h
+++ b/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);
diff --git a/APMrover2/afs_rover.cpp b/APMrover2/afs_rover.cpp
new file mode 100644
index 0000000000..c08c9ff6dc
--- /dev/null
+++ b/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
diff --git a/APMrover2/afs_rover.h b/APMrover2/afs_rover.h
new file mode 100644
index 0000000000..60bc96ab63
--- /dev/null
+++ b/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 .
+ */
+#pragma once
+
+/*
+ advanced failsafe support for rover
+ */
+
+#if ADVANCED_FAILSAFE == ENABLED
+#include
+
+/*
+ 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
diff --git a/APMrover2/config.h b/APMrover2/config.h
index 7382298688..3b5131cc3c 100644
--- a/APMrover2/config.h
+++ b/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
diff --git a/APMrover2/failsafe.cpp b/APMrover2/failsafe.cpp
index b749bc984e..5a4849ea7c 100644
--- a/APMrover2/failsafe.cpp
+++ b/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
diff --git a/APMrover2/wscript b/APMrover2/wscript
index 1c03981d69..af2d3a544e 100644
--- a/APMrover2/wscript
+++ b/APMrover2/wscript
@@ -21,6 +21,7 @@ def build(bld):
'PID',
'AP_Stats',
'AP_Beacon',
+ 'AP_AdvancedFailsafe',
],
)