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

View File

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

View File

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

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 // continue with mission in auto mode
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe - Continuing Auto Mode"); gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe - Continuing Auto Mode");
} else { } else {
switch (g.fs_action) { switch ((FailsafeAction)g.fs_action.get()) {
case Failsafe_Action_None: case FailsafeAction::None:
break;
case FailsafeAction::SmartRTL:
if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
break; break;
case Failsafe_Action_RTL:
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; 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::Hold:
set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
break; break;
case Failsafe_Action_SmartRTL_Hold: case FailsafeAction::SmartRTL_Hold:
if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) { if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) {
set_mode(mode_hold, 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) void Rover::handle_battery_failsafe(const char* type_str, const int8_t action)
{ {
switch ((Failsafe_Action)action) { switch ((FailsafeAction)action) {
case Failsafe_Action_None: case FailsafeAction::None:
break; break;
case Failsafe_Action_SmartRTL: case FailsafeAction::SmartRTL:
if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) { if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
break; break;
} }
FALLTHROUGH; FALLTHROUGH;
case Failsafe_Action_RTL: case FailsafeAction::RTL:
if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) { if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) {
break; break;
} }
FALLTHROUGH; FALLTHROUGH;
case Failsafe_Action_Hold: case FailsafeAction::Hold:
set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE); set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
break; break;
case Failsafe_Action_SmartRTL_Hold: case FailsafeAction::SmartRTL_Hold:
if (!set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) { if (!set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE); set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
} }
break; break;
case Failsafe_Action_Terminate: case FailsafeAction::Terminate:
#if ADVANCED_FAILSAFE == ENABLED #if ADVANCED_FAILSAFE == ENABLED
char battery_type_str[17]; char battery_type_str[17];
snprintf(battery_type_str, 17, "%s battery", type_str); snprintf(battery_type_str, 17, "%s battery", type_str);

View File

@ -18,28 +18,26 @@ void Rover::fence_check()
// if there is a new breach take action // if there is a new breach take action
if (new_breaches) { if (new_breaches) {
// if the user wants some kind of response and motors are armed // 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 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) { if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
switch (fence.get_action()) { switch ((FailsafeAction)fence.get_action()) {
case Failsafe_Action_None: case FailsafeAction::None:
break;
case FailsafeAction::SmartRTL:
if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
break; break;
case Failsafe_Action_RTL:
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; 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::Hold:
set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
break; break;
case Failsafe_Action_SmartRTL_Hold: case FailsafeAction::SmartRTL_Hold:
if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) { if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) {
set_mode(mode_hold, ModeReason::FENCE_BREACHED); set_mode(mode_hold, ModeReason::FENCE_BREACHED);
} }