From 55d483457b1e09736442dbb8c83e2ec7aa008bc0 Mon Sep 17 00:00:00 2001
From: Konrad <konrad@auterion.com>
Date: Mon, 27 Nov 2023 09:25:51 +0100
Subject: [PATCH] FixedwingPositionControl: regulate npfg output to zero when
 it is not certain

---
 src/modules/fw_pos_control/FixedwingPositionControl.cpp | 9 ++-------
 src/modules/fw_pos_control/FixedwingPositionControl.hpp | 2 --
 2 files changed, 2 insertions(+), 9 deletions(-)

diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp
index 577c0d5072a0..a1f414d2b925 100644
--- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp
@@ -316,7 +316,6 @@ FixedwingPositionControl::vehicle_attitude_poll()
 		}
 
 		const Eulerf euler_angles(R);
-		_roll = euler_angles(0);
 		_pitch = euler_angles(1);
 		_yaw = euler_angles(2);
 
@@ -508,10 +507,10 @@ FixedwingPositionControl::landing_status_publish()
 
 float FixedwingPositionControl::getCorrectedNpfgRollSetpoint()
 {
-	// Scale the npfg output if npfg is not certaiin for correct output
+	// 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));
-	return _last_roll_setpoint + can_run_factor * (new_roll_setpoint - _last_roll_setpoint);
+	return can_run_factor * (new_roll_setpoint);
 }
 
 void
@@ -2457,10 +2456,6 @@ FixedwingPositionControl::Run()
 				}
 			}
 
-			_last_roll_setpoint = _att_sp.roll_body;
-
-		} else {
-			_last_roll_setpoint = _roll;
 		}
 
 		// if there's any change in landing gear setpoint publish it
diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp
index 0a18decc3b7a..bbf4caba12bb 100644
--- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp
+++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp
@@ -256,7 +256,6 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	double _current_longitude{0};
 	float _current_altitude{0.f};
 
-	float _roll{0.f};
 	float _pitch{0.0f};
 	float _yaw{0.0f};
 	float _yawrate{0.0f};
@@ -400,7 +399,6 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
 	param_t _param_handle_airspeed_trans{PARAM_INVALID};
 	float _param_airspeed_trans{NAN}; // [m/s]
-	float _last_roll_setpoint{0.f}; // Roll setpoint set in the last iteration
 
 	// ESTIMATOR RESET COUNTERS