diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index da50271d30..dbab150d9a 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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, diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 1ab65cc2aa..6b5bfd0bd6 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -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; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 7b5e713c6f..6300dcd804 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/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 *