Commander: high wind speed handling updates

- add logic for detecting high wind speed in Commander, and handle it toghether with wind speed warning - trigger and enforce RTL if COM_WIND_MAX is breached Signed-off-by: Silvan Fuhrer <[email protected]>
PX4 · May 9, 2022 · 5d6c8c9 · 5d6c8c9
1 parent 1aad82f
commit 5d6c8c9
Show file tree

Hide file tree

Showing 6 changed files with 47 additions and 13 deletions.
diff --git a/msg/failure_detector_status.msg b/msg/failure_detector_status.msg
@@ -6,7 +6,6 @@ bool fd_pitch
 bool fd_alt
 bool fd_ext
 bool fd_arm_escs
-bool fd_high_wind
 bool fd_battery
 bool fd_imbalanced_prop
 

diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg
@@ -17,9 +17,8 @@ uint8 FAILURE_PITCH = 2	        # (1 << 1)
 uint8 FAILURE_ALT = 4 	        # (1 << 2)
 uint8 FAILURE_EXT = 8 	        # (1 << 3)
 uint8 FAILURE_ARM_ESC = 16      # (1 << 4)
-uint8 FAILURE_HIGH_WIND = 32        # (1 << 5)
-uint8 FAILURE_BATTERY = 64          # (1 << 6)
-uint8 FAILURE_IMBALANCED_PROP = 128 # (1 << 7)
+uint8 FAILURE_BATTERY = 32          # (1 << 5)
+uint8 FAILURE_IMBALANCED_PROP = 64 # (1 << 6)
 
 # HIL
 uint8 HIL_STATE_OFF = 0

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
@@ -2906,9 +2906,10 @@ Commander::run()
 			}
 		}
 
-		// Publish wind speed warning if enabled via parameter
-		if (_param_com_wind_warn.get() > FLT_EPSILON && !_vehicle_land_detected.landed) {
-			checkWindAndWarn();
+		// Check wind speed actions if either high wind warning or high wind RTL is enabled
+		if ((_param_com_wind_warn.get() > FLT_EPSILON || _param_com_wind_max.get() > FLT_EPSILON)
+		    && !_vehicle_land_detected.landed) {
+			checkWindSpeedThresholds();
 		}
 
 		_status_flags.flight_terminated = _armed.force_failsafe || _armed.lockdown || _armed.manual_lockdown;
@@ -3087,7 +3088,6 @@ Commander::run()
 			fd_status.fd_alt = _failure_detector.getStatusFlags().alt;
 			fd_status.fd_ext = _failure_detector.getStatusFlags().ext;
 			fd_status.fd_arm_escs = _failure_detector.getStatusFlags().arm_escs;
-			fd_status.fd_high_wind = _failure_detector.getStatusFlags().high_wind;
 			fd_status.fd_battery = _failure_detector.getStatusFlags().battery;
 			fd_status.fd_imbalanced_prop = _failure_detector.getStatusFlags().imbalanced_prop;
 			fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
@@ -4482,7 +4482,7 @@ void Commander::send_parachute_command()
 	set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
 }
 
-void Commander::checkWindAndWarn()
+void Commander::checkWindSpeedThresholds()
 {
 	wind_s wind_estimate;
 
@@ -4492,7 +4492,26 @@ void Commander::checkWindAndWarn()
 		// 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 || hrt_elapsed_time(&_last_wind_warning) > 60_s;
 
-		if (wind.longerThan(_param_com_wind_warn.get()) && warning_timeout_passed) {
+		if (_param_com_wind_max.get() > FLT_EPSILON
+		    && wind.longerThan(_param_com_wind_max.get())
+		    && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
+		    && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
+
+			main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, _internal_state);
+			_status_changed = true;
+			mavlink_log_critical(&_mavlink_log_pub, "Wind speeds above limit, abort operation and RTL (%.1f m/s)\t",
+					     (double)wind.norm());
+
+			events::send<float>(events::ID("commander_high_wind_rtl"),
+			{events::Log::Warning, events::LogInternal::Info},
+			"Wind speeds above limit, abort operation and RTL ({1:.1m/s})", wind.norm());
+
+		} else if (_param_com_wind_warn.get() > FLT_EPSILON
+			   && wind.longerThan(_param_com_wind_warn.get())
+			   && warning_timeout_passed
+			   && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
+			   && _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
+
 			mavlink_log_critical(&_mavlink_log_pub, "High wind speed detected (%.1f m/s), landing advised\t", (double)wind.norm());
 
 			events::send<float>(events::ID("commander_high_wind_warning"),

diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
@@ -182,7 +182,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 
 	void send_parachute_command();
 
-	void checkWindAndWarn();
+	void checkWindSpeedThresholds();
 
 	DEFINE_PARAMETERS(
 
@@ -274,7 +274,8 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 
 		(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
 		(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
-		(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max
+		(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max,
+		(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max
 	)
 
 	enum class PrearmedMode {

diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
@@ -1083,3 +1083,20 @@ PARAM_DEFINE_FLOAT(COM_WIND_WARN, -1.f);
  * @group Commander
  */
 PARAM_DEFINE_INT32(COM_FLT_TIME_MAX, -1);
+
+/**
+ * Wind speed RLT threshold
+ *
+ * Wind speed threshold above which an automatic return to launch is triggered
+ * and enforced as long as the threshold is exceeded.
+ *
+ * A negative value disables the feature.
+ *
+ * @min -1
+ * @max 30
+ * @decimal 1
+ * @increment 0.1
+ * @group Commander
+ * @unit m/s
+ */
+PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f);
diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp
@@ -67,7 +67,6 @@ union failure_detector_status_u {
 		uint16_t alt : 1;
 		uint16_t ext : 1;
 		uint16_t arm_escs : 1;
-		uint16_t high_wind : 1;
 		uint16_t battery : 1;
 		uint16_t imbalanced_prop : 1;
 	} flags;