From 8be8f06923a0d8d77d912fb89b45a410c8dbbca3 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 6 May 2022 11:50:20 +0300 Subject: [PATCH] FixedWingPositionControl: throttle compensation - compensate cruise throttle for wind for vehicles without airspeed sensor - compensate cruise and maximum throttle for air density Signed-off-by: RomanBapst --- .../FixedwingPositionControl.cpp | 55 ++++++++++++++----- .../FixedwingPositionControl.hpp | 9 ++- .../fw_pos_control_l1_params.c | 25 +++++++-- 3 files changed, 68 insertions(+), 21 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 493a19d9a5..d4735cae35 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -481,6 +481,45 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const hrt_abstime &now, con return airspeed_setpoint; } +float FixedwingPositionControl::getMinimumCruiseThrottleWindCompensated(float throttle_cruise) +{ + if (_param_fw_wind_cruise_throttle_scale.get() > 0.0f) { + throttle_cruise += _wind_vel.length() * _param_fw_wind_cruise_throttle_scale.get(); + } + + return throttle_cruise; +} + +void FixedwingPositionControl::compensateCruiseAndMaxThrottleForAirDensityAndWind(float &throttle_cruise, + float &throttle_max, + float throttle_min) +{ + if (_param_fw_thr_alt_scl.get() > FLT_EPSILON) { + // compensate cruise throttle and maximum throttle for air density + vehicle_air_data_s air_data; + + if (_vehicle_air_data_sub.copy(&air_data)) { + if (PX4_ISFINITE(air_data.rho)) { + // scale throttle as a function of sqrt(rho0/rho) + const float eas2tas = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / air_data.rho); + const float scale = constrain(eas2tas * _param_fw_thr_alt_scl.get(), 1.f, 2.f); + + throttle_max = constrain(throttle_max * scale, throttle_min, 1.0f); + throttle_cruise = constrain(throttle_cruise * scale, throttle_cruise, + throttle_cruise + (throttle_max - throttle_cruise) * 0.9f); + } + } + } + + if (!_tecs.airspeed_sensor_enabled()) { + // enforce minimum cruise throttle based on wind conditions if airspeed is not controlled + const float throttle_cruise_wind_compensated = getMinimumCruiseThrottleWindCompensated(throttle_cruise); + throttle_cruise = math::min(throttle_cruise_wind_compensated, + throttle_cruise + (throttle_max - throttle_cruise) * 0.9f); + + } +} + void FixedwingPositionControl::tecs_status_publish() { @@ -2702,21 +2741,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo _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); - } - } - } + compensateCruiseAndMaxThrottleForAirDensityAndWind(throttle_cruise, throttle_max, throttle_min); _tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()), _current_altitude, alt_sp, diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index ba1b987c10..87347ef00d 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -415,8 +415,11 @@ private: float get_tecs_thrust(); float get_manual_airspeed_setpoint(); - float get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed, const Vector2f &ground_speed, - float dt); + float get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cruise_airspeed, + const Vector2f &ground_speed, float dt); + float getMinimumCruiseThrottleWindCompensated(float throttle_cruise); + void compensateCruiseAndMaxThrottleForAirDensityAndWind(float &throttle_cruise, float &throttle_max, + float throttle_min); void reset_takeoff_state(bool force = false); void reset_landing_state(); @@ -525,6 +528,8 @@ private: (ParamFloat) _takeoff_pitch_min, + (ParamFloat) _param_fw_wind_cruise_throttle_scale, + (ParamFloat) _param_nav_fw_alt_rad ) 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 76ec5ce013..6e39c5dc6b 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 @@ -244,12 +244,13 @@ PARAM_DEFINE_FLOAT(NPFG_PERIOD_SF, 1.5f); PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.6f); /** - * Scale throttle by pressure change + * Throttle air density scale * - * 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. + * This scale is applied to the air density compensation factor for TECS cruise and maximum throttle. + * E.g. cruise_throttle_compensated = cruise_throttle * eas2tas * FW_THR_ALT_SCL + * where eas2tas is the conversion factor from equivalent to true airspeed (calculated using estimated air density) * - * When flying without airspeed sensor this will help to keep a constant performance over large altitude ranges. + * This compensation is most important for vehicles without airspeed sensors in order to keep a constant performance over large density altitude ranges. * * The default value of 0 will disable scaling. * @@ -998,3 +999,19 @@ PARAM_DEFINE_INT32(FW_GPSF_LT, 30); * @group Mission */ PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f); + +/** + * Wind-based cruise throttle adaption. + * + * Scale factor from horizontal wind magnitude to cruise throttle increment. + * cruise_throttle_comp = cruise_throttle + wind_magnitude * FW_WIND_CTHR_SC. + * This parameter only has an effect if airspeed is not controlled (e.g. disabled or not valid). + * + * @unit norm + * @min 0 + * @max 0.05 + * @decimal 3 + * @increment 0.001 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_WIND_CTHR_SC, 0.0f);