From cae7e1b88bdbb38a2ce3dbfae49cacaaf1be756f Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 7 Jul 2022 11:25:15 +0300 Subject: [PATCH] improved trim throttle compensation - allow compensation based on vehicle weight (parameterized) - use density for calculating trim throttle compensation instead of pressure - removed parameter FW_THR_ALT_SCL Signed-off-by: RomanBapst --- .../FixedwingPositionControl.cpp | 74 ++++++++++++------- .../FixedwingPositionControl.hpp | 27 ++++++- .../fw_pos_control_l1_params.c | 27 +++++++ 3 files changed, 99 insertions(+), 29 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 5c099c8420..e3ce212fed 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -380,7 +380,7 @@ FixedwingPositionControl::vehicle_attitude_poll() _body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az}; _body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz}; - // update TECS load factor + // load factor due to banking const float load_factor = 1.f / cosf(euler_angles(0)); _tecs.set_load_factor(load_factor); } @@ -442,20 +442,24 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const float control_interva float airspeed_min_adjusted = _param_fw_airspd_min.get(); - /* - * Calculate accelerated stall airspeed factor from commanded bank angle and use it to increase minimum airspeed. - * - * Increase lift vector to balance additional weight in bank - * cos(bank angle) = W/L = 1/n, n is the load factor - * - * lift is proportional to airspeed^2 so the increase in stall speed is Vsacc = Vs * sqrt(n) - */ + float load_factor = 1.0f; - if (_airspeed_valid && PX4_ISFINITE(_att_sp.roll_body)) { - airspeed_min_adjusted = constrain(_param_fw_airspd_stall.get() / sqrtf(cosf(_att_sp.roll_body)), - airspeed_min_adjusted, _param_fw_airspd_max.get()); + if (PX4_ISFINITE(_att_sp.roll_body)) { + load_factor = 1.0f / sqrtf(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); + } + + // Stall speed increases with the square root of the load factor times the weight ratio + // Vs ~ sqrt(load_factor * weight_ratio) + airspeed_min_adjusted = constrain(_param_fw_airspd_stall.get() * sqrtf(load_factor * weight_ratio), + airspeed_min_adjusted, _param_fw_airspd_max.get()); + // constrain setpoint airspeed_setpoint = constrain(airspeed_setpoint, airspeed_min_adjusted, _param_fw_airspd_max.get()); @@ -2356,6 +2360,12 @@ FixedwingPositionControl::Run() vehicle_control_mode_poll(); wind_poll(); + vehicle_air_data_s air_data; + + if (_vehicle_air_data_sub.update(&air_data)) { + _air_density = PX4_ISFINITE(air_data.rho) ? air_data.rho : _air_density; + } + if (_vehicle_land_detected_sub.updated()) { vehicle_land_detected_s vehicle_land_detected; @@ -2562,6 +2572,29 @@ FixedwingPositionControl::get_nav_speed_2d(const Vector2f &ground_speed) return nav_speed_2d; } +float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min, + float throttle_max) +{ + 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)) { + // scale throttle as a function of sqrt(rho0/rho) + const float eas2tas = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / _air_density); + const float eas2tas_at_5000m_amsl = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / AIR_DENSITY_STANDARD_ATMOS_5000_AMSL); + air_density_throttle_scale = constrain(eas2tas, 1.f, eas2tas_at_5000m_amsl); + } + + // 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); +} + void FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, float pitch_min_rad, float pitch_max_rad, @@ -2636,21 +2669,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva _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); - } - } - } + const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min, + throttle_max); _tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()), _current_altitude, diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index df0c4000b0..61a63f08a8 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -136,6 +136,15 @@ static constexpr float MIN_AUTO_TIMESTEP = 0.01f; // [s] maximum time step between auto control updates static constexpr float MAX_AUTO_TIMESTEP = 0.05f; +// [.] minimum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived) +static constexpr float MIN_WEIGHT_RATIO = 0.5f; + +// [.] maximum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived) +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; + class FixedwingPositionControl final : public ModuleBase, public ModuleParams, public px4::WorkItem { @@ -275,6 +284,7 @@ private: float _airspeed{0.0f}; float _eas2tas{1.0f}; + float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; /* wind estimates */ @@ -594,6 +604,16 @@ private: */ void set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid); + /** + * @brief Compensate trim throttle for air density and vehicle weight. + * + * @param trim throttle required at sea level during standard conditions. + * @param throttle_min Minimum allowed trim throttle. + * @param throttle_max Maximum allowed trim throttle. + * @return Trim throttle compensated for air density and vehicle weight. + */ + float compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min, float throttle_max); + void publishOrbitStatus(const position_setpoint_s pos_sp); SlewRate _airspeed_slew_rate_controller; @@ -684,7 +704,6 @@ private: (ParamFloat) _param_climbrate_target, (ParamFloat) _param_sinkrate_target, - (ParamFloat) _param_fw_thr_alt_scl, (ParamFloat) _param_fw_thr_trim, (ParamFloat) _param_fw_thr_idle, (ParamFloat) _param_fw_thr_lnd_max, @@ -708,7 +727,11 @@ private: (ParamFloat) _takeoff_pitch_min, - (ParamFloat) _param_nav_fw_alt_rad + (ParamFloat) _param_nav_fw_alt_rad, + + (ParamFloat) _param_weight_base, + (ParamFloat) _param_weight_gross + ) }; 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 9645a46165..a19a0111b9 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 @@ -980,3 +980,30 @@ PARAM_DEFINE_INT32(FW_GPSF_LT, 30); * @group Mission */ PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f); + +/** + * Vehicle base weight. + * + * This is the weight of the vehicle at which it's performance limits were derived. A zero or negative value + * disables trim throttle and minimum airspeed compensation based on weight. + * + * @unit kg + * @decimal 1 + * @increment 0.5 + * @group Mission + */ +PARAM_DEFINE_FLOAT(WEIGHT_BASE, -1.0f); + +/** + * Vehicle gross weight. + * + * This is the actual weight of the vehicle at any time. This value will differ from WEIGHT_BASE in case weight was added + * or removed from the base weight. Examples are the addition of payloads or larger batteries. A zero or negative value + * disables trim throttle and minimum airspeed compensation based on weight. + * + * @unit kg + * @decimal 1 + * @increment 0.1 + * @group Mission + */ +PARAM_DEFINE_FLOAT(WEIGHT_GROSS, -1.0f);