Skip to content

Commit

Permalink
AP_TECS: remove intermediate rate limited height demand and rework flare
Browse files Browse the repository at this point in the history
  • Loading branch information
@jschall
jschall committed Jun 24, 2024
1 parent 6d24840 commit 43f02f0
Show file tree
Hide file tree
Showing 2 changed files with 23 additions and 136 deletions.
151 changes: 22 additions & 129 deletions libraries/AP_TECS/AP_TECS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -282,6 +282,14 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = {
// @Increment: 0.2
// @User: Advanced
AP_GROUPINFO("HDEM_TCONST", 33, AP_TECS, _hgt_dem_tconst, 3.0f),

// @Param: LAND_VACC
// @DisplayName: Vertical acceleration when transitioning to flare
// @Description: This parameter sets the deceleration when transitioning to flare
// @Range: 1.0 5.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("LAND_VACC", 34, AP_TECS, _landVertAcc, 1.0),

AP_GROUPEND
};
Expand Down Expand Up @@ -528,104 +536,22 @@ void AP_TECS::_update_height_demand(void)
_sink_rate_limit = _maxSinkRate_approach;
}

if (_flags.is_doing_auto_land) {
float land_sink_rate_adj = _land_sink + _land_sink_rate_change*_distance_beyond_land_wp;
float land_sink_rate_limit = MAX(sqrt_controller(_hgt_afe, 0, _landVertAcc, _DT),land_sink_rate_adj);
_sink_rate_limit = MIN(land_sink_rate_limit, _sink_rate_limit);
}

if (!_landing.is_flaring()) {
// Apply 2 point moving average to demanded height
const float hgt_dem = 0.5f * (_hgt_dem_in + _hgt_dem_in_prev);
_hgt_dem_in_prev = _hgt_dem_in;

// Limit height rate of change
if ((hgt_dem - _hgt_dem_rate_ltd) > (_climb_rate_limit * _DT)) {
_hgt_dem_rate_ltd = _hgt_dem_rate_ltd + _climb_rate_limit * _DT;
_sink_fraction = 0.0f;
} else if ((hgt_dem - _hgt_dem_rate_ltd) < (-_sink_rate_limit * _DT)) {
_hgt_dem_rate_ltd = _hgt_dem_rate_ltd - _sink_rate_limit * _DT;
_sink_fraction = 1.0f;
} else {
const float numerator = hgt_dem - _hgt_dem_rate_ltd;
const float denominator = - _sink_rate_limit * _DT;
if (is_negative(numerator) && is_negative(denominator)) {
_sink_fraction = numerator / denominator;
} else {
_sink_fraction = 0.0f;
}
_hgt_dem_rate_ltd = hgt_dem;
}

// Apply a first order lag to height demand and compensate for lag when commencing height
// control after takeoff to prevent plane pushing nose to level before climbing again. Post takeoff
// compensation offset is decayed using the same time constant as the height demand filter.
// This hack compensates for the lack of a climb rate feedforward command
// from ArduPlane by differentiating the height demand.
// TODO add climb rate feedforward and eliminate this.
const float coef = MIN(_DT / (_DT + MAX(_hgt_dem_tconst, _DT)), 1.0f);
_hgt_rate_dem = (_hgt_dem_rate_ltd - _hgt_dem_lpf) / _hgt_dem_tconst;
_hgt_dem_lpf = _hgt_dem_rate_ltd * coef + (1.0f - coef) * _hgt_dem_lpf;
_post_TO_hgt_offset *= (1.0f - coef);
_hgt_dem = _hgt_dem_lpf + _post_TO_hgt_offset;

// during approach compensate for height filter lag
if (_flags.is_doing_auto_land) {
_hgt_dem += _hgt_dem_tconst * _hgt_rate_dem;
} else {
// 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
bool max_climb_condition = (_pitch_dem_unc > _PITCHmaxf) ||
(_SEBdot_dem_clip == clipStatus::MAX);
bool max_descent_condition = (_pitch_dem_unc < _PITCHminf) ||
(_SEBdot_dem_clip == clipStatus::MIN);
if (_using_airspeed_for_throttle) {
// large height errors will result in the throttle saturating
max_climb_condition |= (_thr_clip_status == clipStatus::MAX) &&
!((_flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || (_flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING));
max_descent_condition |= (_thr_clip_status == clipStatus::MIN) && !_landing.is_flaring();
}
const float hgt_dem_alpha = _DT / MAX(_DT + _hgt_dem_tconst, _DT);
if (max_climb_condition && _hgt_dem > _hgt_dem_prev) {
_max_climb_scaler *= (1.0f - hgt_dem_alpha);
} else if (max_descent_condition && _hgt_dem < _hgt_dem_prev) {
_max_sink_scaler *= (1.0f - hgt_dem_alpha);
} else {
_max_climb_scaler = _max_climb_scaler * (1.0f - hgt_dem_alpha) + hgt_dem_alpha;
_max_sink_scaler = _max_sink_scaler * (1.0f - hgt_dem_alpha) + hgt_dem_alpha;
}
}
_hgt_dem_prev = _hgt_dem;
} else {
// when flaring force height rate demand to the
// configured sink rate and adjust the demanded height to
// be kinematically consistent with the height rate.

// set all height filter states to current height to prevent large pitch transients if flare is aborted
_hgt_dem_lpf = _height;
_hgt_dem_rate_ltd = _height;
_hgt_dem_in_prev = _height;

if (!_flare_initialised) {
_flare_hgt_dem_adj = _hgt_dem;
_flare_hgt_dem_ideal = _hgt_afe;
_hgt_at_start_of_flare = _hgt_afe;
_hgt_rate_at_flare_entry = _climb_rate;
_flare_initialised = true;
}

// adjust the flare sink rate to increase/decrease as your travel further beyond the land wp
float land_sink_rate_adj = _land_sink + _land_sink_rate_change*_distance_beyond_land_wp;

// bring it in linearly with height
float p;
if (_hgt_at_start_of_flare > _flare_holdoff_hgt) {
p = constrain_float((_hgt_at_start_of_flare - _hgt_afe) / (_hgt_at_start_of_flare - _flare_holdoff_hgt), 0.0f, 1.0f);
} else {
p = 1.0f;
}
_hgt_rate_dem = _hgt_rate_at_flare_entry * (1.0f - p) - land_sink_rate_adj * p;

_flare_hgt_dem_ideal += _DT * _hgt_rate_dem; // the ideal height profile to follow
_flare_hgt_dem_adj += _DT * _hgt_rate_dem; // the demanded height profile that includes the pre-flare height tracking offset

// fade across to the ideal height profile
_hgt_dem = _flare_hgt_dem_adj * (1.0f - p) + _flare_hgt_dem_ideal * p;

// correct for offset between height above ground and height above datum used by control loops
_hgt_dem += (_hgt_afe - _height);
_hgt_rate_dem = (_hgt_dem_in_raw - _hgt_dem_lpf) / _hgt_dem_tconst;
_hgt_dem_lpf = _hgt_dem_in_raw * coef + (1.0f - coef) * _hgt_dem_lpf;
_hgt_rate_dem = constrain_float(_hgt_rate_dem, -_sink_rate_limit, _climb_rate_limit);

_hgt_dem = _hgt_dem_in_raw;
}
}

Expand Down Expand Up @@ -1141,9 +1067,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_last_throttle_dem = aparm.throttle_cruise * 0.01f;
_last_pitch_dem = _ahrs.get_pitch();
_hgt_afe = hgt_afe;
_hgt_dem_in_prev = hgt_afe;
_hgt_dem_lpf = hgt_afe;
_hgt_dem_rate_ltd = hgt_afe;
_hgt_dem_prev = hgt_afe;
_TAS_dem_adj = _TAS_dem;
_flags.reset = true;
Expand All @@ -1159,10 +1083,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_need_reset = false;

// misc variables used for alternative precision landing pitch control
_hgt_at_start_of_flare = 0.0f;
_hgt_rate_at_flare_entry = 0.0f;
_hgt_afe = 0.0f;
_pitch_min_at_flare_entry = 0.0f;

_max_climb_scaler = 1.0f;
_max_sink_scaler = 1.0f;
Expand All @@ -1177,10 +1098,8 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_PITCHminf = 0.000174533f * ptchMinCO_cd;
_hgt_afe = hgt_afe;
_hgt_dem_lpf = hgt_afe;
_hgt_dem_rate_ltd = hgt_afe;
_hgt_dem_prev = hgt_afe;
_hgt_dem = hgt_afe;
_hgt_dem_in_prev = hgt_afe;
_hgt_dem_in_raw = hgt_afe;
_TAS_dem_adj = _TAS_dem;
_flags.reset = true;
Expand Down Expand Up @@ -1249,19 +1168,6 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
_hgt_afe = hgt_afe;
_load_factor = load_factor;

// Don't allow height deamnd to continue changing in a direction that saturates vehicle manoeuvre limits
// if vehicle is unable to follow the demanded climb or descent.
const bool max_climb_condition = (_pitch_dem_unc > _PITCHmaxf || _thr_clip_status == clipStatus::MAX) &&
!(_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING);
const bool max_descent_condition = _pitch_dem_unc < _PITCHminf || _thr_clip_status == clipStatus::MIN;
if (max_climb_condition && _hgt_dem_in_raw > _hgt_dem_in_prev) {
_hgt_dem_in = _hgt_dem_in_prev;
} else if (max_descent_condition && _hgt_dem_in_raw < _hgt_dem_in_prev) {
_hgt_dem_in = _hgt_dem_in_prev;
} else {
_hgt_dem_in = _hgt_dem_in_raw;
}

if (aparm.takeoff_throttle_max != 0 &&
(_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) {
_THRmaxf = aparm.takeoff_throttle_max * 0.01f;
Expand Down Expand Up @@ -1303,16 +1209,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,

// calculate the expected pitch angle from the demanded climb rate and airspeed fo ruse 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();
const float pitch_limit_deg = _landing.get_pitch_cd();

// in flare use min pitch from LAND_PITCH_DEG
_PITCHminf = MAX(_PITCHminf, pitch_limit_deg);
Expand All @@ -1327,10 +1224,6 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
_THRminf = 0;
} 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()) {
Expand Down
8 changes: 1 addition & 7 deletions libraries/AP_TECS/AP_TECS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -196,6 +196,7 @@ class AP_TECS {
AP_Int32 _options;
AP_Float _flare_holdoff_hgt;
AP_Float _hgt_dem_tconst;
AP_Float _landVertAcc;

enum {
OPTION_GLIDER_ONLY=(1<<0),
Expand Down Expand Up @@ -277,15 +278,11 @@ class AP_TECS {
// height demands
float _hgt_dem_in_raw; // height demand input from autopilot before any modification (m)
float _hgt_dem_in; // height demand input from autopilot after unachievable climb or descent limiting (m)
float _hgt_dem_in_prev; // previous value of _hgt_dem_in (m)
float _hgt_dem_lpf; // height demand after application of low pass filtering (m)
float _flare_hgt_dem_adj; // height rate demand duirng flare adjusted for height tracking offset at flare entry (m)
float _flare_hgt_dem_ideal; // height we want to fly at during flare (m)
float _hgt_dem; // height demand sent to control loops (m)
float _hgt_dem_prev; // _hgt_dem from previous frame (m)

// height rate demands
float _hgt_dem_rate_ltd; // height demand after application of the rate limiter (m)
float _hgt_rate_dem; // height rate demand sent to control loops

// offset applied to height demand post takeoff to compensate for height demand filter lag
Expand Down Expand Up @@ -384,10 +381,7 @@ class AP_TECS {
float _SKEdot;

// variables used for precision landing pitch control
float _hgt_at_start_of_flare;
float _hgt_rate_at_flare_entry;
float _hgt_afe;
float _pitch_min_at_flare_entry;

// used to scale max climb and sink limits to match vehicle ability
float _max_climb_scaler;
Expand Down

0 comments on commit 43f02f0

Please sign in to comment.