From c5835a48deb2abc7d403aaab7a657e1a5f624af8 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 6 Mar 2024 15:51:54 +0100 Subject: [PATCH] FW Position Controller: do not publish roll angle constrain warning in VTOL transition (#22842) * FW Position Control: some cosmetical changes Signed-off-by: Silvan Fuhrer * FW Position Control: disable roll constraining warning in VTOL transition In transitions it is expected that the roll is constrained, and instead of defining an aribitrary threshold let's rather disable the user warning in that case. Signed-off-by: Silvan Fuhrer * FW Pos C: define magic numbers for roll constraining warning as constants Signed-off-by: Silvan Fuhrer --------- Signed-off-by: Silvan Fuhrer --- .../fw_pos_control/FixedwingPositionControl.cpp | 14 ++++++++------ .../fw_pos_control/FixedwingPositionControl.hpp | 7 +++++++ 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 8b79e1ee4d..f850e6e22a 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -512,25 +512,27 @@ float FixedwingPositionControl::getCorrectedNpfgRollSetpoint() float new_roll_setpoint(_npfg.getRollSetpoint()); const float can_run_factor(constrain(_npfg.canRun(_local_pos, _wind_valid), 0.f, 1.f)); - // If the npfg was not running before, reset the user warning variables. hrt_abstime now{hrt_absolute_time()}; - if ((now - _time_since_last_npfg_call) > 2_s) { + // Warn the user when the scale is less than 90% for at least 2 seconds (disable in transition) + + // If the npfg was not running before, reset the user warning variables. + if ((now - _time_since_last_npfg_call) > ROLL_WARNING_TIMEOUT) { _need_report_npfg_uncertain_condition = true; _time_since_first_reduced_roll = 0U; } - // Warn the user when the scale is less than 90% for at least 2 seconds. - if ((1.f - can_run_factor) < 0.1f) { + if (_vehicle_status.in_transition_mode || can_run_factor > ROLL_WARNING_CAN_RUN_THRESHOLD) { + // NPFG reports a good condition or we are in transition, reset the user warning variables. _need_report_npfg_uncertain_condition = true; _time_since_first_reduced_roll = 0U; } else if (_need_report_npfg_uncertain_condition) { if (_time_since_first_reduced_roll == 0U) { - _time_since_first_reduced_roll = hrt_absolute_time(); + _time_since_first_reduced_roll = now; } - if ((now - _time_since_first_reduced_roll) > 2_s) { + if ((now - _time_since_first_reduced_roll) > ROLL_WARNING_TIMEOUT) { _need_report_npfg_uncertain_condition = false; events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning, "Roll command reduced due to uncertain velocity/wind estimates!"); diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 43887707c8..277ba35fbf 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -169,6 +169,13 @@ static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f; // [m/s] maximum reference altitude rate threshhold static constexpr float MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT = 0.1f; +// [s] Timeout that has to pass in roll-constraining failsafe before warning is triggered +static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s; + +// [-] Can-run threshold needed to trigger the roll-constraining failsafe warning +static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f; + + class FixedwingPositionControl final : public ModuleBase, public ModuleParams, public px4::WorkItem {