Hey there,
We are currently working on making so changes to the glide functionality and are looking to better understand some on the TECS implementation. We intent to share your improvement once the functionally is completed and tested so we want to take the opportunity to do any clean up is necessary. @dagar I believe had work on the last TECS refactoring?
-
In the
_update_energy_estimates()method (see below), is there as reason for_SPE_estimateand_SKE_estimateto evaluated after L237 being used L225 -
We are trying to understand the purpose of
_TAS_setpoint_adjas it is same as_TAS_setpoint -
Likewise
_TAS_setpoint_lastseems unused as it’s only use in _initialize_states -
in _initialize_states why are the _STE_rate_error_filter and _TAS_rate_filter reset to 0.0 at every iteration?
void TECS::_update_energy_estimates()
{
// Calculate specific energy demands in units of (m**2/sec**2)
_SPE_setpoint = _hgt_setpoint * CONSTANTS_ONE_G; // potential energy
_SKE_setpoint = 0.5f * _TAS_setpoint_adj * _TAS_setpoint_adj; // kinetic energy
// Calculate total energy error
_STE_error = _SPE_setpoint - _SPE_estimate + _SKE_setpoint - _SKE_estimate;
// Calculate the specific energy balance demand which specifies how the available total
// energy should be allocated to speed (kinetic energy) and height (potential energy)
// Calculate the specific energy balance error
_SEB_error = SEB_setpoint() - (_SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting);
// Calculate specific energy rate demands in units of (m**2/sec**3)
_SPE_rate_setpoint = _hgt_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change
//! Why _TAS_rate_setpoint and not _TAS_setpoint_adj
_SKE_rate_setpoint = _tas_state * _TAS_rate_setpoint; // kinetic energy rate of change
// Calculate specific energies in units of (m**2/sec**2)
_SPE_estimate = _vert_pos_state * CONSTANTS_ONE_G; // potential energy
_SKE_estimate = 0.5f * _tas_state * _tas_state; // kinetic energy
// Calculate specific energy rates in units of (m**2/sec**3)
_SPE_rate = _vert_vel_state * CONSTANTS_ONE_G; // potential energy rate of change
_SKE_rate = _tas_state * _tas_rate_filtered;// kinetic energy rate of change
}void TECS::_update_speed_setpoint()
{
// Set the TAS 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 true airspeed rate of change based on first order response of true airspeed error
_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _airspeed_error_gain, velRateMin, velRateMax);
}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_initialized) {
// 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_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;
_TAS_setpoint_last = _EAS * EAS2TAS;
_TAS_setpoint_adj = _TAS_setpoint_last;
_underspeed_detected = false;
_uncommanded_descent_recovery = false;
_STE_rate_error = 0.0f;
_hgt_setpoint = baro_altitude;
if (_dt > DT_MAX || _dt < DT_MIN) {
_dt = DT_DEFAULT;
}
_alt_control_traj_generator.reset(0, 0, baro_altitude);
_velocity_control_traj_generator.reset(0.0f, 0.0f, baro_altitude);
} 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;
// airspeed demand states are set to track the measured airspeed
_TAS_setpoint_last = _EAS * EAS2TAS;
_TAS_setpoint_adj = _EAS * EAS2TAS;
_hgt_setpoint = baro_altitude;
// disable speed and decent error condition checks
_underspeed_detected = false;
_uncommanded_descent_recovery = false;
}
// filter specific energy rate error using first order filter with 0.5 second time constant
_STE_rate_error_filter.setParameters(DT_DEFAULT, _STE_rate_time_const);
_STE_rate_error_filter.reset(0.0f);
// filter true airspeed rate using first order filter with 0.5 second time constant
_TAS_rate_filter.setParameters(DT_DEFAULT, _speed_derivative_time_const);
_TAS_rate_filter.reset(0.0f);
_states_initialized = true;
}