improved trim throttle compensation

- allow compensation based on vehicle weight (parameterized)
- use density for calculating trim throttle compensation instead of pressure
- removed parameter FW_THR_ALT_SCL

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -380,7 +380,7 @@ FixedwingPositionControl::vehicle_attitude_poll()
 		_body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az};
 		_body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz};
 
-		// update TECS load factor
+		// load factor due to banking
 		const float load_factor = 1.f / cosf(euler_angles(0));
 		_tecs.set_load_factor(load_factor);
 	}
@@ -442,20 +442,24 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const float control_interva
 
 	float airspeed_min_adjusted = _param_fw_airspd_min.get();
 
-	/*
-	 * Calculate accelerated stall airspeed factor from commanded bank angle and use it to increase minimum airspeed.
-	 *
-	 * Increase lift vector to balance additional weight in bank
-	 * cos(bank angle) = W/L = 1/n, n is the load factor
-	 *
-	 * lift is proportional to airspeed^2 so the increase in stall speed is Vsacc = Vs * sqrt(n)
-	 */
+	float load_factor = 1.0f;
 
-	if (_airspeed_valid && PX4_ISFINITE(_att_sp.roll_body)) {
-		airspeed_min_adjusted = constrain(_param_fw_airspd_stall.get() / sqrtf(cosf(_att_sp.roll_body)),
-						  airspeed_min_adjusted, _param_fw_airspd_max.get());
+	if (PX4_ISFINITE(_att_sp.roll_body)) {
+		load_factor = 1.0f / sqrtf(cosf(_att_sp.roll_body));
+	}
+
+	float weight_ratio = 1.0f;
+
+	if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) {
+		weight_ratio = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), MIN_WEIGHT_RATIO,
+					       MAX_WEIGHT_RATIO);
 	}
 
+	// Stall speed increases with the square root of the load factor times the weight ratio
+	// Vs ~ sqrt(load_factor * weight_ratio)
+	airspeed_min_adjusted = constrain(_param_fw_airspd_stall.get() * sqrtf(load_factor * weight_ratio),
+					  airspeed_min_adjusted, _param_fw_airspd_max.get());
+
 	// constrain setpoint
 	airspeed_setpoint = constrain(airspeed_setpoint, airspeed_min_adjusted, _param_fw_airspd_max.get());
 
@@ -2356,6 +2360,12 @@ FixedwingPositionControl::Run()
 		vehicle_control_mode_poll();
 		wind_poll();
 
+		vehicle_air_data_s air_data;
+
+		if (_vehicle_air_data_sub.update(&air_data)) {
+			_air_density = PX4_ISFINITE(air_data.rho) ? air_data.rho : _air_density;
+		}
+
 		if (_vehicle_land_detected_sub.updated()) {
 			vehicle_land_detected_s vehicle_land_detected;
 
@@ -2562,6 +2572,29 @@ FixedwingPositionControl::get_nav_speed_2d(const Vector2f &ground_speed)
 	return nav_speed_2d;
 }
 
+float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min,
+		float throttle_max)
+{
+	float weight_ratio = 1.0f;
+
+	if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) {
+		weight_ratio = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), MIN_WEIGHT_RATIO,
+					       MAX_WEIGHT_RATIO);
+	}
+
+	float air_density_throttle_scale = 1.0f;
+
+	if (PX4_ISFINITE(_air_density)) {
+		// scale throttle as a function of sqrt(rho0/rho)
+		const float eas2tas = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / _air_density);
+		const float eas2tas_at_5000m_amsl = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / AIR_DENSITY_STANDARD_ATMOS_5000_AMSL);
+		air_density_throttle_scale = constrain(eas2tas, 1.f, eas2tas_at_5000m_amsl);
+	}
+
+	// compensate trim throttle for both weight and air density
+	return math::constrain(throttle_trim * sqrtf(weight_ratio) * air_density_throttle_scale, throttle_min, throttle_max);
+}
+
 void
 FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
 		float pitch_min_rad, float pitch_max_rad,
@@ -2636,21 +2669,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	_tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), in_air_alt_control,
 					     _current_altitude, _local_pos.vz);
 
-	/* scale throttle cruise by baro pressure */
-	if (_param_fw_thr_alt_scl.get() > FLT_EPSILON) {
-		vehicle_air_data_s air_data;
-
-		if (_vehicle_air_data_sub.copy(&air_data)) {
-			if (PX4_ISFINITE(air_data.baro_pressure_pa) && PX4_ISFINITE(_param_fw_thr_alt_scl.get())) {
-				// scale throttle as a function of sqrt(p0/p) (~ EAS -> TAS at low speeds and altitudes ignoring temperature)
-				const float eas2tas = sqrtf(CONSTANTS_STD_PRESSURE_PA / air_data.baro_pressure_pa);
-				const float scale = constrain((eas2tas - 1.0f) * _param_fw_thr_alt_scl.get() + 1.f, 1.f, 2.f);
-
-				throttle_max = constrain(throttle_max * scale, throttle_min, 1.0f);
-				throttle_cruise = constrain(throttle_cruise * scale, throttle_min + 0.01f, throttle_max - 0.01f);
-			}
-		}
-	}
+	const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min,
+					 throttle_max);
 
 	_tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()),
 				    _current_altitude,

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -136,6 +136,15 @@ static constexpr float MIN_AUTO_TIMESTEP = 0.01f;
 // [s] maximum time step between auto control updates
 static constexpr float MAX_AUTO_TIMESTEP = 0.05f;
 
+// [.] minimum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived)
+static constexpr float MIN_WEIGHT_RATIO = 0.5f;
+
+// [.] maximum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived)
+static constexpr float MAX_WEIGHT_RATIO = 2.0f;
+
+// air density of standard athmosphere at 5000m above mean sea level [kg/m^3]
+static constexpr float AIR_DENSITY_STANDARD_ATMOS_5000_AMSL = 0.7363f;
+
 class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
 	public px4::WorkItem
 {
@@ -275,6 +284,7 @@ private:
 
 	float _airspeed{0.0f};
 	float _eas2tas{1.0f};
+	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};
 
 	/* wind estimates */
 
@@ -594,6 +604,16 @@ private:
 	 */
 	void set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);
 
+	/**
+	 * @brief Compensate trim throttle for air density and vehicle weight.
+	 *
+	 * @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.
+	 */
+	float compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min, float throttle_max);
+
 	void publishOrbitStatus(const position_setpoint_s pos_sp);
 
 	SlewRate<float> _airspeed_slew_rate_controller;
@@ -684,7 +704,6 @@ private:
 		(ParamFloat<px4::params::FW_T_CLMB_R_SP>) _param_climbrate_target,
 		(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
 
-		(ParamFloat<px4::params::FW_THR_ALT_SCL>) _param_fw_thr_alt_scl,
 		(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_LND_MAX>) _param_fw_thr_lnd_max,
@@ -708,7 +727,11 @@ private:
 
 		(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min,
 
-		(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad
+		(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad,
+
+		(ParamFloat<px4::params::WEIGHT_BASE>) _param_weight_base,
+		(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross
+
 	)
 
 };

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -980,3 +980,30 @@ PARAM_DEFINE_INT32(FW_GPSF_LT, 30);
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f);
+
+/**
+ * Vehicle base weight.
+ *
+ * This is the weight of the vehicle at which it's performance limits were derived. A zero or negative value
+ * disables trim throttle and minimum airspeed compensation based on weight.
+ *
+ * @unit kg
+ * @decimal 1
+ * @increment 0.5
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(WEIGHT_BASE, -1.0f);
+
+/**
+ * Vehicle gross weight.
+ *
+ * This is the actual weight of the vehicle at any time. This value will differ from WEIGHT_BASE in case weight was added
+ * or removed from the base weight. Examples are the addition of payloads or larger batteries. A zero or negative value
+ * disables trim throttle and minimum airspeed compensation based on weight.
+ *
+ * @unit kg
+ * @decimal 1
+ * @increment 0.1
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(WEIGHT_GROSS, -1.0f);