Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: FS_OPTION allows failsafe from hold mode

This commit is contained in:
Henry Wurzburg 2019-09-29 21:32:07 -05:00 committed by Randy Mackay
parent 23983d03b5
commit 006004be13
4 changed files with 16 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
APMrover2/Parameters.cpp
View File

@ -620,6 +620,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Advanced
AP_GROUPINFO("LOIT_SPEED_GAIN", 47, ParametersG2, loiter_speed_gain, 0.5f),
// @Param: FS_OPTIONS
// @DisplayName: Rover Failsafe Options
// @Description: Bitmask to enable Rover failsafe options
// @Values: 0:None,1:Failsafe enabled in Hold mode
// @Bitmask: 0:Failsafe enabled in Hold mode
// @User: Advanced
AP_GROUPINFO("FS_OPTIONS", 48, ParametersG2, fs_options, 0),
AP_GROUPEND
};

3
APMrover2/Parameters.h
View File

@ -394,6 +394,9 @@ public:
// gain for speed of correction in loiter
AP_Float loiter_speed_gain;
// FS options
AP_Int32 fs_options;
};
extern const AP_Param::Info var_info[];

4
APMrover2/Rover.h
View File

@ -462,6 +462,10 @@ private:
Failsafe_Action_Terminate = 5
};
enum class Failsafe_Options : uint32_t {
Failsafe_Option_Active_In_Hold = (1<<0)
};
static constexpr int8_t _failsafe_priorities[] = {
Failsafe_Action_Terminate,
Failsafe_Action_Hold,

2
APMrover2/failsafe.cpp
View File

@ -68,7 +68,7 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
failsafe.bits != 0 &&
millis() - failsafe.start_time > g.fs_timeout * 1000 &&
control_mode != &mode_rtl &&
control_mode != &mode_hold) {
((control_mode != &mode_hold || (g2.fs_options & (uint32_t)Failsafe_Options::Failsafe_Option_Active_In_Hold)))) {
failsafe.triggered = failsafe.bits;
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", (unsigned int)failsafe.triggered);