Commander: add wind speed warning

Publishes periodically (max every 1 min) a warning if the current wind estimate
is above COM_WIND_WARN.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/commander/Commander.cpp

@@ -2648,6 +2648,11 @@ Commander::run()
 			}
 		}
 
+		// Publish wind speed warning if enabled via parameter
+		if (_param_com_wind_warn.get() > FLT_EPSILON && !_land_detector.landed) {
+			checkWindAndWarn();
+		}
+
 		/* Get current timestamp */
 		const hrt_abstime now = hrt_absolute_time();
 
@@ -4174,6 +4179,27 @@ void Commander::send_parachute_command()
 	set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
 }
 
+void Commander::checkWindAndWarn()
+{
+	wind_s wind_estimate;
+
+	if (_wind_sub.update(&wind_estimate)) {
+		const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east);
+
+		// 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) {
+			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"),
+			{events::Log::Warning, events::LogInternal::Info},
+			"High wind speed detected ({1:.1m/s}), landing advised", wind.norm());
+			_last_wind_warning = hrt_absolute_time();
+		}
+	}
+}
+
 int Commander::print_usage(const char *reason)
 {
 	if (reason) {

src/modules/commander/Commander.hpp

@@ -87,6 +87,7 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vtol_vehicle_status.h>
+#include <uORB/topics/wind.h>
 
 using math::constrain;
 using systemlib::Hysteresis;
@@ -182,6 +183,8 @@ private:
 
 	void send_parachute_command();
 
+	void checkWindAndWarn();
+
 	DEFINE_PARAMETERS(
 
 		(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
@@ -217,6 +220,8 @@ private:
 
 		(ParamFloat<px4::params::COM_OBC_LOSS_T>) _param_com_obc_loss_t,
 
+		(ParamFloat<px4::params::COM_WIND_WARN>) _param_com_wind_warn,
+
 		// Offboard
 		(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
 		(ParamInt<px4::params::COM_OBL_ACT>) _param_com_obl_act,
@@ -377,6 +382,8 @@ private:
 	safety_s		_safety{};
 	vtol_vehicle_status_s	_vtol_status{};
 
+	hrt_abstime _last_wind_warning{0};
+
 	// commander publications
 	actuator_armed_s        _armed{};
 	commander_state_s       _internal_state{};
@@ -401,6 +408,7 @@ private:
 	uORB::Subscription					_vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription					_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
+	uORB::Subscription					_wind_sub{ORB_ID(wind)};
 
 	uORB::SubscriptionInterval				_parameter_update_sub{ORB_ID(parameter_update), 1_s};
 

src/modules/commander/commander_params.c

@@ -1025,3 +1025,20 @@ PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1);
  * @value 2 Enforce SD card presence
  */
 PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1);
+
+/**
+ * Wind speed warning threshold
+ *
+ * A warning is triggered if the currently estimated wind speed is above this value.
+ * Warning is sent periodically (every 1min).
+ *
+ * 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_WARN, -1.f);