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

Rover: make Failsafe_Action enum class

This commit is contained in:
Pierre Kancir 2023-09-21 16:22:37 +02:00 committed by Peter Barker
parent 499dd5ea5b
commit 7dc0c567ce
4 changed files with 48 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Rover/Parameters.cpp
View File

@ -109,7 +109,7 @@ const AP_Param::Info Rover::var_info[] = {
// @Description: What to do on a failsafe event
// @Values: 0:Nothing,1:RTL,2:Hold,3:SmartRTL or RTL,4:SmartRTL or Hold
// @User: Standard
GSCALAR(fs_action, "FS_ACTION", Failsafe_Action_Hold),
GSCALAR(fs_action, "FS_ACTION", (int8_t)FailsafeAction::Hold),
// @Param: FS_TIMEOUT
// @DisplayName: Failsafe timeout

26
Rover/Rover.h
View File

@ -394,13 +394,13 @@ private:
bool get_wp_bearing_deg(float &bearing) const override;
bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
enum Failsafe_Action {
Failsafe_Action_None = 0,
Failsafe_Action_RTL = 1,
Failsafe_Action_Hold = 2,
Failsafe_Action_SmartRTL = 3,
Failsafe_Action_SmartRTL_Hold = 4,
Failsafe_Action_Terminate = 5
enum class FailsafeAction: int8_t {
None = 0,
RTL = 1,
Hold = 2,
SmartRTL = 3,
SmartRTL_Hold = 4,
Terminate = 5
};
enum class Failsafe_Options : uint32_t {
@ -408,12 +408,12 @@ private:
};
static constexpr int8_t _failsafe_priorities[] = {
Failsafe_Action_Terminate,
Failsafe_Action_Hold,
Failsafe_Action_RTL,
Failsafe_Action_SmartRTL_Hold,
Failsafe_Action_SmartRTL,
Failsafe_Action_None,
(int8_t)FailsafeAction::Terminate,
(int8_t)FailsafeAction::Hold,
(int8_t)FailsafeAction::RTL,
(int8_t)FailsafeAction::SmartRTL_Hold,
(int8_t)FailsafeAction::SmartRTL,
(int8_t)FailsafeAction::None,
-1 // the priority list must end with a sentinel of -1
};
static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,

42
Rover/failsafe.cpp
View File

@ -80,25 +80,23 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool o
// continue with mission in auto mode
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe - Continuing Auto Mode");
} else {
switch (g.fs_action) {
case Failsafe_Action_None:
switch ((FailsafeAction)g.fs_action.get()) {
case FailsafeAction::None:
break;
case Failsafe_Action_RTL:
if (!set_mode(mode_rtl, ModeReason::FAILSAFE)) {
set_mode(mode_hold, ModeReason::FAILSAFE);
case FailsafeAction::SmartRTL:
if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
break;
}
break;
case Failsafe_Action_Hold:
set_mode(mode_hold, ModeReason::FAILSAFE);
break;
case Failsafe_Action_SmartRTL:
if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) {
if (!set_mode(mode_rtl, ModeReason::FAILSAFE)) {
set_mode(mode_hold, ModeReason::FAILSAFE);
}
FALLTHROUGH;
case FailsafeAction::RTL:
if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) {
break;
}
FALLTHROUGH;
case FailsafeAction::Hold:
set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
break;
case Failsafe_Action_SmartRTL_Hold:
case FailsafeAction::SmartRTL_Hold:
if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) {
set_mode(mode_hold, ModeReason::FAILSAFE);
}
@ -110,28 +108,28 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool o
void Rover::handle_battery_failsafe(const char* type_str, const int8_t action)
{
switch ((Failsafe_Action)action) {
case Failsafe_Action_None:
switch ((FailsafeAction)action) {
case FailsafeAction::None:
break;
case Failsafe_Action_SmartRTL:
case FailsafeAction::SmartRTL:
if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
break;
}
FALLTHROUGH;
case Failsafe_Action_RTL:
case FailsafeAction::RTL:
if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) {
break;
}
FALLTHROUGH;
case Failsafe_Action_Hold:
case FailsafeAction::Hold:
set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
break;
case Failsafe_Action_SmartRTL_Hold:
case FailsafeAction::SmartRTL_Hold:
if (!set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
}
break;
case Failsafe_Action_Terminate:
case FailsafeAction::Terminate:
#if ADVANCED_FAILSAFE == ENABLED
char battery_type_str[17];
snprintf(battery_type_str, 17, "%s battery", type_str);

30
Rover/fence.cpp
View File

@ -18,28 +18,26 @@ void Rover::fence_check()
// if there is a new breach take action
if (new_breaches) {
// if the user wants some kind of response and motors are armed
if (fence.get_action() != Failsafe_Action_None) {
if ((FailsafeAction)fence.get_action() != FailsafeAction::None) {
// if within 100m of the fence, it will take the action specified by the FENCE_ACTION parameter
if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
switch (fence.get_action()) {
case Failsafe_Action_None:
switch ((FailsafeAction)fence.get_action()) {
case FailsafeAction::None:
break;
case Failsafe_Action_RTL:
if (!set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) {
set_mode(mode_hold, ModeReason::FENCE_BREACHED);
case FailsafeAction::SmartRTL:
if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
break;
}
break;
case Failsafe_Action_Hold:
set_mode(mode_hold, ModeReason::FENCE_BREACHED);
break;
case Failsafe_Action_SmartRTL:
if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) {
if (!set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) {
set_mode(mode_hold, ModeReason::FENCE_BREACHED);
}
FALLTHROUGH;
case FailsafeAction::RTL:
if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) {
break;
}
FALLTHROUGH;
case FailsafeAction::Hold:
set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
break;
case Failsafe_Action_SmartRTL_Hold:
case FailsafeAction::SmartRTL_Hold:
if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) {
set_mode(mode_hold, ModeReason::FENCE_BREACHED);
}