From 2fad6ba5b87ab0afff027d5493dc5b118846ad58 Mon Sep 17 00:00:00 2001 From: Alexis Paques Date: Tue, 22 Oct 2024 11:17:31 +0200 Subject: [PATCH] Navigator: failsafe: Add descend mode as a failsafe. The land mode supposes we have a GPS causing issues when flying with a local position (such as mocap or vision) This adds the alternative to use the descend mode. --- src/modules/commander/commander_params.c | 16 +++++++- src/modules/commander/failsafe/failsafe.cpp | 42 +++++++++++++++++++++ src/modules/commander/failsafe/failsafe.h | 12 +++++- src/modules/navigator/geofence_params.c | 1 + 4 files changed, 67 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index da245544ec79..94e1023e56ad 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -271,6 +271,9 @@ PARAM_DEFINE_INT32(COM_DISARM_MAN, 1); * @value 0 Warning * @value 2 Land mode * @value 3 Return at critical level, land at emergency level + * @value 4 Descend mode + * @min 0 + * @max 4 */ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0); @@ -304,6 +307,7 @@ PARAM_DEFINE_FLOAT(COM_FAIL_ACT_T, 5.f); * @value 0 Warning * @value 1 Return * @value 2 Land + * @value 3 Descend * @increment 1 */ PARAM_DEFINE_INT32(COM_IMB_PROP_ACT, 0); @@ -328,6 +332,7 @@ PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 1.0f); * @value 0 Return mode * @value 1 Land mode * @value 2 Hold mode + * @value 3 Descend mode * @group Commander */ PARAM_DEFINE_INT32(COM_QC_ACT, 0); @@ -346,6 +351,9 @@ PARAM_DEFINE_INT32(COM_QC_ACT, 0); * @value 5 Hold mode * @value 6 Terminate * @value 7 Disarm + * @value 8 Descend + * @min 0 + * @max 8 * @group Commander */ PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0); @@ -596,8 +604,9 @@ PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0); * @value 3 Land mode * @value 5 Terminate * @value 6 Disarm + * @value 7 Descend * @min 0 - * @max 6 + * @max 7 * * @group Commander */ @@ -615,8 +624,9 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0); * @value 3 Land mode * @value 5 Terminate * @value 6 Disarm + * @value 7 Descend * @min 1 - * @max 6 + * @max 7 * * @group Commander */ @@ -649,6 +659,7 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0); * @value 2 Land mode * @value 3 Return mode * @value 4 Terminate + * @value 5 Descend * @group Commander */ PARAM_DEFINE_INT32(COM_ACT_FAIL_ACT, 0); @@ -923,6 +934,7 @@ PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f); * @value 3 Return * @value 4 Terminate * @value 5 Land + * @value 6 Descend * @increment 1 */ PARAM_DEFINE_INT32(COM_WIND_MAX_ACT, 0); diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 534d62807f6a..9e6512d80254 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -63,6 +63,11 @@ FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value) options.clear_condition = ClearCondition::OnModeChangeOrDisarm; break; + case gcs_connection_loss_failsafe_mode::Descend: + options.action = Action::Descend; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + case gcs_connection_loss_failsafe_mode::Terminate: options.allow_user_takeover = UserTakeoverAllowed::Never; options.action = Action::Terminate; @@ -117,6 +122,11 @@ FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value) options.clear_condition = ClearCondition::OnModeChangeOrDisarm; break; + case geofence_violation_action::Descend_mode: + options.action = Action::Descend; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + default: options.action = Action::Warn; break; @@ -148,6 +158,11 @@ FailsafeBase::ActionOptions Failsafe::fromImbalancedPropActParam(int param_value options.action = Action::Land; options.clear_condition = ClearCondition::OnModeChangeOrDisarm; break; + + case imbalanced_propeller_failsafe_mode::Descend: + options.action = Action::Descend; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; } return options; @@ -172,6 +187,11 @@ FailsafeBase::ActionOptions Failsafe::fromActuatorFailureActParam(int param_valu options.clear_condition = ClearCondition::OnModeChangeOrDisarm; break; + case actuator_failure_failsafe_mode::Descend_mode: + options.action = Action::Descend; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + case actuator_failure_failsafe_mode::Return_mode: options.action = Action::RTL; options.clear_condition = ClearCondition::OnModeChangeOrDisarm; @@ -211,6 +231,10 @@ FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value options.action = Action::RTL; break; + case LowBatteryAction::Descend: + options.action = Action::Descend; + break; + case LowBatteryAction::Land: options.action = Action::Land; break; @@ -231,6 +255,10 @@ FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value options.action = Action::RTL; break; + case LowBatteryAction::Descend: + options.action = Action::Descend; + break; + case LowBatteryAction::ReturnOrLand: case LowBatteryAction::Land: options.action = Action::Land; @@ -267,6 +295,11 @@ FailsafeBase::ActionOptions Failsafe::fromQuadchuteActParam(int param_value) options.clear_condition = ClearCondition::OnModeChangeOrDisarm; break; + case command_after_quadchute::Descend_mode: + options.action = Action::Descend; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + case command_after_quadchute::Hold_mode: options.action = Action::Hold; options.clear_condition = ClearCondition::OnModeChangeOrDisarm; @@ -307,6 +340,10 @@ FailsafeBase::Action Failsafe::fromOffboardLossActParam(int param_value, uint8_t user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; break; + case offboard_loss_failsafe_mode::Descend: + action = Action::Descend; + break; + case offboard_loss_failsafe_mode::Hold_mode: action = Action::Hold; user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; @@ -360,6 +397,11 @@ FailsafeBase::ActionOptions Failsafe::fromHighWindLimitActParam(int param_value) options.clear_condition = ClearCondition::OnModeChangeOrDisarm; break; + case command_after_high_wind_failsafe::Descend: + options.action = Action::Descend; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + default: options.action = Action::Warn; break; diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 2ebb962cb1c4..2fb8772b59c8 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -64,7 +64,8 @@ class Failsafe : public FailsafeBase Warning = 0, // Warning Return = 1, // Return mode (deprecated) Land = 2, // Land mode - ReturnOrLand = 3 // Return mode at critically low level, Land mode at current position if reaching dangerously low levels + ReturnOrLand = 3, // Return mode at critically low level, Land mode at current position if reaching dangerously low levels + Descend = 4, // Land without using position }; enum class offboard_loss_failsafe_mode : int32_t { @@ -76,6 +77,7 @@ class Failsafe : public FailsafeBase Hold_mode = 5, Terminate = 6, Disarm = 7, + Descend = 8, }; enum class position_control_navigation_loss_response : int32_t { @@ -89,6 +91,7 @@ class Failsafe : public FailsafeBase Land_mode = 2, Return_mode = 3, Terminate = 4, + Descend_mode = 5, }; enum class imbalanced_propeller_failsafe_mode : int32_t { @@ -96,6 +99,7 @@ class Failsafe : public FailsafeBase Warning = 0, Return = 1, Land = 2, + Descend = 3, }; enum class geofence_violation_action : int32_t { @@ -105,6 +109,7 @@ class Failsafe : public FailsafeBase Return_mode = 3, Terminate = 4, Land_mode = 5, + Descend_mode = 6, }; enum class gcs_connection_loss_failsafe_mode : int32_t { @@ -114,6 +119,7 @@ class Failsafe : public FailsafeBase Land_mode = 3, Terminate = 5, Disarm = 6, + Descend = 7, }; enum class command_after_quadchute : int32_t { @@ -121,6 +127,7 @@ class Failsafe : public FailsafeBase Return_mode = 0, Land_mode = 1, Hold_mode = 2, + Descend_mode = 3, }; // COM_RC_IN_MODE parameter values @@ -138,7 +145,8 @@ class Failsafe : public FailsafeBase Hold_mode = 2, Return_mode = 3, Terminate = 4, - Land_mode = 5 + Land_mode = 5, + Descend = 6, }; enum class command_after_remaining_flight_time_low : int32_t { diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index a775405282e5..172d713b2a75 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -57,6 +57,7 @@ * @value 3 Return mode * @value 4 Terminate * @value 5 Land mode + * @value 6 Descend mode * @group Geofence */ PARAM_DEFINE_INT32(GF_ACTION, 2);