From 4632fbd60011f44a091a9329550569b7ee493aeb Mon Sep 17 00:00:00 2001
From: Silvan Fuhrer <silvan@auterion.com>
Date: Fri, 16 Jun 2023 15:07:04 +0200
Subject: [PATCH] FWRateController: use param find for VT_DIFTHR_EN as pure FW
 build doesn't have VTOL module built

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
---
 src/modules/fw_rate_control/FixedwingRateControl.cpp | 12 +++++++++---
 src/modules/fw_rate_control/FixedwingRateControl.hpp |  6 ++++--
 2 files changed, 13 insertions(+), 5 deletions(-)

diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp
index 80810205beb1..4d1bab51bd6e 100644
--- a/src/modules/fw_rate_control/FixedwingRateControl.cpp
+++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp
@@ -48,6 +48,8 @@ FixedwingRateControl::FixedwingRateControl(bool vtol) :
 	_vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_fw) : ORB_ID(vehicle_thrust_setpoint)),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
+	_handle_param_vt_fw_difthr_en = param_find("VT_FW_DIFTHR_EN");
+
 	/* fetch initial parameter values */
 	parameters_update();
 
@@ -86,6 +88,10 @@ FixedwingRateControl::parameters_update()
 		// set FF gains to 0 as we add the FF control outside of the rate controller
 		Vector3f(0.f, 0.f, 0.f));
 
+	if (_handle_param_vt_fw_difthr_en != PARAM_INVALID) {
+		param_get(_handle_param_vt_fw_difthr_en, &_param_vt_fw_difthr_en);
+	}
+
 
 	return PX4_OK;
 }
@@ -281,9 +287,9 @@ void FixedwingRateControl::Run()
 			// Update saturation status from control allocation feedback
 			// TODO: send the unallocated value directly for better anti-windup
 			Vector3<bool> diffthr_enabled(
-				_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::ROLL_BIT),
-				_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::PITCH_BIT),
-				_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::YAW_BIT)
+				_param_vt_fw_difthr_en & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::ROLL_BIT),
+				_param_vt_fw_difthr_en & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::PITCH_BIT),
+				_param_vt_fw_difthr_en & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::YAW_BIT)
 			);
 
 			if (_vehicle_status.is_vtol_tailsitter) {
diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp
index 45e97ab25c6e..14f4a6427f29 100644
--- a/src/modules/fw_rate_control/FixedwingRateControl.hpp
+++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp
@@ -151,6 +151,9 @@ class FixedwingRateControl final : public ModuleBase<FixedwingRateControl>, publ
 		PITCH_BIT = (1 << 2),
 	};
 
+	param_t _handle_param_vt_fw_difthr_en{PARAM_INVALID};
+	int32_t _param_vt_fw_difthr_en{0};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::FW_ACRO_X_MAX>) _param_fw_acro_x_max,
 		(ParamFloat<px4::params::FW_ACRO_Y_MAX>) _param_fw_acro_y_max,
@@ -201,8 +204,7 @@ class FixedwingRateControl final : public ModuleBase<FixedwingRateControl>, publ
 		(ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll,
 		(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw,
 
-		(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man,
-		(ParamInt<px4::params::VT_FW_DIFTHR_EN>) _param_vt_fw_difthr_en
+		(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man
 	)
 
 	RateControl _rate_control; ///< class for rate control calculations