FW Position Controller: do not publish roll angle constrain warning i…

…n VTOL transition (#22842) * FW Position Control: some cosmetical changes Signed-off-by: Silvan Fuhrer <[email protected]> * 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 <[email protected]> * FW Pos C: define magic numbers for roll constraining warning as constants Signed-off-by: Silvan Fuhrer <[email protected]> --------- Signed-off-by: Silvan Fuhrer <[email protected]>
PX4 · Mar 6, 2024 · c5835a4 · c5835a4
1 parent 6f9a378
commit c5835a4
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 6 deletions.
diff --git 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
@@ -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<FixedwingPositionControl>, public ModuleParams,
 	public px4::WorkItem
 {