FixedwingPositionControl: Only warn user when roll is reduced for a l…

…onger period of time

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

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -414,6 +414,8 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	// 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;