dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FixedwingPositionControl: Only warn user when roll is reduced for a longer period of time

This commit is contained in:
Konrad 2024-02-12 13:38:40 +01:00 committed by Silvan Fuhrer
parent b8714f8980
commit 1a1891073e
2 changed files with 28 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -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);
}

2
src/modules/fw_pos_control/FixedwingPositionControl.hpp
View File

@ -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;