void AP_TECS::_update_height_demand(void)
{
// Apply 2 point moving average to demanded height
// This is required because height demand is only updated at 5Hz
_hgt_dem = 0.5f * (_hgt_dem + _hgt_dem_in_old);
_hgt_dem_in_old = _hgt_dem;
float max_sink_rate = _maxSinkRate;
if (_maxSinkRate_approach > 0 && is_on_land_approach(true)) {
// special sink rate for approach to accommodate steep slopes and reverse thrust.
// A special check must be done to see if we're LANDing on approach but also if
// we're in that tiny window just starting NAV_LAND but still in NORMAL mode. If
// we have a steep slope with a short approach we'll want to allow acquiring the
// glide slope right away.
max_sink_rate = _maxSinkRate_approach;
}
// Limit height rate of change
if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f))
{
_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
}
else if ((_hgt_dem - _hgt_dem_prev) < (-max_sink_rate * 0.1f))
{
_hgt_dem = _hgt_dem_prev - max_sink_rate * 0.1f;
}
_hgt_dem_prev = _hgt_dem;
// Apply first order lag to height demand
_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
// in final landing stage force height rate demand to the
// configured sink rate and adjust the demanded height to
// be kinematically consistent with the height rate.
if (_flight_stage == FLIGHT_LAND_FINAL) {
_integ7_state = 0;
if (_flare_counter == 0) {
_hgt_rate_dem = _climb_rate;
_land_hgt_dem = _hgt_dem_adj;
}
// bring it in over 1s to prevent overshoot
if (_flare_counter < 10) {
_hgt_rate_dem = _hgt_rate_dem * 0.8f - 0.2f * _land_sink;
_flare_counter++;
} else {
_hgt_rate_dem = - _land_sink;
}
_land_hgt_dem += 0.1f * _hgt_rate_dem;
_hgt_dem_adj = _land_hgt_dem;
} else {
_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
_flare_counter = 0;
}
// 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
// us to consistently be above the desired glide slope. This will
// be replaced with a better zero-lag filter in the future.
float new_hgt_dem = _hgt_dem_adj;
if (_flight_stage == FLIGHT_LAND_APPROACH || _flight_stage == FLIGHT_LAND_FINAL || _flight_stage == FLIGHT_LAND_PREFLARE) {
new_hgt_dem += (_hgt_dem_adj - _hgt_dem_adj_last)*10.0f*(timeConstant()+1);
}
_hgt_dem_adj_last = _hgt_dem_adj;
_hgt_dem_adj = new_hgt_dem;
}
void AP_TECS::_update_pitch(void)
{
// Calculate Speed/Height Control Weighting
// This is used to determine how the pitch control prioritises speed and height control
// A weighting of 1 provides equal priority (this is the normal mode of operation)
// A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available
// A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected. In this instance, if airspeed
// rises above the demanded value, the pitch angle will be increased by the TECS controller.
float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f);
if (!_ahrs.airspeed_sensor_enabled()) {
SKE_weighting = 0.0f;
} else if ( _underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
SKE_weighting = 2.0f;
} else if (_flight_stage == AP_TECS::FLIGHT_LAND_APPROACH ||
_flight_stage == AP_TECS::FLIGHT_LAND_PREFLARE ||
_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
if (_spdWeightLand < 0) {
// use sliding scale from normal weight down to zero at landing
float scaled_weight = _spdWeight * (1.0f - _path_proportion);
SKE_weighting = constrain_float(scaled_weight, 0.0f, 2.0f);
} else {
SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f);
}
}
float SPE_weighting = 2.0f - SKE_weighting;
// Calculate Specific Energy Balance demand, and error
float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
// Calculate integrator state, constraining input if pitch limits are exceeded
float integ7_input = SEB_error * _get_i_gain();
if (_pitch_dem > _PITCHmaxf)
{
integ7_input = MIN(integ7_input, _PITCHmaxf - _pitch_dem);
}
else if (_pitch_dem < _PITCHminf)
{
integ7_input = MAX(integ7_input, _PITCHminf - _pitch_dem);
}
_integ7_state = _integ7_state + integ7_input * _DT;
#if 0
if (_flight_stage == FLIGHT_LAND_FINAL && fabsf(_climb_rate) > 0.2f) {
::printf("_hgt_rate_dem=%.1f _hgt_dem_adj=%.1f climb=%.1f _flare_counter=%u _pitch_dem=%.1f SEB_dem=%.2f SEBdot_dem=%.2f SEB_error=%.2f SEBdot_error=%.2f\n",
_hgt_rate_dem, _hgt_dem_adj, _climb_rate, _flare_counter, degrees(_pitch_dem),
SEB_dem, SEBdot_dem, SEB_error, SEBdot_error);
}
#endif
// Apply max and min values for integrator state that will allow for no more than
// 5deg of saturation. This allows for some pitch variation due to gusts before the
// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
// During flare a different damping gain is used
float gainInv = (_integ5_state * timeConstant() * GRAVITY_MSS);
float temp = SEB_error + SEBdot_dem * timeConstant();
float pitch_damp = _ptchDamp;
if (_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
pitch_damp = _landDamp;
} else if (!is_zero(_land_pitch_damp) && is_on_land_approach(false)) {
pitch_damp = _land_pitch_damp;
}
temp += SEBdot_error * pitch_damp;
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
temp += _PITCHminf * gainInv;
}
_integ7_state = constrain_float(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
// Calculate pitch demand from specific energy balance signals
_pitch_dem_unc = (temp + _integ7_state) / gainInv;
// 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 / _integ5_state;
if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr)
{
_pitch_dem = _last_pitch_dem + ptchRateIncr;
}
else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr)
{
_pitch_dem = _last_pitch_dem - ptchRateIncr;
}
// re-constrain pitch demand
_pitch_dem = constrain_float(_pitch_dem, _PITCHminf, _PITCHmaxf);
_last_pitch_dem = _pitch_dem;
}