fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

FixedWingPositionControl: throttle compensation

Browse Source 
- compensate cruise throttle for wind for vehicles without airspeed sensor
- compensate cruise and maximum throttle for air density

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2022-05-06 11:50:20 +03:00
parent 15296ab453
commit 8be8f06923
3 changed files with 68 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@ -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<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min,
(ParamFloat<px4::params::FW_WIND_CTHR_SC>) _param_fw_wind_cruise_throttle_scale,
(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad
)

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

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