From 1a1891073ee0acf4d0b2b3cc8ae96c9367c5277c Mon Sep 17 00:00:00 2001 From: Konrad Date: Mon, 12 Feb 2024 13:38:40 +0100 Subject: [PATCH] FixedwingPositionControl: Only warn user when roll is reduced for a longer period of time --- .../FixedwingPositionControl.cpp | 32 +++++++++++++++---- .../FixedwingPositionControl.hpp | 2 ++ 2 files changed, 28 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 5672e7f446..eaaf5bf5cc 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -510,18 +510,38 @@ float FixedwingPositionControl::getCorrectedNpfgRollSetpoint() { // Scale the npfg output to zero if npfg is not certain for correct output float new_roll_setpoint(_npfg.getRollSetpoint()); - const float can_run_factor(_npfg.canRun(_local_pos, _wind_valid)); + const float can_run_factor(constrain(_npfg.canRun(_local_pos, _wind_valid), 0.f, 1.f)); - if ((1.f - can_run_factor) < FLT_EPSILON) { + // 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) { _need_report_npfg_uncertain_condition = true; + _time_since_first_reduced_roll = 0U; } - if (((1.f - can_run_factor) > FLT_EPSILON) && _need_report_npfg_uncertain_condition) { - _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!"); + // Warn the user when the scale is less than 90% for at least 2 seconds. + if ((1.f - can_run_factor) < 0.1f) { + _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(); + } + + if ((now - _time_since_first_reduced_roll) > 2_s) { + _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!"); + } + + } else { + // Nothing to do, already reported. } + _time_since_last_npfg_call = now; + return can_run_factor * (new_roll_setpoint); } diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index e1593916bc..43887707c8 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -414,6 +414,8 @@ private: // nonlinear path following guidance - lateral-directional position control NPFG _npfg; bool _need_report_npfg_uncertain_condition{false}; ///< boolean if reporting of uncertain npfg output condition is needed + hrt_abstime _time_since_first_reduced_roll{0U}; ///< absolute time since start when entering reduced roll angle for the first time + hrt_abstime _time_since_last_npfg_call{0U}; ///< absolute time since start when the npfg reduced roll angle calculations was last performed PerformanceModel _performance_model;