From fab7cbda8d4e904e4693bb6c3953aab79db13c99 Mon Sep 17 00:00:00 2001
From: Silvan <silvan@auterion.com>
Date: Mon, 16 Mar 2026 16:07:29 +0100
Subject: [PATCH] fix(npfg): use NFPG_DAMPING and NPFG_PERIOD to calculate
 directional p gain

Signed-off-by: Silvan <silvan@auterion.com>
---
 src/lib/npfg/AirspeedDirectionController.hpp                | 6 ++++--
 .../FwLateralLongitudinalControl.cpp                        | 2 ++
 .../FwLateralLongitudinalControl.hpp                        | 4 +++-
 3 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/src/lib/npfg/AirspeedDirectionController.hpp b/src/lib/npfg/AirspeedDirectionController.hpp
index d0111729ca..c6295792db 100644
--- a/src/lib/npfg/AirspeedDirectionController.hpp
+++ b/src/lib/npfg/AirspeedDirectionController.hpp
@@ -59,13 +59,15 @@
 #ifndef PX4_AIRSPEEDDIRECTIONONTROLLER_HPP
 #define PX4_AIRSPEEDDIRECTIONONTROLLER_HPP
 
+#include <matrix/math.hpp>
+#include <lib/mathlib/mathlib.h>
+
 class AirspeedDirectionController
 {
 public:
-
 	AirspeedDirectionController();
 
-
+	void setPGainFromPeriodAndDamping(float damping, float period) {p_gain_ = 4.f * M_PI_F * damping / math::max(period, FLT_EPSILON);}
 	float controlHeading(const float heading_sp, const float heading, const float airspeed) const;
 
 private:
diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
index 9c5647f1a5..0b4807bad0 100644
--- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
+++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
@@ -115,6 +115,8 @@ FwLateralLongitudinalControl::parameters_update()
 
 	_tecs_alt_time_const_slew_rate.setSlewRate(TECS_ALT_TIME_CONST_SLEW_RATE);
 	_tecs_alt_time_const_slew_rate.setForcedValue(_param_fw_t_h_error_tc.get() * _param_fw_thrtc_sc.get());
+
+	_airspeed_direction_control.setPGainFromPeriodAndDamping(_param_npfg_damping.get(), _param_npfg_period.get());
 }
 
 void FwLateralLongitudinalControl::Run()
diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp
index 87b94b1bf3..6985edc722 100644
--- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp
+++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp
@@ -169,7 +169,9 @@ private:
 		(ParamFloat<px4::params::FW_LND_THRTC_SC>) _param_fw_thrtc_sc,
 		(ParamFloat<px4::params::FW_T_THR_LOW_HGT>) _param_fw_t_thr_low_hgt,
 		(ParamFloat<px4::params::FW_WIND_ARSP_SC>) _param_fw_wind_arsp_sc,
-		(ParamFloat<px4::params::FW_GND_SPD_MIN>) _param_fw_gnd_spd_min
+		(ParamFloat<px4::params::FW_GND_SPD_MIN>) _param_fw_gnd_spd_min,
+		(ParamFloat<px4::params::NPFG_DAMPING>) _param_npfg_damping,
+		(ParamFloat<px4::params::NPFG_PERIOD>) _param_npfg_period
 	)
 
 	hrt_abstime _last_time_loop_ran{};