Scale FW cruise throttle based on baro presure

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -39,8 +39,8 @@ FixedwingPositionControl *l1_control::g_control;
 static int _control_task = -1;			///< task handle for sensor task */
 
 FixedwingPositionControl::FixedwingPositionControl() :
-	_sub_airspeed(ORB_ID(airspeed), 0, 0, nullptr),
-	_sub_sensors(ORB_ID(sensor_bias), 0, 0, nullptr),
+	_sub_airspeed(ORB_ID(airspeed)),
+	_sub_sensors(ORB_ID(sensor_bias)),
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control"))
 {
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
@@ -59,6 +59,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.throttle_idle = param_find("FW_THR_IDLE");
 	_parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX");
 	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
+	_parameter_handles.throttle_alt_scale = param_find("FW_THR_ALT_SCL");
 	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 	_parameter_handles.man_roll_max_deg = param_find("FW_MAN_R_MAX");
 	_parameter_handles.man_pitch_max_deg = param_find("FW_MAN_P_MAX");
@@ -141,8 +142,8 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
 	param_get(_parameter_handles.throttle_idle, &(_parameters.throttle_idle));
 	param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
+	param_get(_parameter_handles.throttle_alt_scale, &(_parameters.throttle_alt_scale));
 	param_get(_parameter_handles.throttle_slew_max, &(_parameters.throttle_slew_max));
-
 	param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
 
 	param_get(_parameter_handles.man_roll_max_deg, &_parameters.man_roll_max_rad);
@@ -1501,16 +1502,23 @@ FixedwingPositionControl::task_main()
 	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_sensor_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
 
 	/* rate limit control mode updates to 5Hz */
 	orb_set_interval(_control_mode_sub, 200);
+
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vehicle_status_sub, 200);
+
 	/* rate limit vehicle land detected updates to 5Hz */
 	orb_set_interval(_vehicle_land_detected_sub, 200);
+
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
 
+	/* rate limit barometer updates to 1 Hz */
+	orb_set_interval(_sensor_baro_sub, 1000);
+
 	/* abort on a nonzero return value from the parameter init */
 	if (parameters_update() != PX4_OK) {
 		/* parameter setup went wrong, abort */
@@ -1823,6 +1831,26 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 					     accel_body, (_global_pos.timestamp > 0), in_air_alt_control,
 					     _global_pos.alt, _local_pos.v_z_valid, _local_pos.vz, _local_pos.az);
 
+	/* scale throttle cruise by baro pressure */
+	if (_parameters.throttle_alt_scale > FLT_EPSILON) {
+
+		bool baro_updated = false;
+		orb_check(_sensor_baro_sub, &baro_updated);
+
+		sensor_baro_s baro;
+
+		if (orb_copy(ORB_ID(sensor_baro), _sensor_baro_sub, &baro) == PX4_OK) {
+			if (PX4_ISFINITE(baro.pressure) && PX4_ISFINITE(_parameters.throttle_alt_scale)) {
+				// scale throttle as a function of sqrt(p0/p) (~ EAS -> TAS at low speeds and altitudes ignoring temperature)
+				const float eas2tas = sqrtf(MSL_PRESSURE_MILLIBAR / baro.pressure);
+				const float scale = constrain(eas2tas * _parameters.throttle_alt_scale, 0.9f, 2.0f);
+
+				throttle_max = constrain(throttle_max * scale, throttle_min, 1.0f);
+				throttle_cruise = constrain(throttle_cruise * scale, throttle_min + 0.01f, throttle_max - 0.01f);
+			}
+		}
+	}
+
 	_tecs.update_pitch_throttle(_R_nb, pitch_for_tecs,
 				    _global_pos.alt, alt_sp,
 				    airspeed_sp, _airspeed, _eas2tas,

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -72,6 +72,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/sensor_bias.h>
+#include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/tecs_status.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -101,6 +102,8 @@ static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH =
 	0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode
 static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f;
 
+static constexpr float MSL_PRESSURE_MILLIBAR = 1013.25f; ///< standard atmospheric pressure in millibar
+
 using math::constrain;
 using math::max;
 using math::min;
@@ -155,6 +158,7 @@ private:
 	int		_vehicle_land_detected_sub{-1};		///< vehicle land detected subscription */
 	int		_params_sub{-1};			///< notification of parameter updates */
 	int		_manual_control_sub{-1};		///< notification of manual control updates */
+	int		_sensor_baro_sub{-1};
 
 	orb_advert_t	_attitude_sp_pub{nullptr};		///< attitude setpoint */
 	orb_advert_t	_tecs_status_pub{nullptr};		///< TECS status publication */
@@ -293,6 +297,7 @@ private:
 		float throttle_idle;
 		float throttle_cruise;
 		float throttle_slew_max;
+		float throttle_alt_scale;
 
 		float man_roll_max_rad;
 		float man_pitch_max_rad;
@@ -351,6 +356,7 @@ private:
 		param_t throttle_idle;
 		param_t throttle_cruise;
 		param_t throttle_slew_max;
+		param_t throttle_alt_scale;
 
 		param_t man_roll_max_deg;
 		param_t man_pitch_max_deg;

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -87,6 +87,24 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
  */
 PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.6f);
 
+/**
+ * Scale throttle by pressure change
+ *
+ * Automatically adjust throttle to account for decreased air density at higher altitudes.
+ * Start with a scale factor of 1.0 and adjust for different propulsion systems.
+ *
+ * When flying without airspeed sensor this will help to keep a constant performance over large altitude ranges.
+ *
+ * The default value of 0 will disable scaling.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_THR_ALT_SCL, 0.0f);
+
 /**
  * Throttle max slew rate
  *