Skip to content

Commit

Permalink
AP_TECS: code murdering spree
Browse files Browse the repository at this point in the history
  • Loading branch information
@jschall
jschall committed May 24, 2024
1 parent b95b9a2 commit 50581ab
Showing 1 changed file with 16 additions and 79 deletions.
95 changes: 16 additions & 79 deletions libraries/AP_TECS/AP_TECS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -576,77 +576,25 @@ void AP_TECS::_update_height_demand(void)
}

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;
_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_dem = _hgt_dem_in_raw;
} else {
// 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) {
_hgt_at_start_of_flare = _hgt_afe;
_flare_initialised = true;
}

// Go to pure sink rate control for flare. Sink rate is already constrained.
_hgt_dem = _height;
_hgt_dem = _height;
_hgt_rate_dem = -_sink_rate_limit;
}
}
Expand Down Expand Up @@ -737,9 +685,15 @@ void AP_TECS::_update_throttle_with_airspeed(void)
*/
SPE_err_max = SPE_err_min = 0;
}

SPE_err_max = MIN(SPE_err_max, _climb_rate_limit * GRAVITY_MSS * timeConstant());
SPE_err_min = MAX(SPE_err_min, -_sink_rate_limit * GRAVITY_MSS * timeConstant());

float SPEdot_dem_max = SPE_err_max / timeConstant();
float SPEdot_dem_min = SPE_err_min / timeConstant();

// rate of change of potential energy is proportional to height error
_SPEdot_dem = (_SPE_dem - _SPE_est) / timeConstant();
_SPEdot_dem = constrain_float((_SPE_dem - _SPE_est) / timeConstant() + _hgt_rate_dem * GRAVITY_MSS, SPEdot_dem_min, SPEdot_dem_max);

// Calculate total energy error
_STE_error = constrain_float((_SPE_dem - _SPE_est), SPE_err_min, SPE_err_max) + _SKE_dem - _SKE_est;
Expand Down Expand Up @@ -1153,9 +1107,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 Down Expand Up @@ -1187,10 +1139,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 @@ -1259,19 +1209,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 @@ -1467,4 +1404,4 @@ AP_TECS *tecs()
{
return AP_TECS::get_singleton();
}
};
};

0 comments on commit 50581ab

Please sign in to comment.