Understanding the TECS implementation

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?

  1. In the _update_energy_estimates() method (see below), is there as reason for _SPE_estimate and _SKE_estimate to evaluated after L237 being used L225

  2. We are trying to understand the purpose of _TAS_setpoint_adj as it is same as _TAS_setpoint

  3. Likewise _TAS_setpoint_last seems unused as it’s only use in _initialize_states

  4. 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);

	// filter true airspeed rate using first order filter with 0.5 second time constant
	_TAS_rate_filter.setParameters(DT_DEFAULT, _speed_derivative_time_const);

	_states_initialized = true;

Are you referring to latest TECS? There was a refactor a couple of weeks ago, please have a look at that one if not yet! While on it we also fixed a couple of bugs.

Oh, I wasn’t aware, we were working on v1.13. I will need to look at the new code.
Do you know in which version these changes should be available? Should we be able to pull only the TECS change to our current version to test?

You would need to check the current main branch. I think the interfaces to TECS have changed a bit, but not crazy much. So you should be able to integrate the new TECS in you software. Or what I always recommend: stay with your development close to upstream development, so base your work on current main.

Yeah, it think this is what make more sense. We were on the latest release.
Do you know what is the process for thing in the dev branch to make it in a release?

It will me in 1.14 I guess, which will be released in a some weeks I hope. But again, I would simply base your work on the current main branch.