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;
// 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) < (-_maxSinkRate * 0.1f))
{
_hgt_dem = _hgt_dem_prev - _maxSinkRate * 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) {
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) {
SKE_weighting = 2.0f;
} else if (_flight_stage == AP_TECS::FLIGHT_LAND_APPROACH || _flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
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 * _integGain;
if (_pitch_dem_unc > _PITCHmaxf)
{
integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc);
}
else if (_pitch_dem_unc < _PITCHminf)
{
integ7_input = max(integ7_input, _PITCHminf - _pitch_dem_unc);
}
_integ7_state = _integ7_state + integ7_input * _DT;
// 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.
float gainInv = (_integ5_state * timeConstant() * GRAVITY_MSS);
float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * timeConstant();
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF)
{
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);
_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;
}
_last_pitch_dem = _pitch_dem;
}
void AP_TECS::_update_throttle(void)
{
// Calculate limits to be applied to potential energy error to prevent over or underspeed occurring due to large height errors
float SPE_err_max = 0.5f * _TASmax * _TASmax - _SKE_dem;
float SPE_err_min = 0.5f * _TASmin * _TASmin - _SKE_dem;
// Calculate total energy error
_STE_error = constrain_float((_SPE_dem - _SPE_est), SPE_err_min, SPE_err_max) + _SKE_dem - _SKE_est;
float STEdot_dem = constrain_float((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
// Apply 0.5 second first order filter to STEdot_error
// This is required to remove accelerometer noise from the measurement
STEdot_error = 0.2f*STEdot_error + 0.8f*_STEdotErrLast;
_STEdotErrLast = STEdot_error;
// Calculate throttle demand
// If underspeed condition is set, then demand full throttle
if (_underspeed)
{
_throttle_dem_unc = 1.0f;
}
else
{
// Calculate gain scaler from specific energy error to throttle
float K_STE2Thr = 1 / (timeConstant() * (_STEdot_max - _STEdot_min));
// Calculate feed-forward throttle
float ff_throttle = 0;
float nomThr = aparm.throttle_cruise * 0.01f;
const Matrix3f &rotMat = _ahrs.get_dcm_matrix();
// Use the demanded rate of change of total energy as the feed-forward demand, but add
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns.
float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y));
STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
ff_throttle = nomThr + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
// Calculate PD + FF throttle
_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
if (aparm.throttle_slewrate != 0) {
float thrRateIncr = _DT * (_THRmaxf - _THRminf) * aparm.throttle_slewrate * 0.01f;
_throttle_dem = constrain_float(_throttle_dem,
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
_last_throttle_dem = _throttle_dem;
}
// Calculate integrator state upper and lower limits
// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
float integ_max = (_THRmaxf - _throttle_dem + 0.1f);
float integ_min = (_THRminf - _throttle_dem - 0.1f);
// Calculate integrator state, constraining state
// Set integrator to a max throttle value during climbout
_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF)
{
_integ6_state = integ_max;
}
else
{
_integ6_state = constrain_float(_integ6_state, integ_min, integ_max);
}
// Sum the components.
// Only use feed-forward component if airspeed is not being used
if (_ahrs.airspeed_sensor_enabled()) {
_throttle_dem = _throttle_dem + _integ6_state;
} else {
_throttle_dem = ff_throttle;
}
}
// Constrain throttle demand
_throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
}
void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
int32_t EAS_dem_cm,
enum FlightStage flight_stage,
bool is_doing_auto_land,
float distance_beyond_land_wp,
int32_t ptchMinCO_cd,
int16_t throttle_nudge,
float hgt_afe,
float load_factor)
{
// Calculate time in seconds since last update
uint64_t now = AP_HAL::micros64();
_DT = (now - _update_pitch_throttle_last_usec) * 1.0e-6f;
_update_pitch_throttle_last_usec = now;
_flags.is_doing_auto_land = is_doing_auto_land;
_distance_beyond_land_wp = distance_beyond_land_wp;
_flight_stage = flight_stage;
// Convert inputs
_hgt_dem = hgt_dem_cm * 0.01f;
_EAS_dem = EAS_dem_cm * 0.01f;
// Update the speed estimate using a 2nd order complementary filter
_update_speed(load_factor);
if (aparm.takeoff_throttle_max != 0 &&
(_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)) {
_THRmaxf = aparm.takeoff_throttle_max * 0.01f;
} else {
_THRmaxf = aparm.throttle_max * 0.01f;
}
_THRminf = aparm.throttle_min * 0.01f;
// 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_cd * 0.01f;
} else {
_PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max_cd * 0.01f);
}
if (_pitch_min >= 0) {
_PITCHminf = aparm.pitch_limit_min_cd * 0.01f;
} else {
_PITCHminf = MAX(_pitch_min, aparm.pitch_limit_min_cd * 0.01f);
}
// 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 (flight_stage == FLIGHT_LAND_FINAL) {
// in flare use min pitch from LAND_PITCH_CD
_PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f);
// and use max pitch from TECS_LAND_PMAX
if (_land_pitch_max != 0) {
_PITCHmaxf = MIN(_PITCHmaxf, _land_pitch_max);
}
// and allow zero throttle
_THRminf = 0;
} else if ((flight_stage == FLIGHT_LAND_APPROACH || flight_stage == FLIGHT_LAND_PREFLARE) && (-_climb_rate) > _land_sink) {
// constrain the pitch in landing as we get close to the flare
// point. Use a simple linear limit from 15 meters after the
// landing point
float time_to_flare = (- hgt_afe / _climb_rate) - aparm.land_flare_sec;
if (time_to_flare < 0) {
// we should be flaring already
_PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f);
} else if (time_to_flare < timeConstant()*2) {
// smoothly move the min pitch to the flare min pitch over
// twice the time constant
float p = time_to_flare/(2*timeConstant());
float pitch_limit_cd = p*aparm.pitch_limit_min_cd + (1-p)*aparm.land_pitch_cd;
#if 0
::printf("ttf=%.1f hgt_afe=%.1f _PITCHminf=%.1f pitch_limit=%.1f climb=%.1f\n",
time_to_flare, hgt_afe, _PITCHminf, pitch_limit_cd*0.01f, _climb_rate);
#endif
_PITCHminf = MAX(_PITCHminf, pitch_limit_cd*0.01f);
}
}
if (flight_stage == FLIGHT_TAKEOFF || flight_stage == FLIGHT_LAND_ABORT) {
if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) {
// we have reached our target speed in takeoff, allow for
// normal throttle control
_flags.reached_speed_takeoff = true;
}
}
// convert to radians
_PITCHmaxf = radians(_PITCHmaxf);
_PITCHminf = radians(_PITCHminf);
// initialise selected states and variables if DT > 1 second or in climbout
_initialise_states(ptchMinCO_cd, hgt_afe);
// Calculate Specific Total Energy Rate Limits
_update_STE_rate_lim();
// Calculate the speed demand
_update_speed_demand();
// Calculate the height demand
_update_height_demand();
// Detect underspeed condition
_detect_underspeed();
// Calculate specific energy quantitiues
_update_energies();
// Calculate throttle demand - use simple pitch to throttle if no
// airspeed sensor.
// Note that caller can demand the use of
// synthetic airspeed for one loop if needed. This is required
// during QuadPlane transition when pitch is constrained
if (_ahrs.airspeed_sensor_enabled() || _use_synthetic_airspeed || _use_synthetic_airspeed_once) {
_update_throttle_with_airspeed();
_use_synthetic_airspeed_once = false;
} else {
_update_throttle_without_airspeed(throttle_nudge);
}
// Detect bad descent due to demanded airspeed being too high
_detect_bad_descent();
// Calculate pitch demand
_update_pitch();
// log to DataFlash
DataFlash_Class::instance()->Log_Write("TECS", "TimeUS,h,dh,hdem,dhdem,spdem,sp,dsp,ith,iph,th,ph,dspdem,w,f", "QfffffffffffffB",
now,
(double)_height,
(double)_climb_rate,
(double)_hgt_dem_adj,
(double)_hgt_rate_dem,
(double)_TAS_dem_adj,
(double)_TAS_state,
(double)_vel_dot,
(double)_integTHR_state,
(double)_integSEB_state,
(double)_throttle_dem,
(double)_pitch_dem,
(double)_TAS_rate_dem,
(double)logging.SKE_weighting,
_flags_byte);
DataFlash_Class::instance()->Log_Write("TEC2", "TimeUS,KErr,PErr,EDelta,LF", "Qffff",
now,
(double)logging.SKE_error,
(double)logging.SPE_error,
(double)logging.SEB_delta,
(double)load_factor);
}
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 ( _flags.underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
SKE_weighting = 2.0f;
} else if (_flags.is_doing_auto_land) {
if (_spdWeightLand < 0) {
// use sliding scale from normal weight down to zero at landing
float scaled_weight = _spdWeight * (1.0f - constrain_float(_path_proportion,0,1));
SKE_weighting = constrain_float(scaled_weight, 0.0f, 2.0f);
} else {
SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f);
}
}
logging.SKE_weighting = SKE_weighting;
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);
logging.SKE_error = _SKE_dem - _SKE_est;
logging.SPE_error = _SPE_dem - _SPE_est;
// Calculate integrator state, constraining input if pitch limits are exceeded
float integSEB_input = SEB_error * _get_i_gain();
if (_pitch_dem > _PITCHmaxf)
{
integSEB_input = MIN(integSEB_input, _PITCHmaxf - _pitch_dem);
}
else if (_pitch_dem < _PITCHminf)
{
integSEB_input = MAX(integSEB_input, _PITCHminf - _pitch_dem);
}
float integSEB_delta = integSEB_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 = (_TAS_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) && _flags.is_doing_auto_land) {
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;
}
float integSEB_min = (gainInv * (_PITCHminf - 0.0783f)) - temp;
float integSEB_max = (gainInv * (_PITCHmaxf + 0.0783f)) - temp;
float integSEB_range = integSEB_max - integSEB_min;
logging.SEB_delta = integSEB_delta;
// don't allow the integrator to rise by more than 20% of its full
// range in one step. This prevents single value glitches from
// causing massive integrator changes. See Issue#4066
integSEB_delta = constrain_float(integSEB_delta, -integSEB_range*0.1f, integSEB_range*0.1f);
// integrate
_integSEB_state = constrain_float(_integSEB_state + integSEB_delta, integSEB_min, integSEB_max);
// Calculate pitch demand from specific energy balance signals
_pitch_dem_unc = (temp + _integSEB_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 / _TAS_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;
}
/*
calculate throttle demand - airspeed enabled case
*/
void AP_TECS::_update_throttle_with_airspeed(void)
{
// Calculate limits to be applied to potential energy error to prevent over or underspeed occurring due to large height errors
float SPE_err_max = 0.5f * _TASmax * _TASmax - _SKE_dem;
float SPE_err_min = 0.5f * _TASmin * _TASmin - _SKE_dem;
// Calculate total energy error
_STE_error = constrain_float((_SPE_dem - _SPE_est), SPE_err_min, SPE_err_max) + _SKE_dem - _SKE_est;
float STEdot_dem = constrain_float((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
// Apply 0.5 second first order filter to STEdot_error
// This is required to remove accelerometer noise from the measurement
STEdot_error = 0.2f*STEdot_error + 0.8f*_STEdotErrLast;
_STEdotErrLast = STEdot_error;
// Calculate throttle demand
// If underspeed condition is set, then demand full throttle
if (_flags.underspeed)
{
_throttle_dem = 1.0f;
}
else
{
// Calculate gain scaler from specific energy error to throttle
float K_STE2Thr = 1 / (timeConstant() * (_STEdot_max - _STEdot_min));
// Calculate feed-forward throttle
float ff_throttle = 0;
float nomThr = aparm.throttle_cruise * 0.01f;
const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned();
// Use the demanded rate of change of total energy as the feed-forward demand, but add
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns.
float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y));
STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
ff_throttle = nomThr + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
// Calculate PD + FF throttle
float throttle_damp = _thrDamp;
if (_flags.is_doing_auto_land && !is_zero(_land_throttle_damp)) {
throttle_damp = _land_throttle_damp;
}
_throttle_dem = (_STE_error + STEdot_error * throttle_damp) * K_STE2Thr + ff_throttle;
// Constrain throttle demand
_throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
float THRminf_clipped_to_zero = constrain_float(_THRminf, 0, _THRmaxf);
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
if (aparm.throttle_slewrate != 0) {
float thrRateIncr = _DT * (_THRmaxf - THRminf_clipped_to_zero) * aparm.throttle_slewrate * 0.01f;
_throttle_dem = constrain_float(_throttle_dem,
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
_last_throttle_dem = _throttle_dem;
}
// Calculate integrator state upper and lower limits
// Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
// Additionally constrain the integrator state amplitude so that the integrator comes off limits faster.
float maxAmp = 0.5f*(_THRmaxf - THRminf_clipped_to_zero);
float integ_max = constrain_float((_THRmaxf - _throttle_dem + 0.1f),-maxAmp,maxAmp);
float integ_min = constrain_float((_THRminf - _throttle_dem - 0.1f),-maxAmp,maxAmp);
// Calculate integrator state, constraining state
// Set integrator to a max throttle value during climbout
_integTHR_state = _integTHR_state + (_STE_error * _get_i_gain()) * _DT * K_STE2Thr;
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)
{
if (!_flags.reached_speed_takeoff) {
// ensure we run at full throttle until we reach the target airspeed
_throttle_dem = MAX(_throttle_dem, _THRmaxf - _integTHR_state);
}
_integTHR_state = integ_max;
}
else
{
_integTHR_state = constrain_float(_integTHR_state, integ_min, integ_max);
}
// Sum the components.
_throttle_dem = _throttle_dem + _integTHR_state;
}
// Constrain throttle demand
_throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
}
void AP_TECS::_update_height_demand(void)
{
// Apply 2 point moving average to demanded height
_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 && _flags.is_doing_auto_land) {
// 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) {
_integSEB_state = 0;
if (_flare_counter == 0) {
_hgt_rate_dem = _climb_rate;
_land_hgt_dem = _hgt_dem_adj;
}
// 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 over 1s to prevent overshoot
if (_flare_counter < 10) {
_hgt_rate_dem = _hgt_rate_dem * 0.8f - 0.2f * land_sink_rate_adj;
_flare_counter++;
} else {
_hgt_rate_dem = - land_sink_rate_adj;
}
_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 (_flags.is_doing_auto_land) {
if (hgt_dem_lag_filter_slew < 1) {
hgt_dem_lag_filter_slew += 0.1f; // increment at 10Hz to gradually apply the compensation at first
} else {
hgt_dem_lag_filter_slew = 1;
}
new_hgt_dem += hgt_dem_lag_filter_slew*(_hgt_dem_adj - _hgt_dem_adj_last)*10.0f*(timeConstant()+1);
} else {
hgt_dem_lag_filter_slew = 0;
}
_hgt_dem_adj_last = _hgt_dem_adj;
_hgt_dem_adj = new_hgt_dem;
}
void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
int32_t EAS_dem_cm,
enum FlightStage flight_stage,
uint8_t mission_cmd_id,
int32_t ptchMinCO_cd,
int16_t throttle_nudge,
float hgt_afe,
float load_factor)
{
// Calculate time in seconds since last update
uint32_t now = AP_HAL::micros();
_DT = MAX((now - _update_pitch_throttle_last_usec), 0U) * 1.0e-6f;
_update_pitch_throttle_last_usec = now;
// store the mission cmd. This is needed due to the delay between
// starting an approach, and switching to flight stage approach.
// This delay can become a problem with short approaches on steep
// landings using reverse thrust.
_mission_cmd_id = mission_cmd_id;
// Update the speed estimate using a 2nd order complementary filter
_update_speed(load_factor);
// Convert inputs
_hgt_dem = hgt_dem_cm * 0.01f;
_EAS_dem = EAS_dem_cm * 0.01f;
if (aparm.takeoff_throttle_max != 0 &&
(_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)) {
_THRmaxf = aparm.takeoff_throttle_max * 0.01f;
} else {
_THRmaxf = aparm.throttle_max * 0.01f;
}
_THRminf = aparm.throttle_min * 0.01f;
// 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_cd * 0.01f;
} else {
_PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max_cd * 0.01f);
}
// apply temporary pitch limit and clear
_PITCHmaxf = constrain_float(_PITCHmaxf, -90, _pitch_max_limit);
_pitch_max_limit = 90;
if (_pitch_min >= 0) {
_PITCHminf = aparm.pitch_limit_min_cd * 0.01f;
} else {
_PITCHminf = MAX(_pitch_min, aparm.pitch_limit_min_cd * 0.01f);
}
if (flight_stage == FLIGHT_LAND_FINAL) {
// in flare use min pitch from LAND_PITCH_CD
_PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f);
// and use max pitch from TECS_LAND_PMAX
if (_land_pitch_max > 0) {
_PITCHmaxf = MIN(_PITCHmaxf, _land_pitch_max);
}
// and allow zero throttle
_THRminf = 0;
} else if ((flight_stage == FLIGHT_LAND_APPROACH || flight_stage == FLIGHT_LAND_PREFLARE) && (-_climb_rate) > _land_sink) {
// constrain the pitch in landing as we get close to the flare
// point. Use a simple linear limit from 15 meters after the
// landing point
float time_to_flare = (- hgt_afe / _climb_rate) - aparm.land_flare_sec;
if (time_to_flare < 0) {
// we should be flaring already
_PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f);
} else if (time_to_flare < timeConstant()*2) {
// smoothly move the min pitch to the flare min pitch over
// twice the time constant
float p = time_to_flare/(2*timeConstant());
float pitch_limit_cd = p*aparm.pitch_limit_min_cd + (1-p)*aparm.land_pitch_cd;
#if 0
::printf("ttf=%.1f hgt_afe=%.1f _PITCHminf=%.1f pitch_limit=%.1f climb=%.1f\n",
time_to_flare, hgt_afe, _PITCHminf, pitch_limit_cd*0.01f, _climb_rate);
#endif
_PITCHminf = MAX(_PITCHminf, pitch_limit_cd*0.01f);
}
}
// convert to radians
_PITCHmaxf = radians(_PITCHmaxf);
_PITCHminf = radians(_PITCHminf);
_flight_stage = flight_stage;
// initialise selected states and variables if DT > 1 second or in climbout
_initialise_states(ptchMinCO_cd, hgt_afe);
// Calculate Specific Total Energy Rate Limits
_update_STE_rate_lim();
// Calculate the speed demand
_update_speed_demand();
// Calculate the height demand
_update_height_demand();
// Detect underspeed condition
_detect_underspeed();
// Calculate specific energy quantitiues
_update_energies();
// Calculate throttle demand - use simple pitch to throttle if no airspeed sensor
if (_ahrs.airspeed_sensor_enabled()) {
_update_throttle();
} else {
_update_throttle_option(throttle_nudge);
}
// Detect bad descent due to demanded airspeed being too high
_detect_bad_descent();
// Calculate pitch demand
_update_pitch();
// Write internal variables to the log_tuning structure. This
// structure will be logged in dataflash at 10Hz
log_tuning.hgt_dem = _hgt_dem_adj;
log_tuning.hgt = _height;
log_tuning.dhgt_dem = _hgt_rate_dem;
log_tuning.dhgt = _climb_rate;
log_tuning.spd_dem = _TAS_dem_adj;
log_tuning.spd = _integ5_state;
log_tuning.dspd = _vel_dot;
log_tuning.ithr = _integ6_state;
log_tuning.iptch = _integ7_state;
log_tuning.thr = _throttle_dem;
log_tuning.ptch = _pitch_dem;
log_tuning.dspd_dem = _TAS_rate_dem;
log_tuning.time_us = AP_HAL::micros64();
}
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;
}
