dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2022-07-07 11:25:15 +03:00 committed by Martina Rivizzigno
parent 461d0561b8
commit cae7e1b88b
3 changed files with 99 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -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,

27
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -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<FixedwingPositionControl>, 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<float> _airspeed_slew_rate_controller;
@ -684,7 +704,6 @@ private:
(ParamFloat<px4::params::FW_T_CLMB_R_SP>) _param_climbrate_target,
(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
(ParamFloat<px4::params::FW_THR_ALT_SCL>) _param_fw_thr_alt_scl,
(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
(ParamFloat<px4::params::FW_THR_LND_MAX>) _param_fw_thr_lnd_max,
@ -708,7 +727,11 @@ private:
(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min,
(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad
(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad,
(ParamFloat<px4::params::WEIGHT_BASE>) _param_weight_base,
(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross
)
};

27
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -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);