void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout,
float EAS2TAS)
{
if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_states_initalized) {
// On first time through or when not using TECS of if there has been a large time slip,
// states must be reset to allow filters to a clean start
_vert_accel_state = 0.0f;
_vert_vel_state = 0.0f;
_vert_pos_state = baro_altitude;
_tas_rate_state = 0.0f;
_tas_state = _EAS * EAS2TAS;
_throttle_integ_state = 0.0f;
_pitch_integ_state = 0.0f;
_last_throttle_setpoint = (_in_air ? throttle_cruise : 0.0f);;
_last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max);
_pitch_setpoint_unc = _last_pitch_setpoint;
_hgt_setpoint_adj_prev = baro_altitude;
_hgt_setpoint_adj = _hgt_setpoint_adj_prev;
_hgt_setpoint_prev = _hgt_setpoint_adj_prev;
_hgt_setpoint_in_prev = _hgt_setpoint_adj_prev;
_TAS_setpoint_last = _EAS * EAS2TAS;
_TAS_setpoint_adj = _TAS_setpoint_last;
_underspeed_detected = false;
_uncommanded_descent_recovery = false;
_STE_rate_error = 0.0f;
if (_dt > DT_MAX || _dt < DT_MIN) {
_dt = DT_DEFAULT;
}
} else if (_climbout_mode_active) {
// During climbout use the lower pitch angle limit specified by the
// calling controller
_pitch_setpoint_min = pitch_min_climbout;
// throttle lower limit is set to a value that prevents throttle reduction
_throttle_setpoint_min = _throttle_setpoint_max - 0.01f;
// height demand and associated states are set to track the measured height
_hgt_setpoint_adj_prev = baro_altitude;
_hgt_setpoint_adj = _hgt_setpoint_adj_prev;
_hgt_setpoint_prev = _hgt_setpoint_adj_prev;
// airspeed demand states are set to track the measured airspeed
_TAS_setpoint_last = _EAS * EAS2TAS;
_TAS_setpoint_adj = _EAS * EAS2TAS;
// disable speed and decent error condition checks
_underspeed_detected = false;
_uncommanded_descent_recovery = false;
}
_states_initalized = true;
}
void TECS::_update_speed_setpoint()
{
// Set the airspeed demand to the minimum value if an underspeed or
// or a uncontrolled descent condition exists to maximise climb rate
if ((_uncommanded_descent_recovery) || (_underspeed_detected)) {
_TAS_setpoint = _TAS_min;
}
_TAS_setpoint = constrain(_TAS_setpoint, _TAS_min, _TAS_max);
// Calculate limits for the demanded rate of change of speed based on physical performance limits
// with a 50% margin to allow the total energy controller to correct for errors.
float velRateMax = 0.5f * _STE_rate_max / _tas_state;
float velRateMin = 0.5f * _STE_rate_min / _tas_state;
_TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max);
// calculate the demanded rate of change of speed proportional to speed error
// and apply performance limits
_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _speed_error_gain, velRateMin, velRateMax);
}
void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspeed, float EAS2TAS)
{
// Calculate the time in seconds since the last update and use the default time step value if out of bounds
uint64_t now = ecl_absolute_time();
const float dt = constrain((now - _speed_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX);
// Convert equivalent airspeed quantities to true airspeed
_EAS_setpoint = airspeed_setpoint;
_TAS_setpoint = _EAS_setpoint * EAS2TAS;
_TAS_max = _indicated_airspeed_max * EAS2TAS;
_TAS_min = _indicated_airspeed_min * EAS2TAS;
// If airspeed measurements are not being used, fix the airspeed estimate to halfway between
// min and max limits
if (!ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) {
_EAS = 0.5f * (_indicated_airspeed_min + _indicated_airspeed_max);
} else {
_EAS = indicated_airspeed;
}
// If first time through or not flying, reset airspeed states
if (_speed_update_timestamp == 0 || !_in_air) {
_tas_rate_state = 0.0f;
_tas_state = (_EAS * EAS2TAS);
}
// Obtain a smoothed airspeed estimate using a second order complementary filter
// Update TAS rate state
float tas_error = (_EAS * EAS2TAS) - _tas_state;
float tas_rate_state_input = tas_error * _tas_estimate_freq * _tas_estimate_freq;
// limit integrator input to prevent windup
if (_tas_state < 3.1f) {
tas_rate_state_input = max(tas_rate_state_input, 0.0f);
}
// Update TAS state
_tas_rate_state = _tas_rate_state + tas_rate_state_input * dt;
float tas_state_input = _tas_rate_state + _speed_derivative + tas_error * _tas_estimate_freq * 1.4142f;
_tas_state = _tas_state + tas_state_input * dt;
// Limit the airspeed state to a minimum of 3 m/s
_tas_state = max(_tas_state, 3.0f);
_speed_update_timestamp = now;
}
void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint,
float EAS_setpoint, float indicated_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max)
{
// Calculate the time since last update (seconds)
uint64_t now = ecl_absolute_time();
_dt = constrain((now - _pitch_update_timestamp) * 1e-6f, DT_MIN, DT_MAX);
// Set class variables from inputs
_throttle_setpoint_max = throttle_max;
_throttle_setpoint_min = throttle_min;
_pitch_setpoint_max = pitch_limit_max;
_pitch_setpoint_min = pitch_limit_min;
_climbout_mode_active = climb_out_setpoint;
// Initialize selected states and variables as required
_initialize_states(pitch, throttle_cruise, baro_altitude, pitch_min_climbout, eas_to_tas);
// Don't run TECS control algorithms when not in flight
if (!_in_air) {
return;
}
// Update the true airspeed state estimate
_update_speed_states(EAS_setpoint, indicated_airspeed, eas_to_tas);
// Calculate rate limits for specific total energy
_update_STE_rate_lim();
// Detect an underspeed condition
_detect_underspeed();
// Detect an uncommanded descent caused by an unachievable airspeed demand
_detect_uncommanded_descent();
// Calculate the demanded true airspeed
_update_speed_setpoint();
// Calculate the demanded height
_update_height_setpoint(hgt_setpoint, baro_altitude);
// Calculate the specific energy values required by the control loop
_update_energy_estimates();
// Calculate the throttle demand
_update_throttle_setpoint(throttle_cruise, rotMat);
// Calculate the pitch demand
_update_pitch_setpoint();
// Update time stamps
_pitch_update_timestamp = now;
// Set TECS mode for next frame
if (_underspeed_detected) {
_tecs_mode = ECL_TECS_MODE_UNDERSPEED;
} else if (_uncommanded_descent_recovery) {
_tecs_mode = ECL_TECS_MODE_BAD_DESCENT;
} else if (_climbout_mode_active) {
_tecs_mode = ECL_TECS_MODE_CLIMBOUT;
} else {
// This is the default operation mode
_tecs_mode = ECL_TECS_MODE_NORMAL;
}
}
/*
* This function implements a complementary filter to estimate the climb rate when
* inertial nav data is not available. It also calculates a true airspeed derivative
* which is used by the airspeed complimentary filter.
*/
void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat,
const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air,
float altitude, bool vz_valid, float vz, float az)
{
// calculate the time lapsed since the last update
uint64_t now = ecl_absolute_time();
float dt = constrain((now - _state_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX);
bool reset_altitude = false;
if (_state_update_timestamp == 0 || dt > DT_MAX) {
dt = DT_DEFAULT;
reset_altitude = true;
}
if (!altitude_lock || !in_air) {
reset_altitude = true;
}
if (reset_altitude) {
_vert_pos_state = altitude;
if (vz_valid) {
_vert_vel_state = -vz;
} else {
_vert_vel_state = 0.0f;
}
_vert_accel_state = 0.0f;
_states_initalized = false;
}
_state_update_timestamp = now;
_EAS = airspeed;
_in_air = in_air;
// Generate the height and climb rate state estimates
if (vz_valid) {
// Set the velocity and position state to the the INS data
_vert_vel_state = -vz;
_vert_pos_state = altitude;
} else {
// Get height acceleration
float hgt_ddot_mea = -az;
// If we have no vertical INS data, estimate the vertical velocity using a complementary filter
// Perform filter calculation using backwards Euler integration
// Coefficients selected to place all three filter poles at omega
// Reference Paper: Optimising the Gains of the Baro-Inertial Vertical Channel
// Widnall W.S, Sinha P.K, AIAA Journal of Guidance and Control, 78-1307R
float omega2 = _hgt_estimate_freq * _hgt_estimate_freq;
float hgt_err = altitude - _vert_pos_state;
float vert_accel_input = hgt_err * omega2 * _hgt_estimate_freq;
_vert_accel_state = _vert_accel_state + vert_accel_input * dt;
float vert_vel_input = _vert_accel_state + hgt_ddot_mea + hgt_err * omega2 * 3.0f;
_vert_vel_state = _vert_vel_state + vert_vel_input * dt;
float vert_pos_input = _vert_vel_state + hgt_err * _hgt_estimate_freq * 3.0f;
// If more than 1 second has elapsed since last update then reset the position state
// to the measured height
if (reset_altitude) {
_vert_pos_state = altitude;
} else {
_vert_pos_state = _vert_pos_state + vert_pos_input * dt;
}
}
// Update and average speed rate of change if airspeed is being measured
if (ISFINITE(airspeed) && airspeed_sensor_enabled()) {
// Assuming the vehicle is flying X axis forward, use the X axis measured acceleration
// compensated for gravity to estimate the rate of change of speed
float speed_deriv_raw = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0);
// Apply some noise filtering
_speed_derivative = 0.95f * _speed_derivative + 0.05f * speed_deriv_raw;
} else {
_speed_derivative = 0.0f;
}
if (!_in_air) {
_states_initalized = false;
}
}
void TECS::_update_pitch_setpoint()
{
/*
* The SKE_weighting variable controls how speed and height control are prioritised by the pitch demand calculation.
* A weighting of 1 givea equal speed and height priority
* A weighting of 0 gives 100% priority to height control and must be used when no airspeed measurement is available.
* A weighting of 2 provides 100% priority to speed control and is used when:
* a) an underspeed condition is detected.
* b) during climbout where a minimum pitch angle has been set to ensure height is gained. If the airspeed
* rises above the demanded value, the pitch angle demand is increased by the TECS controller to prevent the vehicle overspeeding.
* The weighting can be adjusted between 0 and 2 depending on speed and height accuracy requirements.
*/
// Calculate the weighting applied to control of specific kinetic energy error
float SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f);
if ((_underspeed_detected || _climbout_mode_active) && airspeed_sensor_enabled()) {
SKE_weighting = 2.0f;
} else if (!airspeed_sensor_enabled()) {
SKE_weighting = 0.0f;
}
// Calculate the weighting applied to control of specific potential energy error
float SPE_weighting = 2.0f - SKE_weighting;
// Calculate the specific energy balance demand which specifies how the available total
// energy should be allocated to speed (kinetic energy) and height (potential energy)
float SEB_setpoint = _SPE_setpoint * SPE_weighting - _SKE_setpoint * SKE_weighting;
// Calculate the specific energy balance rate demand
float SEB_rate_setpoint = _SPE_rate_setpoint * SPE_weighting - _SKE_rate_setpoint * SKE_weighting;
// Calculate the specific energy balance and balance rate error
_SEB_error = SEB_setpoint - (_SPE_estimate * SPE_weighting - _SKE_estimate * SKE_weighting);
_SEB_rate_error = SEB_rate_setpoint - (_SPE_rate * SPE_weighting - _SKE_rate * SKE_weighting);
// Calculate derivative from change in climb angle to rate of change of specific energy balance
float climb_angle_to_SEB_rate = _tas_state * _pitch_time_constant * CONSTANTS_ONE_G;
// Calculate pitch integrator input term
float pitch_integ_input = _SEB_error * _integrator_gain;
// Prevent the integrator changing in a direction that will increase pitch demand saturation
// Decay the integrator at the control loop time constant if the pitch demand from the previous time step is saturated
if (_pitch_setpoint_unc > _pitch_setpoint_max) {
pitch_integ_input = min(pitch_integ_input,
min((_pitch_setpoint_max - _pitch_setpoint_unc) * climb_angle_to_SEB_rate / _pitch_time_constant, 0.0f));
} else if (_pitch_setpoint_unc < _pitch_setpoint_min) {
pitch_integ_input = max(pitch_integ_input,
max((_pitch_setpoint_min - _pitch_setpoint_unc) * climb_angle_to_SEB_rate / _pitch_time_constant, 0.0f));
}
// Update the pitch integrator state
_pitch_integ_state = _pitch_integ_state + pitch_integ_input * _dt;
// Calculate a specific energy correction that doesn't include the integrator contribution
float SEB_correction = _SEB_error + _SEB_rate_error * _pitch_damping_gain + SEB_rate_setpoint * _pitch_time_constant;
// During climbout, bias the demanded pitch angle so that a zero speed error produces a pitch angle
// demand equal to the minimum pitch angle set by the mission plan. This prevents the integrator
// having to catch up before the nose can be raised to reduce excess speed during climbout.
if (_climbout_mode_active) {
SEB_correction += _pitch_setpoint_min * climb_angle_to_SEB_rate;
}
// Sum the correction terms and convert to a pitch angle demand. This calculation assumes:
// a) The climb angle follows pitch angle with a lag that is small enough not to destabilise the control loop.
// b) The offset between climb angle and pitch angle (angle of attack) is constant, excluding the effect of
// pitch transients due to control action or turbulence.
_pitch_setpoint_unc = (SEB_correction + _pitch_integ_state) / climb_angle_to_SEB_rate;
_pitch_setpoint = constrain(_pitch_setpoint_unc, _pitch_setpoint_min, _pitch_setpoint_max);
// Comply with the specified vertical acceleration limit by applying a pitch rate limit
float ptchRateIncr = _dt * _vert_accel_limit / _tas_state;
if ((_pitch_setpoint - _last_pitch_setpoint) > ptchRateIncr) {
_pitch_setpoint = _last_pitch_setpoint + ptchRateIncr;
} else if ((_pitch_setpoint - _last_pitch_setpoint) < -ptchRateIncr) {
_pitch_setpoint = _last_pitch_setpoint - ptchRateIncr;
}
_last_pitch_setpoint = _pitch_setpoint;
}
void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::Dcmf &rotMat)
{
// Calculate total energy error
_STE_error = _SPE_setpoint - _SPE_estimate + _SKE_setpoint - _SKE_estimate;
// Calculate demanded rate of change of total energy, respecting vehicle limits
float STE_rate_setpoint = constrain((_SPE_rate_setpoint + _SKE_rate_setpoint), _STE_rate_min, _STE_rate_max);
// Calculate the total energy rate error, applying a first order IIR filter
// to reduce the effect of accelerometer noise
_STE_rate_error = 0.2f * (STE_rate_setpoint - _SPE_rate - _SKE_rate) + 0.8f * _STE_rate_error;
// Calculate the throttle demand
if (_underspeed_detected) {
// always use full throttle to recover from an underspeed condition
_throttle_setpoint = 1.0f;
} else {
// Adjust the demanded total energy rate to compensate for induced drag rise in turns.
// Assume induced drag scales linearly with normal load factor.
// The additional normal load factor is given by (1/cos(bank angle) - 1)
float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (1.0f / constrain(cosPhi, 0.1f, 1.0f) - 1.0f);
// Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle
// as the starting point. Assume:
// Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max
// Specific total energy rate = 0 at cruise throttle
// Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
float throttle_predicted = 0.0f;
if (STE_rate_setpoint >= 0) {
// throttle is between cruise and maximum
throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max - throttle_cruise);
} else {
// throttle is between cruise and minimum
throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min - throttle_cruise);
}
// Calculate gain scaler from specific energy error to throttle
float STE_to_throttle = 1.0f / (_throttle_time_constant * (_STE_rate_max - _STE_rate_min));
// Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits
_throttle_setpoint = (_STE_error + _STE_rate_error * _throttle_damping_gain) * STE_to_throttle + throttle_predicted;
_throttle_setpoint = constrain(_throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
// Rate limit the throttle demand
if (fabsf(_throttle_slewrate) > 0.01f) {
float throttle_increment_limit = _dt * (_throttle_setpoint_max - _throttle_setpoint_min) * _throttle_slewrate;
_throttle_setpoint = constrain(_throttle_setpoint, _last_throttle_setpoint - throttle_increment_limit,
_last_throttle_setpoint + throttle_increment_limit);
}
_last_throttle_setpoint = _throttle_setpoint;
// Calculate throttle integrator state upper and lower limits with allowance for
// 10% throttle saturation to accommodate noise on the demand
float integ_state_max = (_throttle_setpoint_max - _throttle_setpoint + 0.1f);
float integ_state_min = (_throttle_setpoint_min - _throttle_setpoint - 0.1f);
// Calculate a throttle demand from the integrated total energy error
// This will be added to the total throttle demand to compensate for steady state errors
_throttle_integ_state = _throttle_integ_state + (_STE_error * _integrator_gain) * _dt * STE_to_throttle;
if (_climbout_mode_active) {
// During climbout, set the integrator to maximum throttle to prevent transient throttle drop
// at end of climbout when we transition to closed loop throttle control
_throttle_integ_state = integ_state_max;
} else {
// Respect integrator limits during closed loop operation.
_throttle_integ_state = constrain(_throttle_integ_state, integ_state_min, integ_state_max);
}
if (airspeed_sensor_enabled()) {
// Add the integrator feedback during closed loop operation with an airspeed sensor
_throttle_setpoint = _throttle_setpoint + _throttle_integ_state;
} else {
// when flying without an airspeed sensor, use the predicted throttle only
_throttle_setpoint = throttle_predicted;
}
_throttle_setpoint = constrain(_throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
}
}