diff --git a/msg/failure_detector_status.msg b/msg/failure_detector_status.msg index 5f7dbc1b31..3466d2dc96 100644 --- 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 index 6c92ae83fd..919de6bdc3 100644 --- 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 index 98b63e3448..3e34b4dd4b 100644 --- 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(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(events::ID("commander_high_wind_warning"), diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 2d0df30fde..983cc60ad2 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -182,7 +182,7 @@ private: void send_parachute_command(); - void checkWindAndWarn(); + void checkWindSpeedThresholds(); DEFINE_PARAMETERS( @@ -274,7 +274,8 @@ private: (ParamFloat) _param_bat_low_thr, (ParamFloat) _param_bat_crit_thr, - (ParamInt) _param_com_flt_time_max + (ParamInt) _param_com_flt_time_max, + (ParamFloat) _param_com_wind_max ) enum class PrearmedMode { diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f96b650a81..2553dff4a5 100644 --- 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 index bf9d164d30..5798c6dd9f 100644 --- 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;