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 <silvan@auterion.com>

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
 			 * <profile name="dev">
-			 * This check can be configured via <param>COM_WIND_MAX</param> parameter.
+			 * This check can be configured via <param>COM_WIND_MAX</param> and <param>COM_WIND_MAX_ACT</param> parameters.
 			 * </profile>
 			 */
 			reporter.armingCheckFailure<float>(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<float>(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

src/modules/commander/HealthAndArmingChecks/checks/windCheck.hpp

@@ -52,7 +52,7 @@ private:
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max,
-					(ParamFloat<px4::params::COM_WIND_WARN>) _param_com_wind_warn
-
+					(ParamFloat<px4::params::COM_WIND_WARN>) _param_com_wind_warn,
+					(ParamInt<px4::params::COM_WIND_MAX_ACT>) _param_com_wind_max_act
 				       )
 };

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
  *

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

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<px4::params::COM_ACT_FAIL_ACT>) _param_com_actuator_failure_act,
 					(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
 					(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
-					(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act
+					(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
+					(ParamInt<px4::params::COM_WIND_MAX_ACT>) _param_com_wind_max_act
 				       );
 
 };