diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 12c1ed628d..ec07706687 100644 --- a/src/modules/commander/Commander.cpp +++ b/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(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) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 490d5a231a..34724b5100 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -87,6 +87,7 @@ #include #include #include +#include using math::constrain; using systemlib::Hysteresis; @@ -182,6 +183,8 @@ private: void send_parachute_command(); + void checkWindAndWarn(); + DEFINE_PARAMETERS( (ParamInt) _param_nav_dll_act, @@ -217,6 +220,8 @@ private: (ParamFloat) _param_com_obc_loss_t, + (ParamFloat) _param_com_wind_warn, + // Offboard (ParamFloat) _param_com_of_loss_t, (ParamInt) _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}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 6eb6d72c80..f2010d3d81 100644 --- a/src/modules/commander/commander_params.c +++ b/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);