review comments

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,31 @@ FixedwingPositionControl::init()
 	return true;
 }
 
+float FixedwingPositionControl::getMaximumClimbRate()
+{
+
+	float weight_factor = 1.0f;
+
+	if (_param_weight_base.get() > 0.0f && _param_weight_gross.get() > 0.0f) {
+		weight_factor = math::constrain(_param_weight_base.get() / _param_weight_gross.get(), MIN_WEIGHT_RATIO,
+						MAX_WEIGHT_RATIO);
+	}
+
+	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 * weight_factor;
+}
+
 int
 FixedwingPositionControl::parameters_update()
 {
@@ -121,7 +146,7 @@ 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_climb_rate(getMaximumClimbRate());
 	_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
 	_tecs.set_min_sink_rate(_param_fw_t_sink_min.get());
 	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
@@ -2266,6 +2291,7 @@ 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());
 		}
 
 		if (_vehicle_land_detected_sub.updated()) {

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,7 @@ private:
 	 */
 	float get_terrain_altitude_takeoff(float takeoff_alt);
 
+	float getMaximumClimbRate();
 	/**
 	 * @brief Maps the manual control setpoint (pilot sticks) to height rate commands
 	 *
@@ -938,6 +945,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',