From 54d26e084a3cd43b0fc25b19969e65061e454269 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 6 Nov 2023 11:58:21 +0100 Subject: [PATCH] Commander: windCheck: add COM_WIND_MAX_ACT param to set high wind failsafe action (#21373) Has options *None where the check is disabled, and *Warning, where only a warning is published (which replaces the high wind warning from the COM_WIND_WARN limit). Signed-off-by: Silvan Fuhrer --- .../checks/windCheck.cpp | 21 +++++++-- .../checks/windCheck.hpp | 4 +- src/modules/commander/commander_params.c | 31 ++++++++++--- src/modules/commander/failsafe/failsafe.cpp | 45 ++++++++++++++++++- src/modules/commander/failsafe/failsafe.h | 13 +++++- 5 files changed, 98 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/windCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/windCheck.cpp index fd77a75267..50dad032dc 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/windCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/windCheck.cpp @@ -48,22 +48,35 @@ void WindChecks::checkAndReport(const Context &context, Report &reporter) // publish a warning if it's the first since in air or 60s have passed since the last warning const bool warning_timeout_passed = _last_wind_warning == 0 || now - _last_wind_warning > 60_s; + const bool wind_limit_exceeded = _param_com_wind_max.get() > FLT_EPSILON && wind.longerThan(_param_com_wind_max.get()); + reporter.failsafeFlags().wind_limit_exceeded = false; // reset, will be set below if needed - reporter.failsafeFlags().wind_limit_exceeded = _param_com_wind_max.get() > FLT_EPSILON - && wind.longerThan(_param_com_wind_max.get()); + if (_param_com_wind_max_act.get() > 1 && wind_limit_exceeded) { - if (reporter.failsafeFlags().wind_limit_exceeded) { + // only set failsafe flag if the high wind failsafe action is higher than warning + reporter.failsafeFlags().wind_limit_exceeded = true; /* EVENT * @description * - * This check can be configured via COM_WIND_MAX parameter. + * This check can be configured via COM_WIND_MAX and COM_WIND_MAX_ACT parameters. * */ reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_wind_too_high"), events::Log::Warning, "Wind speed is above limit ({1:.1m/s})", wind.norm()); + } else if (_param_com_wind_max_act.get() == 1 // warning only + && wind_limit_exceeded + && warning_timeout_passed + && context.status().nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL + && context.status().nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { + + events::send(events::ID("check_above_wind_limits_warning"), + {events::Log::Warning, events::LogInternal::Warning}, + "Wind speed above limit ({1:.1m/s}), landing advised", wind.norm()); + _last_wind_warning = now; + } else if (_param_com_wind_warn.get() > FLT_EPSILON && wind.longerThan(_param_com_wind_warn.get()) && warning_timeout_passed diff --git a/src/modules/commander/HealthAndArmingChecks/checks/windCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/windCheck.hpp index e59c0c309f..e8c995e8eb 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/windCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/windCheck.hpp @@ -52,7 +52,7 @@ private: DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamFloat) _param_com_wind_max, - (ParamFloat) _param_com_wind_warn - + (ParamFloat) _param_com_wind_warn, + (ParamInt) _param_com_wind_max_act ) }; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 623e8994f6..da0afc5b67 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -1052,14 +1052,10 @@ PARAM_DEFINE_FLOAT(COM_WIND_WARN, -1.f); PARAM_DEFINE_INT32(COM_FLT_TIME_MAX, -1); /** - * Wind speed RTL threshold + * High wind speed failsafe threshold * - * Wind speed threshold above which an automatic return to launch is triggered. - * It is not possible to resume the mission or switch to any auto mode other than - * RTL or Land if this threshold is exceeded. Taking over in any manual - * mode is still possible. - * - * Set to -1 to disable. + * Wind speed threshold above which an automatic failsafe action is triggered. + * Failsafe action can be specified with COM_WIND_MAX_ACT. * * @min -1 * @decimal 1 @@ -1069,6 +1065,27 @@ PARAM_DEFINE_INT32(COM_FLT_TIME_MAX, -1); */ PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f); +/** + * High wind failsafe mode + * + * Action the system takes when a wind speed above the specified threshold is detected. + * See COM_WIND_MAX to set the failsafe threshold. + * If enabled, it is not possible to resume the mission or switch to any auto mode other than + * RTL or Land if this threshold is exceeded. Taking over in any manual + * mode is still possible. + * + * @group Commander + * + * @value 0 None + * @value 1 Warning + * @value 2 Hold + * @value 3 Return + * @value 4 Terminate + * @value 5 Land + * @increment 1 + */ +PARAM_DEFINE_INT32(COM_WIND_MAX_ACT, 0); + /** * EPH threshold for RTL * diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index f8d64308d4..86c6d7b4c1 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -323,6 +323,48 @@ FailsafeBase::Action Failsafe::fromOffboardLossActParam(int param_value, uint8_t return action; } +FailsafeBase::ActionOptions Failsafe::fromHighWindLimitActParam(int param_value) +{ + ActionOptions options{}; + + switch (command_after_high_wind_failsafe(param_value)) { + case command_after_high_wind_failsafe::None: + options.action = Action::None; + break; + + case command_after_high_wind_failsafe::Warning: + options.action = Action::Warn; + break; + + case command_after_high_wind_failsafe::Hold_mode: + options.allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly; // ensure the user can escape again + options.action = Action::Hold; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + + case command_after_high_wind_failsafe::Return_mode: + options.action = Action::RTL; + options.clear_condition = ClearCondition::OnModeChangeOrDisarm; + break; + + case command_after_high_wind_failsafe::Terminate: + options.allow_user_takeover = UserTakeoverAllowed::Never; + options.action = Action::Terminate; + options.clear_condition = ClearCondition::Never; + break; + + case command_after_high_wind_failsafe::Land_mode: + options.action = Action::Land; + break; + + default: + options.action = Action::Warn; + break; + } + + return options; +} + void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, const failsafe_flags_s &status_flags) { @@ -390,9 +432,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, } } - CHECK_FAILSAFE(status_flags, wind_limit_exceeded, - ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm).cannotBeDeferred()); + ActionOptions(fromHighWindLimitActParam(_param_com_wind_max_act.get()).cannotBeDeferred())); CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL).cannotBeDeferred()); // trigger RTL if low position accurancy is detected diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 753bf2b210..26981eb754 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -132,6 +132,15 @@ private: StickInputDisabled = 4 // input disabled }; + enum class command_after_high_wind_failsafe : int32_t { + None = 0, + Warning = 1, + Hold_mode = 2, + Return_mode = 3, + Terminate = 4, + Land_mode = 5 + }; + static ActionOptions fromNavDllOrRclActParam(int param_value); static ActionOptions fromGfActParam(int param_value); @@ -140,6 +149,7 @@ private: static ActionOptions fromBatteryWarningActParam(int param_value, uint8_t battery_warning); static ActionOptions fromQuadchuteActParam(int param_value); static Action fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode); + static ActionOptions fromHighWindLimitActParam(int param_value); const int _caller_id_mode_fallback{genCallerId()}; bool _last_state_mode_fallback{false}; @@ -171,7 +181,8 @@ private: (ParamInt) _param_com_actuator_failure_act, (ParamInt) _param_com_low_bat_act, (ParamInt) _param_com_obl_rc_act, - (ParamInt) _param_com_qc_act + (ParamInt) _param_com_qc_act, + (ParamInt) _param_com_wind_max_act ); };