handle FW_THR_MAX as well

Signed-off-by: RomanBapst <[email protected]>
PX4 · Jul 9, 2024 · b3dd9c7 · b3dd9c7
1 parent 9c8df36
commit b3dd9c7
Showing 1 changed file with 9 additions and 3 deletions.
diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp
@@ -172,6 +172,8 @@ class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
 
 	param_t _param_handle_pitch_sp_offset{PARAM_INVALID};
 	float _param_pitch_sp_offset{0.0f};
+	param_t _param_handle_fw_thr_max{PARAM_INVALID};
+	float _param_fw_thr_max{0.0f};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::ASPD_WIND_NSD>) _param_aspd_wind_nsd,
@@ -198,8 +200,7 @@ class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
 
 		// external parameters
 		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
-		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
-		(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max
+		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim
 	)
 
 	void 		init(); 	/**< initialization of the airspeed validator instances */
@@ -217,6 +218,7 @@ AirspeedModule::AirspeedModule():
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
 	_param_handle_pitch_sp_offset = param_find("FW_PSP_OFF");
+	_param_handle_fw_thr_max = param_find("FW_THR_MAX");
 	// initialise parameters
 	update_params();
 
@@ -463,6 +465,10 @@ void AirspeedModule::update_params()
 		param_get(_param_handle_pitch_sp_offset, &_param_pitch_sp_offset);
 	}
 
+	if (_param_handle_fw_thr_max != PARAM_INVALID) {
+		param_get(_param_handle_fw_thr_max, &_param_fw_thr_max);
+	}
+
 	_param_airspeed_scale[0] = _param_airspeed_scale_1.get();
 	_param_airspeed_scale[1] = _param_airspeed_scale_2.get();
 	_param_airspeed_scale[2] = _param_airspeed_scale_3.get();
@@ -500,7 +506,7 @@ void AirspeedModule::update_params()
 		_airspeed_validator[i].set_enable_first_principle_check(_param_airspeed_checks_on.get() &
 				CheckTypeBits::CHECK_TYPE_FIRST_PRINCIPLE_BIT);
 		_airspeed_validator[i].set_psp_off_param(math::radians(_param_pitch_sp_offset));
-		_airspeed_validator[i].set_throttle_max_param(_param_fw_thr_max.get());
+		_airspeed_validator[i].set_throttle_max_param(_param_fw_thr_max);
 		_airspeed_validator[i].set_fp_t_window(_aspd_fp_t_window.get());
 	}
 }