diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index a7501a028b..dec648fdef 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -91,7 +91,7 @@ const AP_Param::Info Rover::var_info[] = { // @Param: FS_ACTION // @DisplayName: Failsafe Action // @Description: What to do on a failsafe event - // @Values: 0:Nothing,1:RTL,2:Hold,3:SmartRTL or RTL,4:SmartRTL or Hold,5:Terminate + // @Values: 0:Nothing,1:RTL,2:Hold,3:SmartRTL or RTL,4:SmartRTL or Hold,5:Terminate,6:Loiter or Hold // @User: Standard GSCALAR(fs_action, "FS_ACTION", (int8_t)FailsafeAction::Hold), diff --git a/Rover/Rover.h b/Rover/Rover.h index 33c8765067..b18b84a1cc 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -416,7 +416,8 @@ private: Hold = 2, SmartRTL = 3, SmartRTL_Hold = 4, - Terminate = 5 + Terminate = 5, + Loiter_Hold = 6, }; enum class Failsafe_Options : uint32_t { diff --git a/Rover/failsafe.cpp b/Rover/failsafe.cpp index 3aabdfbacf..72f09f3f6e 100644 --- a/Rover/failsafe.cpp +++ b/Rover/failsafe.cpp @@ -101,6 +101,11 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool o set_mode(mode_hold, ModeReason::FAILSAFE); } break; + case FailsafeAction::Loiter_Hold: + if (!set_mode(mode_loiter, ModeReason::FAILSAFE)) { + set_mode(mode_hold, ModeReason::FAILSAFE); + } + break; case FailsafeAction::Terminate: arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE); break; @@ -127,6 +132,11 @@ void Rover::handle_battery_failsafe(const char* type_str, const int8_t action) case FailsafeAction::Hold: set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE); break; + case FailsafeAction::Loiter_Hold: + if (!set_mode(mode_loiter, ModeReason::BATTERY_FAILSAFE)) { + set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE); + } + break; case FailsafeAction::SmartRTL_Hold: if (!set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) { set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE); diff --git a/Rover/fence.cpp b/Rover/fence.cpp index fb54195c3e..0c3e225719 100644 --- a/Rover/fence.cpp +++ b/Rover/fence.cpp @@ -67,6 +67,11 @@ void Rover::fence_check() set_mode(mode_hold, ModeReason::FENCE_BREACHED); } break; + case FailsafeAction::Loiter_Hold: + if (!set_mode(mode_loiter, ModeReason::FENCE_BREACHED)) { + set_mode(mode_hold, ModeReason::FENCE_BREACHED); + } + break; case FailsafeAction::Terminate: arming.disarm(AP_Arming::Method::FENCEBREACH); break;