Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: Takeoff improvements 

- Refactor and split set_pitch_max_limit method.
- New _update_pitch_limits to encapsulate all relevant functionality.
- Automatically reset if pitch and throttle are overriden.
- nullified TAKEOFF alt_dem offset on external throttle.
- Simplify use of TKOFF_THR_MIN.
- Prevent takeoff altitude overshoot by capping the altitude setpoint offset.
- Move pitch limits after vertical acceleration limitation.
This commit is contained in:
George Zogopoulos 2024-08-05 17:30:56 +02:00 committed by Andrew Tridgell
parent 5ea787a46d
commit 880ebbcdad
2 changed files with 160 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

232
libraries/AP_TECS/AP_TECS.cpp
View File

@ -1054,9 +1054,6 @@ void AP_TECS::_update_pitch(void)
_pitch_dem_unc += (_TAS_dem_adj - _pitch_ff_v0) * _pitch_ff_k;
}
// Constrain pitch demand
_pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
// Rate limit the pitch demand to comply with specified vertical
// acceleration limit
float ptchRateIncr = _DT * _vertAccLim / _TAS_state;
@ -1067,6 +1064,9 @@ void AP_TECS::_update_pitch(void)
_pitch_dem = _last_pitch_dem - ptchRateIncr;
}
// Constrain pitch demand
_pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
_last_pitch_dem = _pitch_dem;
#if HAL_LOGGING_ENABLED
@ -1110,11 +1110,13 @@ void AP_TECS::_update_pitch(void)
#endif
}
void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
void AP_TECS::_initialise_states(float hgt_afe)
{
// Initialise states and variables if DT > 0.2 second or TECS is getting overriden or in climbout.
_flags.reset = false;
// Initialise states and variables if DT > 0.2 second or in climbout
_need_reset = _need_reset || (_flag_pitch_forced && _flag_throttle_forced);
if (_DT > 0.2f || _need_reset) {
_SKE_weighting = 1.0f;
_integTHR_state = 0.0f;
@ -1156,7 +1158,16 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_pitch_measured_lpf.reset(_ahrs.get_pitch());
} else if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
_PITCHminf = CentiDegreesToRadians(ptchMinCO_cd);
if (!_flag_throttle_forced) {
// Calculate the takeoff target height offset before _hgt_dem_in_raw gets reset below.
// Prevent the offset from becoming negative.
_post_TO_hgt_offset = MAX(MIN(_climb_rate_limit * _hgt_dem_tconst, _hgt_dem_in_raw - hgt_afe), 0);
} else {
// If throttle is externally forced, this mechanism of adding energy is unnecessary.
_post_TO_hgt_offset = 0;
}
_hgt_afe = hgt_afe;
_hgt_dem_lpf = hgt_afe;
_hgt_dem_rate_ltd = hgt_afe;
@ -1166,13 +1177,13 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_hgt_dem_in_raw = hgt_afe;
_flags.underspeed = false;
_flags.badDescent = false;
_post_TO_hgt_offset = _climb_rate_limit * _hgt_dem_tconst; // Replacement prevents oscillating hgt_rate_dem.
_TAS_dem_adj = _TAS_dem;
_max_climb_scaler = 1.0f;
_max_sink_scaler = 1.0f;
_pitch_demand_lpf.reset(_ahrs.get_pitch());
_pitch_measured_lpf.reset(_ahrs.get_pitch());
if (!_flag_have_reset_after_takeoff) {
_flags.reset = true;
_flag_have_reset_after_takeoff = true;
@ -1250,77 +1261,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
// Update the throttle limits.
_update_throttle_limits();
// work out the maximum and minimum pitch
// if TECS_PITCH_{MAX,MIN} isn't set then use
// LIM_PITCH_{MAX,MIN}. Don't allow TECS_PITCH_{MAX,MIN} to be
// larger than LIM_PITCH_{MAX,MIN}
if (_pitch_max == 0) {
_PITCHmaxf = aparm.pitch_limit_max;
} else {
_PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max);
}
if (_pitch_min >= 0) {
_PITCHminf = aparm.pitch_limit_min;
} else {
_PITCHminf = MAX(_pitch_min, aparm.pitch_limit_min);
}
// apply temporary pitch limit and clear
if (_pitch_max_limit < 90) {
_PITCHmaxf = constrain_float(_PITCHmaxf, -90, _pitch_max_limit);
_PITCHminf = constrain_float(_PITCHminf, -_pitch_max_limit, _PITCHmaxf);
_pitch_max_limit = 90;
}
if (!_landing.is_on_approach()) {
// reset land pitch min when not landing
_land_pitch_min = _PITCHminf;
}
// calculate the expected pitch angle from the demanded climb rate and airspeed for use during approach and flare
if (_landing.is_flaring()) {
// smoothly move the min pitch to the required minimum at touchdown
float p; // 0 at start of flare, 1 at finish
if (!_flare_initialised) {
p = 0.0f;
} else if (_hgt_at_start_of_flare > _flare_holdoff_hgt) {
p = constrain_float((_hgt_at_start_of_flare - _hgt_afe) / _hgt_at_start_of_flare, 0.0f, 1.0f);
} else {
p = 1.0f;
}
const float pitch_limit_deg = (1.0f - p) * _pitch_min_at_flare_entry + p * 0.01f * _landing.get_pitch_cd();
// in flare use min pitch from LAND_PITCH_DEG
_PITCHminf = MAX(_PITCHminf, pitch_limit_deg);
// and use max pitch from TECS_LAND_PMAX
if (_land_pitch_max != 0) {
// note that this allows a flare pitch outside the normal TECS auto limits
_PITCHmaxf = _land_pitch_max;
}
} else if (_landing.is_on_approach()) {
_PITCHminf = MAX(_PITCHminf, aparm.pitch_limit_min);
_pitch_min_at_flare_entry = _PITCHminf;
_flare_initialised = false;
} else {
_flare_initialised = false;
}
if (_landing.is_on_approach()) {
// don't allow the lower bound of pitch to decrease, nor allow
// it to increase rapidly. This prevents oscillation of pitch
// demand while in landing approach based on rapidly changing
// time to flare estimate
if (_land_pitch_min <= -90) {
_land_pitch_min = _PITCHminf;
}
const float flare_pitch_range = 20;
const float delta_per_loop = (flare_pitch_range/_landTimeConst) * _DT;
_PITCHminf = MIN(_PITCHminf, _land_pitch_min+delta_per_loop);
_land_pitch_min = MAX(_land_pitch_min, _PITCHminf);
_PITCHminf = MAX(_land_pitch_min, _PITCHminf);
}
_update_pitch_limits(ptchMinCO_cd);
if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
if (!_flags.reached_speed_takeoff && _TAS_state >= _TASmin && _TASmin > 0) {
@ -1330,15 +1271,8 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
}
}
// convert to radians
_PITCHmaxf = radians(_PITCHmaxf);
_PITCHminf = radians(_PITCHminf);
// don't allow max pitch to go below min pitch
_PITCHmaxf = MAX(_PITCHmaxf, _PITCHminf);
// initialise selected states and variables if DT > 1 second or in climbout
_initialise_states(ptchMinCO_cd, hgt_afe);
_initialise_states(hgt_afe);
// Calculate Specific Total Energy Rate Limits
_update_STE_rate_lim();
@ -1460,10 +1394,9 @@ void AP_TECS::_update_throttle_limits() {
// Configure min throttle.
// If less min throttle is allowed during takeoff, use it.
// Use takeoff min throttle, if applicable.
bool use_takeoff_throttle = _flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING;
const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE);
use_takeoff_throttle = use_takeoff_throttle && (use_throttle_range == 1) && (aparm.takeoff_throttle_min != 0);
use_takeoff_throttle = use_takeoff_throttle && (aparm.takeoff_throttle_min != 0);
if ( use_takeoff_throttle ) {
_THRminf = MIN(_THRminf, aparm.takeoff_throttle_min * 0.01f);
}
@ -1491,13 +1424,128 @@ void AP_TECS::_update_throttle_limits() {
_THRminf = MAX(_THRminf, _THRminf_ext);
// Allow a minimum of 1% throttle range, primarily to prevent TECS numerical errors.
if (_THRmaxf < 1) {
_THRmaxf = MAX(_THRmaxf, _THRminf + 0.01f);
const float thr_eps = 0.01;
if (fabsf(_THRminf-_THRmaxf) < thr_eps) {
_flag_throttle_forced = true;
if (_THRmaxf < 1) {
_THRmaxf = MAX(_THRmaxf, _THRminf + 0.01f);
} else {
_THRminf = MIN(_THRminf, _THRmaxf - 0.01f);
}
} else {
_THRminf = MIN(_THRminf, _THRmaxf - 0.01f);
_flag_throttle_forced = false;
}
// Reset the external throttle limits.
_THRminf_ext = -1.0f;
_THRmaxf_ext = 1.0f;
}
void AP_TECS::set_pitch_min(const float pitch_min) {
// Don't change the limit if it is already covered.
if (pitch_min > _PITCHminf_ext) {
_PITCHminf_ext = pitch_min;
}
}
void AP_TECS::set_pitch_max(const float pitch_max) {
// Don't change the limit if it is already covered.
if (pitch_max < _PITCHmaxf_ext) {
_PITCHmaxf_ext = pitch_max;
}
}
void AP_TECS::_update_pitch_limits(const int32_t ptchMinCO_cd) {
// If TECS_PITCH_{MAX,MIN} isn't set then use LIM_PITCH_{MAX,MIN}.
// Don't allow TECS_PITCH_{MAX,MIN} to be larger than LIM_PITCH_{MAX,MIN}.
if (_pitch_max == 0) {
_PITCHmaxf = aparm.pitch_limit_max;
} else {
_PITCHmaxf = _pitch_max;
}
if (_pitch_min == 0) {
_PITCHminf = aparm.pitch_limit_min;
} else {
_PITCHminf = _pitch_min;
}
if (!_landing.is_on_approach()) {
// reset land pitch min when not landing
_land_pitch_min = _PITCHminf;
}
// calculate the expected pitch angle from the demanded climb rate and airspeed for use during approach and flare
if (_landing.is_flaring()) {
// smoothly move the min pitch to the required minimum at touchdown
float p; // 0 at start of flare, 1 at finish
if (!_flare_initialised) {
p = 0.0f;
} else if (_hgt_at_start_of_flare > _flare_holdoff_hgt) {
p = constrain_float((_hgt_at_start_of_flare - _hgt_afe) / _hgt_at_start_of_flare, 0.0f, 1.0f);
} else {
p = 1.0f;
}
const float pitch_limit_deg = (1.0f - p) * _pitch_min_at_flare_entry + p * 0.01f * _landing.get_pitch_cd();
// in flare use min pitch from LAND_PITCH_DEG
_PITCHminf = MAX(_PITCHminf, pitch_limit_deg);
// and use max pitch from TECS_LAND_PMAX
if (_land_pitch_max != 0) {
// note that this allows a flare pitch outside the normal TECS auto limits
_PITCHmaxf = _land_pitch_max;
}
} else if (_landing.is_on_approach()) {
_PITCHminf = MAX(_PITCHminf, aparm.pitch_limit_min);
_pitch_min_at_flare_entry = _PITCHminf;
_flare_initialised = false;
} else {
_flare_initialised = false;
}
if (_landing.is_on_approach()) {
// don't allow the lower bound of pitch to decrease, nor allow
// it to increase rapidly. This prevents oscillation of pitch
// demand while in landing approach based on rapidly changing
// time to flare estimate
if (_land_pitch_min <= -90) {
_land_pitch_min = _PITCHminf;
}
const float flare_pitch_range = 20;
const float delta_per_loop = (flare_pitch_range/_landTimeConst) * _DT;
_PITCHminf = MIN(_PITCHminf, _land_pitch_min+delta_per_loop);
_land_pitch_min = MAX(_land_pitch_min, _PITCHminf);
_PITCHminf = MAX(_land_pitch_min, _PITCHminf);
}
// Apply TAKEOFF minimum pitch
if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF
|| _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)
{
_PITCHminf = CentiDegreesToRadians(ptchMinCO_cd);
}
// Apply external limits.
_PITCHmaxf = MIN(_PITCHmaxf, _PITCHmaxf_ext);
_PITCHminf = MAX(_PITCHminf, _PITCHminf_ext);
// Reset the external pitch limits.
_PITCHminf_ext = -90.0f;
_PITCHmaxf_ext = 90.0f;
// convert to radians
_PITCHmaxf = radians(_PITCHmaxf);
_PITCHminf = radians(_PITCHminf);
// don't allow max pitch to go below min pitch
_PITCHmaxf = MAX(_PITCHmaxf, _PITCHminf);
// Test if someone is forcing pitch to a specific value.
const float pitch_eps = DEG_TO_RAD*1;
if (fabsf(_PITCHmaxf-_PITCHminf)<pitch_eps) {
_flag_pitch_forced = true;
} else {
_flag_pitch_forced = false;
}
}

28
libraries/AP_TECS/AP_TECS.h
View File

@ -128,20 +128,22 @@ public:
_flags.propulsion_failed = propulsion_failed;
}
// set pitch max limit in degrees
void set_pitch_max_limit(int8_t pitch_limit) {
_pitch_max_limit = pitch_limit;
}
// set minimum throttle override, [-1, -1] range
// it is applicable for one control cycle only
void set_throttle_min(const float thr_min);
// set minimum throttle override, [0, -1] range
// set maximum throttle override, [0, -1] range
// it is applicable for one control cycle only
void set_throttle_max(const float thr_max);
// set minimum pitch override, in degrees.
// it is applicable for one control cycle only
void set_pitch_min(const float pitch_min);
// set maximum pitch override, in degrees.
// it is applicable for one control cycle only
void set_pitch_max(const float pitch_max);
// force use of synthetic airspeed for one loop
void use_synthetic_airspeed(void) {
_use_synthetic_airspeed_once = true;
@ -371,6 +373,9 @@ private:
// Maximum and minimum throttle safety limits, set externally, typically by servos.cpp:apply_throttle_limits()
float _THRmaxf_ext = 1.0f;
float _THRminf_ext = -1.0f;
// Maximum and minimum pitch limits, set externally, typically by the takeoff logic.
float _PITCHmaxf_ext = 90.0f;
float _PITCHminf_ext = -90.0f;
// Maximum and minimum floating point pitch limits
float _PITCHmaxf;
@ -429,6 +434,10 @@ private:
// need to reset on next loop
bool _need_reset;
// Flag if someone else drives pitch externally.
bool _flag_pitch_forced;
// Flag if someone else drives throttle externally.
bool _flag_throttle_forced;
// Checks if we reset at the beginning of takeoff.
bool _flag_have_reset_after_takeoff;
@ -485,7 +494,7 @@ private:
void _update_pitch(void);
// Initialise states and variables
void _initialise_states(int32_t ptchMinCO_cd, float hgt_afe);
void _initialise_states(float hgt_afe);
// Calculate specific total energy rate limits
void _update_STE_rate_lim(void);
@ -498,4 +507,7 @@ private:
// Update the allowable throttle range.
void _update_throttle_limits();
// Update the allowable pitch range.
void _update_pitch_limits(const int32_t ptchMinCO_cd);
};