FWPositionControl: trim throttle prediction improvements (#21345)

* FWPositionController: add linear airspeed-to-trim-throttle mapping

Scale trim throttle in a linear way with airspeed.
Different gradients above/below trim airspeed.

The airspeed- and airs density-corrected trim throttle is required for an accurate
throttle prediction in TECS, while the default trim throttle is still needed
as well, as the throttle delta from height rate setpoint delta is linked to it.

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/lib/tecs/TECS.cpp

@@ -570,17 +570,21 @@ float TECSControl::_calcThrottleControlOutput(const STERateLimit &limit, const C
 	// Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max
 	// Specific total energy rate = 0 at cruise throttle
 	// Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
+
+	// assume airspeed and density-independent delta_throttle to sink/climb rate mapping
+	// TODO: include air density for thrust mappings
+	const float throttle_increase_max_STE_rate = limit.STE_rate_max * (param.throttle_max - param.throttle_trim);
+	const float throttle_decrease_min_STE_rate = limit.STE_rate_min * (param.throttle_trim - param.throttle_min);
+
 	float throttle_predicted = 0.0f;
 
 	if (ste_rate.setpoint >= FLT_EPSILON) {
 		// throttle is between trim and maximum
-		throttle_predicted = param.throttle_trim + ste_rate.setpoint / limit.STE_rate_max *
-				     (param.throttle_max - param.throttle_trim);
+		throttle_predicted = param.throttle_trim_adjusted + ste_rate.setpoint / throttle_increase_max_STE_rate;
 
 	} else {
 		// throttle is between trim and minimum
-		throttle_predicted = param.throttle_trim + ste_rate.setpoint / limit.STE_rate_min *
-				     (param.throttle_min - param.throttle_trim);
+		throttle_predicted = param.throttle_trim_adjusted - ste_rate.setpoint / throttle_decrease_min_STE_rate;
 
 	}
 
@@ -667,8 +671,8 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 
 void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
 		  float eas_to_tas, float throttle_min, float throttle_setpoint_max,
-		  float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
-		  const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp)
+		  float throttle_trim, float throttle_trim_adjusted, float pitch_limit_min, float pitch_limit_max, float target_climbrate,
+		  float target_sinkrate, const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp)
 {
 
 	// Calculate the time since last update (seconds)
@@ -684,6 +688,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 	_control_param.pitch_max = pitch_limit_max;
 	_control_param.pitch_min = pitch_limit_min;
 	_control_param.throttle_trim = throttle_trim;
+	_control_param.throttle_trim_adjusted = throttle_trim_adjusted;
 	_control_param.throttle_max = throttle_setpoint_max;
 	_control_param.throttle_min = throttle_min;
 

src/lib/tecs/TECS.hpp

@@ -197,7 +197,8 @@ public:
 		float tas_min;				///< True airpeed demand lower limit [m/s].
 		float pitch_max;			///< Maximum pitch angle allowed in [rad].
 		float pitch_min;			///< Minimal pitch angle allowed in [rad].
-		float throttle_trim;			///< Normalized throttle required to fly level at given eas.
+		float throttle_trim;			///< Normalized throttle required to fly level at trim airspeed and sea level
+		float throttle_trim_adjusted;		///< Trim throttle adjusted for airspeed, load factor and air density
 		float throttle_max;			///< Normalized throttle upper limit.
 		float throttle_min;			///< Normalized throttle lower limit.
 
@@ -578,8 +579,8 @@ public:
 	 */
 	void update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
 		    float eas_to_tas, float throttle_min, float throttle_setpoint_max,
-		    float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
-		    float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
+		    float throttle_trim, float throttle_trim_adjusted, float pitch_limit_min, float pitch_limit_max, float target_climbrate,
+		    float target_sinkrate, float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
 
 	/**
 	 * @brief Initialize the control loop
@@ -695,6 +696,7 @@ private:
 		.pitch_max = 5.0f,
 		.pitch_min = -5.0f,
 		.throttle_trim = 0.0f,
+		.throttle_trim_adjusted = 0.f,
 		.throttle_max = 1.0f,
 		.throttle_min = 0.1f,
 		.altitude_error_gain = 0.2f,

src/modules/fw_path_navigation/FixedwingPositionControl.cpp

@@ -2458,9 +2458,25 @@ FixedwingPositionControl::reset_landing_state()
 	}
 }
 
-float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min,
-		float throttle_max)
+float FixedwingPositionControl::calculateTrimThrottle(float throttle_min,
+		float throttle_max, float airspeed_sp)
 {
+	float throttle_trim =
+		_param_fw_thr_trim.get(); // throttle required for level flight at trim airspeed, at sea level (standard atmosphere)
+
+	// Drag modelling (parasite drag): calculate mapping airspeed-->throttle, assuming a linear relation with different gradient
+	// above and below trim. This is tunable thorugh FW_THR_ASPD_MIN and FW_THR_ASPD_MAX.
+	if (PX4_ISFINITE(airspeed_sp) && _param_fw_thr_aspd_min.get() > FLT_EPSILON
+	    && airspeed_sp < _param_fw_airspd_trim.get()) {
+		throttle_trim = _param_fw_thr_trim.get() - (_param_fw_thr_trim.get() - _param_fw_thr_aspd_min.get()) /
+				(_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) * (_param_fw_airspd_trim.get() - airspeed_sp);
+
+	} else if (PX4_ISFINITE(airspeed_sp) && _param_fw_thr_aspd_max.get() > FLT_EPSILON
+		   && airspeed_sp > _param_fw_airspd_trim.get()) {
+		throttle_trim = _param_fw_thr_trim.get() + (_param_fw_thr_aspd_max.get() - _param_fw_thr_trim.get()) /
+				(_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) * (airspeed_sp - _param_fw_airspd_trim.get());
+	}
+
 	float weight_ratio = 1.0f;
 
 	if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) {
@@ -2550,8 +2566,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	}
 
 	/* update TECS vehicle state estimates */
-	const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min,
-					 throttle_max);
+	const float throttle_trim_adjusted = calculateTrimThrottle(throttle_min,
+					     throttle_max, airspeed_sp);
 
 	// HOTFIX: the airspeed rate estimate using acceleration in body-forward direction has shown to lead to high biases
 	// when flying tight turns. It's in this case much safer to just set the estimated airspeed rate to 0.
@@ -2565,7 +2581,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		     _eas2tas,
 		     throttle_min,
 		     throttle_max,
-		     throttle_trim_comp,
+		     _param_fw_thr_trim.get(),
+		     throttle_trim_adjusted,
 		     pitch_min_rad - radians(_param_fw_psp_off.get()),
 		     pitch_max_rad - radians(_param_fw_psp_off.get()),
 		     desired_max_climbrate,
@@ -2574,7 +2591,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		     -_local_pos.vz,
 		     hgt_rate_sp);
 
-	tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_comp);
+	tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_adjusted);
 }
 
 float

src/modules/fw_path_navigation/FixedwingPositionControl.hpp

@@ -689,14 +689,15 @@ private:
 	void set_control_mode_current(const hrt_abstime &now);
 
 	/**
-	 * @brief Compensate trim throttle for air density and vehicle weight.
+	 * @brief Estimate trim throttle for air density, vehicle weight and current airspeed
 	 *
-	 * @param trim throttle required at sea level during standard conditions.
 	 * @param throttle_min Minimum allowed trim throttle.
 	 * @param throttle_max Maximum allowed trim throttle.
-	 * @return Trim throttle compensated for air density and vehicle weight.
+	 * @param airspeed_sp Current airspeed setpoint (CAS) [m/s]
+	 * @return Estimated trim throttle
 	 */
-	float compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min, float throttle_max);
+	float calculateTrimThrottle(float throttle_min, float throttle_max,
+				    float airspeed_sp);
 
 	void publishOrbitStatus(const position_setpoint_s pos_sp);
 
@@ -906,6 +907,9 @@ private:
 		(ParamFloat<px4::params::FW_T_SPD_DEV_STD>) _param_speed_rate_standard_dev,
 		(ParamFloat<px4::params::FW_T_SPD_PRC_STD>) _param_process_noise_standard_dev,
 
+		(ParamFloat<px4::params::FW_THR_ASPD_MIN>) _param_fw_thr_aspd_min,
+		(ParamFloat<px4::params::FW_THR_ASPD_MAX>) _param_fw_thr_aspd_max,
+
 		(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
 		(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
 		(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,

src/modules/fw_path_navigation/fw_path_navigation_params.c

@@ -1063,3 +1063,33 @@ PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f);
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f);
+
+/**
+ * Throttle at min airspeed
+ *
+ * Required throttle for level flight at minimum airspeed FW_AIRSPD_MIN (sea level, standard atmosphere)
+ *
+ * Set to 0 to disable mapping of airspeed to trim throttle below FW_AIRSPD_TRIM.
+ *
+ * @min 0
+ * @max 1
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_THR_ASPD_MIN, 0.f);
+
+/**
+ * Throttle at max airspeed
+ *
+ * Required throttle for level flight at maximum airspeed FW_AIRSPD_MAX (sea level, standard atmosphere)
+ *
+ * Set to 0 to disable mapping of airspeed to trim throttle.
+ *
+ * @min 0
+ * @max 1
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_THR_ASPD_MAX, 0.f);