aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_TECS: Connsider throttle saturation in height demand limiting

Browse Source
This commit is contained in:
Ryan Beall 2021-11-15 11:22:15 -08:00 committed by Andrew Tridgell
parent 09f06be17e
commit beecca8da9
2 changed files with 38 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_TECS/AP_TECS.cpp
View File

@ -518,6 +518,18 @@ void AP_TECS::_update_height_demand(void)
_flare_counter = 0;
}
// Don't allow height demand to get too far ahead of the vehicles current height
// if vehicle is unable to follow the demanded climb or descent
const bool max_climb_condition = (_pitch_dem_unc > _PITCHmaxf || _thr_clip_status == ThrClipStatus::MAX) &&
!(_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND);
const bool max_descent_condition = _pitch_dem_unc < _PITCHminf || _thr_clip_status == ThrClipStatus::MIN;
if (max_climb_condition && _hgt_dem > _hgt_dem_prev) {
_hgt_dem = _hgt_dem_prev;
} else if (max_descent_condition && _hgt_dem < _hgt_dem_prev) {
_hgt_dem = _hgt_dem_prev;
}
_hgt_dem_prev = _hgt_dem;
// for landing approach we will predict ahead by the time constant
// plus the lag produced by the first order filter. This avoids a
// lagged height demand while constantly descending which causes
@ -670,8 +682,16 @@ void AP_TECS::_update_throttle_with_airspeed(void)
}
_throttle_dem = (_STE_error + STEdot_error * throttle_damp) * K_STE2Thr + ff_throttle;
// Constrain throttle demand
_throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
// Constrain throttle demand and record clipping
if (_throttle_dem > _THRmaxf) {
_thr_clip_status = ThrClipStatus::MAX;
_throttle_dem = _THRmaxf;
} else if (_throttle_dem < _THRminf) {
_thr_clip_status = ThrClipStatus::MIN;
_throttle_dem = _THRminf;
} else {
_thr_clip_status = ThrClipStatus::NONE;
}
float THRminf_clipped_to_zero = constrain_float(_THRminf, 0, _THRmaxf);
@ -721,8 +741,14 @@ void AP_TECS::_update_throttle_with_airspeed(void)
_throttle_dem = _throttle_dem + _integTHR_state;
}
// Constrain throttle demand
_throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
// Constrain throttle demand and record clip status
if (_throttle_dem > _THRmaxf) {
_thr_clip_status = ThrClipStatus::MAX;
_throttle_dem = _THRmaxf;
} else if (_throttle_dem < _THRminf) {
_thr_clip_status = ThrClipStatus::MIN;
_throttle_dem = _THRminf;
}
}
float AP_TECS::_get_i_gain(void)

8
libraries/AP_TECS/AP_TECS.h
View File

@ -324,6 +324,14 @@ private:
float _PITCHmaxf;
float _PITCHminf;
// 1 if throttle is clipping at max value, -1 if clipping at min value, 0 otherwise
enum class ThrClipStatus : int8_t {
MIN = -1,
NONE = 0,
MAX = 1,
};
ThrClipStatus _thr_clip_status;
// Specific energy quantities
float _SPE_dem;
float _SKE_dem;