compensate airspeed setpoint based on weight ratio

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

src/lib/parameters/px4params/srcparser.py

@@ -357,7 +357,7 @@ class SourceParser(object):
                                 'bit/s', 'B/s',
                                 'deg', 'deg*1e7', 'deg/s',
                                 'celcius', 'gauss', 'gauss/s', 'gauss^2',
-                                'hPa', 'kg', 'kg/m^2', 'kg m^2',
+                                'hPa', 'kg', 'kg/m^2', 'kg m^2', 'kg/m^3',
                                 'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', '1/s/sqrt(Hz)', 'm/s/rad',
                                 'Ohm', 'V', 'A',
                                 'us', 'ms', 's',

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -94,6 +94,41 @@ FixedwingPositionControl::init()
 	return true;
 }
 
+float FixedwingPositionControl::getWeightRatio()
+{
+
+	float weight_factor = 1.0f;
+
+	if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) {
+		weight_factor = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), MIN_WEIGHT_RATIO,
+						MAX_WEIGHT_RATIO);
+	}
+
+	return weight_factor;
+}
+
+float FixedwingPositionControl::getMaximumClimbRate()
+{
+	float climbrate_max = _param_fw_t_clmb_max.get();
+
+	const float density_min = _param_density_min.get();
+
+	if (density_min < AIR_DENSITY_STANDARD_ATMOS_1000_AMSL
+	    && density_min > AIR_DENSITY_STANDARD_ATMOS_5000_AMSL) {
+		const float density_gradient = (_param_fw_t_clmb_max.get() - CLIMBRATE_MIN) / (CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C -
+					       density_min);
+		const float delta_rho = _air_density - CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
+		climbrate_max = _param_fw_t_clmb_max.get() + density_gradient * delta_rho;
+	}
+
+	return climbrate_max / getWeightRatio();
+}
+
+float FixedwingPositionControl::getMinimumSinkRate()
+{
+	return _param_fw_t_sink_min.get() * sqrtf(getWeightRatio() * CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / _air_density);
+}
+
 int
 FixedwingPositionControl::parameters_update()
 {
@@ -121,8 +156,8 @@ FixedwingPositionControl::parameters_update()
 	_npfg.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get());
 
 	// TECS parameters
-	_tecs.set_max_climb_rate(_param_fw_t_clmb_max.get());
-	_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
+	_tecs.set_max_climb_rate(getMaximumClimbRate());
+	_tecs.set_max_sink_rate(getMinimumSinkRate());
 	_tecs.set_min_sink_rate(_param_fw_t_sink_min.get());
 	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
 	_tecs.set_equivalent_airspeed_trim(_param_fw_airspd_trim.get());
@@ -382,13 +417,15 @@ FixedwingPositionControl::vehicle_attitude_poll()
 float
 FixedwingPositionControl::get_manual_airspeed_setpoint()
 {
-	float altctrl_airspeed = _param_fw_airspd_trim.get();
+	float altctrl_airspeed = math::constrain(_param_fw_airspd_trim.get() * sqrtf(getWeightRatio()),
+				 _param_fw_airspd_min.get(),
+				 _param_fw_airspd_max.get());
 
 	if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) {
 		// neutral throttle corresponds to trim airspeed
 		return math::interpolateNXY(_manual_control_setpoint_for_airspeed,
 		{-1.f, 0.f, 1.f},
-		{_param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_airspd_max.get()});
+		{_param_fw_airspd_min.get(), altctrl_airspeed, _param_fw_airspd_max.get()});
 
 	} else if (PX4_ISFINITE(_commanded_manual_airspeed_setpoint)) {
 		altctrl_airspeed = _commanded_manual_airspeed_setpoint;
@@ -402,7 +439,8 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 		float calibrated_min_airspeed, const Vector2f &ground_speed)
 {
 	if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) {
-		calibrated_airspeed_setpoint = _param_fw_airspd_trim.get();
+		// increase trim airspeed based on weight ratio in order to keep constant alpha.
+		calibrated_airspeed_setpoint = _param_fw_airspd_trim.get() * sqrtf(getWeightRatio());
 	}
 
 	// Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained
@@ -426,19 +464,12 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 		load_factor_from_bank_angle = 1.0f / 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);
-	}
-
 	// Here we make sure that the set minimum airspeed is automatically adapted to the current load factor.
 	// The minimum airspeed is the controller limit (FW_AIRSPD_MIN, unless either in takeoff or landing) that should
 	// resemble the vehicles stall speed (CAS) with a 1g load plus some safety margin (as no controller tracks perfectly).
 	// Stall speed increases with the square root of the load factor: V_stall ~ sqrt(load_factor).
 	// The load_factor is composed of a term from the bank angle and a term from the weight ratio.
-	calibrated_min_airspeed *= sqrtf(load_factor_from_bank_angle * weight_ratio);
+	calibrated_min_airspeed *= sqrtf(load_factor_from_bank_angle * getWeightRatio());
 
 	// Aditional option to increase the min airspeed setpoint based on wind estimate for more stability in higher winds.
 	if (_airspeed_valid && _wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) {
@@ -928,7 +959,7 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
 
 	tecs_update_pitch_throttle(control_interval,
 				   _current_altitude,
-				   _param_fw_airspd_trim.get(),
+				   _param_fw_airspd_trim.get() * sqrtf(getWeightRatio()),
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
 				   _param_fw_thr_min.get(),
@@ -959,7 +990,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
 
 	tecs_update_pitch_throttle(control_interval,
 				   _current_altitude,
-				   _param_fw_airspd_trim.get(),
+				   _param_fw_airspd_trim.get() * sqrtf(getWeightRatio()),
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
 				   _param_fw_thr_min.get(),
@@ -2266,6 +2297,8 @@ FixedwingPositionControl::Run()
 
 		if (_vehicle_air_data_sub.update(&air_data)) {
 			_air_density = PX4_ISFINITE(air_data.rho) ? air_data.rho : _air_density;
+			_tecs.set_max_climb_rate(getMaximumClimbRate());
+			_tecs.set_min_sink_rate(getMinimumSinkRate());
 		}
 
 		if (_vehicle_land_detected_sub.updated()) {
@@ -2482,13 +2515,6 @@ float FixedwingPositionControl::calculateTrimThrottle(float throttle_min,
 		throttle_trim = _param_fw_thr_trim.get() + slope_above_trim * (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) {
-		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)) {
@@ -2499,7 +2525,8 @@ float FixedwingPositionControl::calculateTrimThrottle(float throttle_min,
 	}
 
 	// 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);
+	return math::constrain(throttle_trim * sqrtf(getWeightRatio()) * air_density_throttle_scale, throttle_min,
+			       throttle_max);
 }
 
 void

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -150,6 +150,12 @@ 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;
 
+// air density of standard athmosphere at 1000m above mean sea level [kg/m^3]
+static constexpr float AIR_DENSITY_STANDARD_ATMOS_1000_AMSL = 1.112f;
+
+// climbrate defining the service ceiling, used to compensate max climbrate based on air density
+static constexpr float CLIMBRATE_MIN = 0.5f; // [m/s]
+
 // [rad] minimum pitch while airspeed has not yet reached a controllable value in manual position controlled takeoff modes
 static constexpr float MIN_PITCH_DURING_MANUAL_TAKEOFF = 0.0f;
 
@@ -482,6 +488,24 @@ private:
 	 */
 	float get_terrain_altitude_takeoff(float takeoff_alt);
 
+	/**
+	 * @brief Return the maximum climb rate achievable given the estimated air density and the vehicle weight.
+	 * @return Maximum climbrate [m/s].
+	 */
+	float getMaximumClimbRate();
+
+	/**
+	 * @brief Return the minimum sink rate achievable given the estimated air density and the vehicle weight.
+	 * @return Minimum sink rate [m/s].
+	 */
+	float getMinimumSinkRate();
+
+	/**
+	 * @brief Return the ratio of actual vehicle weight to vehicle base weight.
+	 * @return Weight ratio.
+	 */
+	float getWeightRatio();
+
 	/**
 	 * @brief Maps the manual control setpoint (pilot sticks) to height rate commands
 	 *
@@ -938,6 +962,8 @@ private:
 
 		(ParamFloat<px4::params::WEIGHT_BASE>) _param_weight_base,
 		(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross,
+		(ParamFloat<px4::params::FW_DENSITY_MIN>) _param_density_min,
+
 
 		(ParamFloat<px4::params::FW_WING_SPAN>) _param_fw_wing_span,
 		(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height,

src/modules/fw_pos_control/fw_path_navigation_params.c

@@ -1093,3 +1093,20 @@ PARAM_DEFINE_FLOAT(FW_THR_ASPD_MIN, 0.f);
  * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_THR_ASPD_MAX, 0.f);
+
+
+/**
+ * Service ceiling density
+ *
+ * Air density at which the vehicle in normal configuration is able to achieve a maximum climb rate of
+ * 0.5m/s at maximum throttle (FW_THR_MAX). Used to compensate for air density in FW_T_CLMB_MAX.
+ * Will only have an effect if value is between 0.7363 (5000m) and 1.112 (1000m).
+ *
+ * @min 0.7363
+ * @max 1.225
+ * @unit kg/m^3
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_DENSITY_MIN, 1.225);

validation/module_schema.yaml

@@ -147,7 +147,7 @@ parameters:
                                 'bit/s', 'B/s',
                                 'deg', 'deg*1e7', 'deg/s',
                                 'celcius', 'gauss', 'gauss/s', 'mgauss', 'mgauss^2',
-                                'hPa', 'kg', 'kg/m^2', 'kg m^2',
+                                'hPa', 'kg', 'kg/m^2', 'kg m^2', 'kg/m^3',
                                 'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad',
                                 'Ohm', 'V', 'A',
                                 'us', 'ms', 's',