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
);
};