Create total energy control system implementation

This is a new, clean and streamlined variant of the mathematical derivation I created a few years ago.

CMakeLists.txt

@@ -56,6 +56,7 @@ px4_add_module(
 		EKF/vel_pos_fusion.cpp
 		EKF/drag_fusion.cpp
 		l1/ecl_l1_pos_controller.cpp
+		tecs/tecs.cpp
 		validation/data_validator.cpp
 		validation/data_validator_group.cpp
 	DEPENDS

EKF/ekf.cpp

@@ -485,6 +485,11 @@ void Ekf::calculateOutputStates()
 	// corrrect for measured accceleration due to gravity
 	delta_vel_NED(2) += _gravity_mss * imu_new.delta_vel_dt;
 
+	// calculate the earth frame velocity derivatives
+	if (imu_new.delta_vel_dt > 1e-4f) {
+		_vel_deriv_ned = delta_vel_NED * (1.0f / imu_new.delta_vel_dt);
+	}
+
 	// save the previous velocity so we can use trapezidal integration
 	Vector3f vel_last = _output_new.vel;
 
@@ -502,14 +507,18 @@ void Ekf::calculateOutputStates()
 	// accumulate the time for each update
 	_output_vert_new.dt += imu_new.delta_vel_dt;
 
-	// calculate the average angular rate across the last IMU update
-	Vector3f ang_rate = _imu_sample_new.delta_ang * (1.0f / _imu_sample_new.delta_ang_dt);
+	// correct velocity for IMU offset
+	if (_imu_sample_new.delta_ang_dt > 1e-4f) {
+		// calculate the average angular rate across the last IMU update
+		Vector3f ang_rate = _imu_sample_new.delta_ang * (1.0f / _imu_sample_new.delta_ang_dt);
 
-	// calculate the velocity of the IMU relative to the body origin
-	Vector3f vel_imu_rel_body = cross_product(ang_rate, _params.imu_pos_body);
+		// calculate the velocity of the IMU relative to the body origin
+		Vector3f vel_imu_rel_body = cross_product(ang_rate, _params.imu_pos_body);
 
-	// rotate the relative velocity into earth frame
-	_vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body;
+		// rotate the relative velocity into earth frame
+		_vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body;
+
+	}
 
 	// store the INS states in a ring buffer with the same length and time coordinates as the IMU data buffer
 	if (_imu_updated) {

EKF/estimator_interface.h

@@ -231,6 +231,14 @@ public:
 		}
 	}
 
+	// get the NED velocity derivative in earth frame
+	void get_vel_deriv_ned(float *vel_deriv)
+	{
+		for (unsigned i = 0; i < 3; i++) {
+			vel_deriv[i] = _vel_deriv_ned(i);
+		}
+	}
+
 	// get the derivative of the vertical position of the body frame origin in local NED earth frame
 	void get_pos_d_deriv(float *pos_d_deriv)
 	{
@@ -380,6 +388,7 @@ protected:
 	imuSample _imu_sample_new{};		// imu sample capturing the newest imu data
 	Matrix3f _R_to_earth_now;		// rotation matrix from body to earth frame at current time
 	Vector3f _vel_imu_rel_body_ned;		// velocity of IMU relative to body origin in NED earth frame
+	Vector3f _vel_deriv_ned;		// velocity derivative at the IMU in NED earth frame (m/s/s)
 
 	uint64_t _imu_ticks{0};	// counter for imu updates
 

tecs/tecs.cpp

@@ -0,0 +1,645 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+#include "tecs.h"
+
+#include <ecl/ecl.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+using math::constrain;
+using math::max;
+using math::min;
+
+/**
+ * @file tecs.cpp
+ *
+ * @author Paul Riseborough
+ */
+
+/*
+ * This function implements a complementary filter to estimate the climb rate when
+ * inertial nav data is not available. It also calculates a true airpseed derivative
+ * which is used by the airspeed complimentary filter.
+ */
+void TECS::update_vehicle_state_estimates(float airspeed, const math::Matrix<3, 3> &rotMat,
+			const math::Vector<3> &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 = max((now - _state_update_timestamp), static_cast<uint64_t>(0)) * 1.0e-6f;
+
+	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;
+
+	// Genrate 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 (PX4_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_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();
+	float dt = max((now - _speed_update_timestamp), UINT64_C(0)) * 1.0e-6f;
+	if (dt < DT_MIN || dt > DT_MAX) {
+		dt = DT_DEFAULT;
+
+	}
+
+	// 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 (!PX4_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_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);
+
+	// Apply limits on the demanded rate of change of speed based based on physical performance limits
+	// with a 50% margin to allow the total energy controller to correct for errors.
+	float velRateMax;
+	float velRateMin;
+
+	if ((_uncommanded_descent_recovery) || (_underspeed_detected)) {
+		velRateMax = 0.5f * _STE_rate_max / _tas_state;
+		velRateMin = 0.5f * _STE_rate_min / _tas_state;
+
+	} else {
+		velRateMax = 0.5f * _STE_rate_max / _tas_state;
+		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_height_setpoint(float desired, float state)
+{
+	// Detect first time through and initialize previous value to demand
+	if (PX4_ISFINITE(desired) && fabsf(_hgt_setpoint_in_prev) < 0.1f) {
+		_hgt_setpoint_in_prev = desired;
+	}
+
+	// Apply a 2 point moving average to demanded height to reduce
+	// intersampling noise effects.
+	if (PX4_ISFINITE(desired)) {
+		_hgt_setpoint = 0.5f * (desired + _hgt_setpoint_in_prev);
+
+	} else {
+		_hgt_setpoint = _hgt_setpoint_in_prev;
+	}
+
+	_hgt_setpoint_in_prev = _hgt_setpoint;
+
+	// Apply a rate limit to respect vehicle performance limitations
+	if ((_hgt_setpoint - _hgt_setpoint_prev) > (_max_climb_rate * _dt)) {
+		_hgt_setpoint = _hgt_setpoint_prev + _max_climb_rate * _dt;
+
+	} else if ((_hgt_setpoint - _hgt_setpoint_prev) < (-_max_sink_rate * _dt)) {
+		_hgt_setpoint = _hgt_setpoint_prev - _max_sink_rate * _dt;
+	}
+
+	_hgt_setpoint_prev = _hgt_setpoint;
+
+	// Apply a first order noise filter
+	_hgt_setpoint_adj = 0.1f * _hgt_setpoint + 0.9f * _hgt_setpoint_adj_prev;
+
+	// Calculate the demanded climb rate proportional to height error plus a feedforward term to provide
+	// tight tracking during steady climb and descent manouvres.
+	_hgt_rate_setpoint = (_hgt_setpoint_adj - state) * _height_error_gain + _height_setpoint_gain_ff * (_hgt_setpoint_adj - _hgt_setpoint_adj_prev) / _dt;
+	_hgt_setpoint_adj_prev = _hgt_setpoint_adj;
+
+	// Limit the rate of change of height demand to respect vehicle performance limits
+	if (_hgt_rate_setpoint > _max_climb_rate) {
+		_hgt_rate_setpoint = _max_climb_rate;
+
+	} else if (_hgt_rate_setpoint < -_max_sink_rate) {
+		_hgt_rate_setpoint = -_max_sink_rate;
+	}
+}
+
+void TECS::_detect_underspeed()
+{
+	if (!_detect_underspeed_enabled) {
+		_underspeed_detected = false;
+		return;
+	}
+
+	if (((_tas_state < _TAS_min * 0.9f) && (_throttle_setpoint >= _throttle_setpoint_max * 0.95f)) || ((_vert_pos_state < _hgt_setpoint_adj)
+			&& _underspeed_detected)) {
+
+		_underspeed_detected = true;
+
+	} else {
+		_underspeed_detected = false;
+	}
+}
+
+void TECS::_update_energy_estimates()
+{
+	// Calculate specific energy demands in units of (m**2/sec**2)
+	_SPE_setpoint = _hgt_setpoint_adj * CONSTANTS_ONE_G; // potential energy
+	_SKE_setpoint = 0.5f * _TAS_setpoint_adj * _TAS_setpoint_adj; // kinetic energy
+
+	// 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
+	_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 ernegy rate of change
+	_SKE_rate = _tas_state * _speed_derivative;// kinetic energy rate of change
+}
+
+void TECS::_update_throttle_setpoint(const float throttle_cruise, const math::Matrix<3, 3> &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 acheived when throttle is set to to _throttle_setpoint_max
+		// Specific total energy rate = 0 at cruise throttle
+		// Specific total energy rate = _STE_rate_min is acheived when throttle is set to 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 accoodate 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 traniton 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);
+	}
+}
+
+void TECS::_detect_uncommanded_descent()
+{
+	/*
+	 * This function detects a condition that can occur when the demanded airspeed is greater than the
+	 * aircraft can achieve in level flight. When this occurs, the vehicle will continue to reduce height
+	 * while attempting to maintain speed.
+	*/
+
+	// Calculate rate of change of total specific energy
+	float STE_rate = _SPE_rate + _SKE_rate;
+
+	// If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode
+	bool enter_mode = !_uncommanded_descent_recovery && !_underspeed_detected && (_STE_error > 200.0f) && (STE_rate < 0.0f) && (_throttle_setpoint >= _throttle_setpoint_max * 0.9f);
+
+	// If we enter an underspeed cindition or recover the required total energy, then exit uncommanded descent recovery mode
+	bool exit_mode = _uncommanded_descent_recovery && (_underspeed_detected || (_STE_error < 0.0f));
+	if (enter_mode) {
+		_uncommanded_descent_recovery = true;
+
+	} else if (exit_mode) {
+		_uncommanded_descent_recovery = 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 fromthe 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::_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 = throttle_cruise;
+		_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_STE_rate_lim()
+{
+	// Calculate the specific total energy upper rate limits from the max throttle climb rate
+	_STE_rate_max = _max_climb_rate * CONSTANTS_ONE_G;
+
+	// Calculate the specific total energy lower rate limits from the min throttle sink rate
+	_STE_rate_min = - _min_sink_rate * CONSTANTS_ONE_G;
+}
+
+void TECS::update_pitch_throttle(const math::Matrix<3, 3> &rotMat, float pitch, float baro_altitude, float hgt_setpoint,
+				 float EAS_setpoint, float indicated_airspeed, float EAS2TAS, 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 = max((now - _pitch_update_timestamp), UINT64_C(0)) * 1.0e-6f;
+
+	// 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, EAS2TAS);
+
+	// Don't run TECS control agorithms when not in flight
+	if (!_in_air) {
+		return;
+	}
+
+	// Update the true airspeed state estimate
+	_update_speed_states(EAS_setpoint, indicated_airspeed, EAS2TAS);
+
+	// 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;
+	}
+
+}

tecs/tecs.h

@@ -0,0 +1,488 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tecs.cpp
+ *
+ * @author Paul Riseborough
+ */
+
+#pragma once
+
+#include <mathlib/mathlib.h>
+#include <stdint.h>
+
+class __EXPORT TECS
+{
+public:
+	TECS() :
+		_state_update_timestamp(0),
+		_speed_update_timestamp(0),
+		_pitch_update_timestamp(0),
+		_hgt_estimate_freq(0.0f),
+		_tas_estimate_freq(0.0f),
+		_max_climb_rate(2.0f),
+		_min_sink_rate(1.0f),
+		_max_sink_rate(2.0f),
+		_pitch_time_constant(5.0f),
+		_throttle_time_constant(8.0f),
+		_pitch_damping_gain(0.0f),
+		_throttle_damping_gain(0.0f),
+		_integrator_gain(0.0f),
+		_vert_accel_limit(0.0f),
+		_load_factor_correction(0.0f),
+		_pitch_speed_weight(1.0f),
+		_height_error_gain(0.0f),
+		_height_setpoint_gain_ff(0.0f),
+		_speed_error_gain(0.0f),
+		_throttle_setpoint(0.0f),
+		_pitch_setpoint(0.0f),
+		_vert_accel_state(0.0f),
+		_vert_vel_state(0.0f),
+		_vert_pos_state(0.0f),
+		_tas_rate_state(0.0f),
+		_tas_state(0.0f),
+		_throttle_integ_state(0.0f),
+		_pitch_integ_state(0.0f),
+		_last_throttle_setpoint(0.0f),
+		_last_pitch_setpoint(0.0f),
+		_speed_derivative(0.0f),
+		_EAS(0.0f),
+		_TAS_max(30.0f),
+		_TAS_min(3.0f),
+		_TAS_setpoint(0.0f),
+		_TAS_setpoint_last(0.0f),
+		_EAS_setpoint(0.0f),
+		_TAS_setpoint_adj(0.0f),
+		_TAS_rate_setpoint(0.0f),
+		_indicated_airspeed_min(3.0f),
+		_indicated_airspeed_max(30.0f),
+		_hgt_setpoint(0.0f),
+		_hgt_setpoint_in_prev(0.0f),
+		_hgt_setpoint_prev(0.0f),
+		_hgt_setpoint_adj(0.0f),
+		_hgt_setpoint_adj_prev(0.0f),
+		_hgt_rate_setpoint(0.0f),
+		_pitch_setpoint_unc(0.0f),
+		_STE_rate_max(0.0f),
+		_STE_rate_min(0.0f),
+		_throttle_setpoint_max(0.0f),
+		_throttle_setpoint_min(0.0f),
+		_pitch_setpoint_max(0.5f),
+		_pitch_setpoint_min(-0.5f),
+		_throttle_slewrate(0.0f),
+		_SPE_setpoint(0.0f),
+		_SKE_setpoint(0.0f),
+		_SPE_rate_setpoint(0.0f),
+		_SKE_rate_setpoint(0.0f),
+		_SPE_estimate(0.0f),
+		_SKE_estimate(0.0f),
+		_SPE_rate(0.0f),
+		_SKE_rate(0.0f),
+		_STE_error(0.0f),
+		_STE_rate_error(0.0f),
+		_SEB_error(0.0f),
+		_SEB_rate_error(0.0f),
+		_dt(0.02f),
+		_underspeed_detected(false),
+		_detect_underspeed_enabled(true),
+		_uncommanded_descent_recovery(false),
+		_climbout_mode_active(false),
+		_airspeed_enabled(false),
+		_states_initalized(false),
+		_in_air(false)
+	{
+	}
+
+	/**
+	 * Get the current airspeed status
+	 *
+	 * @return true if airspeed is enabled for control
+	 */
+	bool airspeed_sensor_enabled()
+	{
+		return _airspeed_enabled;
+	}
+
+	/**
+	 * Set the airspeed enable state
+	 */
+	void enable_airspeed(bool enabled)
+	{
+		_airspeed_enabled = enabled;
+	}
+
+	/**
+	 * Updates the following vehicle kineamtic state estimates:
+	 * Vertical position, velocity and acceleration.
+	 * Speed derivative
+	 * Must be called prior to udating tecs control loops
+	 * Must be called at 50Hz or greater
+	 */
+	void update_vehicle_state_estimates(float airspeed, const math::Matrix<3, 3> &rotMat,
+			  const math::Vector<3> &accel_body, bool altitude_lock, bool in_air,
+					    float altitude, bool vz_valid, float vz, float az);
+
+	/**
+	 * Update the control loop calculations
+	 */
+	void update_pitch_throttle(const math::Matrix<3, 3> &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_setpoint_max, float throttle_cruise,
+				   float pitch_limit_min, float pitch_limit_max);
+
+	float get_throttle_setpoint(void) { return _throttle_setpoint; }
+	float get_pitch_setpoint() { return _pitch_setpoint; }
+	float get_speed_weight() { return _pitch_speed_weight; }
+
+	void reset_state()
+	{
+		_states_initalized = false;
+	}
+
+	enum ECL_TECS_MODE {
+		ECL_TECS_MODE_NORMAL = 0,
+		ECL_TECS_MODE_UNDERSPEED,
+		ECL_TECS_MODE_BAD_DESCENT,
+		ECL_TECS_MODE_CLIMBOUT
+	};
+
+	enum ECL_TECS_MODE _tecs_mode;
+
+	void set_time_const(float time_const)
+	{
+		_pitch_time_constant = time_const;
+	}
+
+	void set_time_const_throt(float time_const_throt)
+	{
+		_throttle_time_constant = time_const_throt;
+	}
+
+	void set_min_sink_rate(float rate)
+	{
+		_min_sink_rate = rate;
+	}
+
+	void set_max_sink_rate(float sink_rate)
+	{
+		_max_sink_rate = sink_rate;
+	}
+
+	void set_max_climb_rate(float climb_rate)
+	{
+		_max_climb_rate = climb_rate;
+	}
+
+	void set_throttle_damp(float throttle_damp)
+	{
+		_throttle_damping_gain = throttle_damp;
+	}
+
+	void set_integrator_gain(float gain)
+	{
+		_integrator_gain = gain;
+	}
+
+	void set_vertical_accel_limit(float limit)
+	{
+		_vert_accel_limit = limit;
+	}
+
+	void set_height_comp_filter_omega(float omega)
+	{
+		_hgt_estimate_freq = omega;
+	}
+
+	void set_speed_comp_filter_omega(float omega)
+	{
+		_tas_estimate_freq = omega;
+	}
+
+	void set_roll_throttle_compensation(float compensation)
+	{
+		_load_factor_correction = compensation;
+	}
+
+	void set_speed_weight(float weight)
+	{
+		_pitch_speed_weight = weight;
+	}
+
+	void set_pitch_damping(float damping)
+	{
+		_pitch_damping_gain = damping;
+	}
+
+	void set_throttle_slewrate(float slewrate)
+	{
+		_throttle_slewrate = slewrate;
+	}
+
+	void set_indicated_airspeed_min(float airspeed)
+	{
+		_indicated_airspeed_min = airspeed;
+	}
+
+	void set_indicated_airspeed_max(float airspeed)
+	{
+		_indicated_airspeed_max = airspeed;
+	}
+
+	void set_heightrate_p(float heightrate_p)
+	{
+		_height_error_gain = heightrate_p;
+	}
+
+	void set_heightrate_ff(float heightrate_ff)
+	{
+		_height_setpoint_gain_ff = heightrate_ff;
+	}
+
+	void set_speedrate_p(float speedrate_p)
+	{
+		_speed_error_gain = speedrate_p;
+	}
+
+	void set_detect_underspeed_enabled(bool enabled)
+	{
+		_detect_underspeed_enabled = enabled;
+	}
+
+	float hgt_setpoint_adj() { return _hgt_setpoint_adj; }
+
+	float vert_pos_state() { return _vert_pos_state; }
+
+	float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
+
+	float tas_state() { return _tas_state; }
+
+	float hgt_rate_setpoint() { return _hgt_rate_setpoint; }
+
+	float vert_vel_state() { return _vert_vel_state; }
+
+	float TAS_rate_setpoint() { return _TAS_rate_setpoint; }
+
+	float speed_derivative() { return _speed_derivative; }
+
+	float STE_error() { return _STE_error; }
+
+	float STE_rate_error() { return _STE_rate_error; }
+
+	float SEB_error() { return _SEB_error; }
+
+	float SEB_rate_error() { return _SEB_rate_error; }
+
+	float throttle_integ_state() { return _throttle_integ_state; }
+
+	float pitch_integ_state() { return _pitch_integ_state; }
+
+	int tecs_mode() { return _tecs_mode; }
+
+	uint64_t timestamp() { return _pitch_update_timestamp; }
+
+	/**
+	 * Handle the altitude reset
+	 *
+	 * If the estimation system resets the height in one discrete step this
+	 * will gracefully even out the reset over time.
+	 */
+	void handle_alt_step(float delta_alt, float altitude)
+	{
+		// add height reset delta to all variables involved
+		// in filtering the demanded height
+		_hgt_setpoint_in_prev += delta_alt;
+		_hgt_setpoint_prev += delta_alt;
+		_hgt_setpoint_adj_prev += delta_alt;
+
+		// reset height states
+		_vert_pos_state = altitude;
+		_vert_accel_state = 0.0f;
+		_vert_vel_state = 0.0f;
+	}
+
+private:
+
+	// timestamps
+	uint64_t _state_update_timestamp;			///< last timestamp of the 50 Hz function call
+	uint64_t _speed_update_timestamp;			///< last timestamp of the speed function call
+	uint64_t _pitch_update_timestamp;			///< last timestamp of the pitch function call
+
+	// controller parameters
+	float _hgt_estimate_freq;					///< cross-over frequency of the height rate complementary filter (rad/sec)
+	float _tas_estimate_freq;					///< cross-over frequency of the true airspeed complementary filter (rad/sec)
+	float _max_climb_rate;						///< climb rate produced by max allowed throttle (m/sec)
+	float _min_sink_rate;						///< sink rate produced by min allowed throttle (m/sec)
+	float _max_sink_rate;						///< maximum safe sink rate (m/sec)
+	float _pitch_time_constant;					///< control time constant used by the pitch demand calculation (sec)
+	float _throttle_time_constant;				///< control time constant used by the throttle demand calculation (sec)
+	float _pitch_damping_gain;					///< damping gain of the pitch demand calculation (sec)
+	float _throttle_damping_gain;				///< damping gain of the throttle demand calculation (sec)
+	float _integrator_gain;						///< integrator gain used by the throttle and pitch demand calculation
+	float _vert_accel_limit;					///< magnitude of the maximum vertical acceleration allowed (m/sec**2)
+	float _load_factor_correction;				///< gain from normal load factor increase to total energy rate demand (m**2/sec**3)
+	float _pitch_speed_weight;					///< speed control weighting used by pitch demand calculation
+	float _height_error_gain;					///< gain from height error to demanded climb rate (1/sec)
+	float _height_setpoint_gain_ff;				///< gain from height demand derivative to demanded climb rate
+	float _speed_error_gain;					///< gain from speed error to demanded speed rate (1/sec)
+
+	// controller outputs
+	float _throttle_setpoint;					///< normalized throttle demand (0..1)
+	float _pitch_setpoint;						///< pitch angle demand (radians)
+
+	// complimentary filter states
+	float _vert_accel_state;					///< complimentary filter state - height second derivative (m/sec**2)
+	float _vert_vel_state;						///< complimentary filter state - height rate (m/sec)
+	float _vert_pos_state;						///< complimentary filter state - height (m)
+	float _tas_rate_state;						///< complimentary filter state - true airspeed first derivative (m/sec**2)
+	float _tas_state;							///< complimentary filter state - true airspeed (m/sec)
+
+	// controller states
+	float _throttle_integ_state;				///< throttle integrator state
+	float _pitch_integ_state;					///< pitch integrator state (rad)
+	float _last_throttle_setpoint;				///< throttle demand rate limiter state (1/sec)
+	float _last_pitch_setpoint;					///< pitch demand rate limiter state (rad/sec)
+	float _speed_derivative;					///< rate of change of speed along X axis (m/sec**2)
+
+	// speed demand calculations
+	float _EAS;									///< equivalent airspeed (m/sec)
+	float _TAS_max;								///< true airpeed demand upper limit (m/sec)
+	float _TAS_min;								///< true airpeed demand lower limit (m/sec)
+	float _TAS_setpoint;						///< current airpeed demand (m/sec)
+	float _TAS_setpoint_last;					///< previous true airpeed demand (m/sec)
+	float _EAS_setpoint;						///< Equivalent airspeed demand (m/sec)
+	float _TAS_setpoint_adj;					///< true airspeed demand tracked by the TECS algorithm (m/sec)
+	float _TAS_rate_setpoint;					///< true airspeed rate demand tracked by the TECS algorithm (m/sec**2)
+	float _indicated_airspeed_min;				///< equivalent airspeed demand lower limit (m/sec)
+	float _indicated_airspeed_max;				///< equivalent airspeed demand upper limit (m/sec)
+
+	// height demand calculations
+	float _hgt_setpoint;						///< demanded height tracked by the TECS algorithm (m)
+	float _hgt_setpoint_in_prev;				///< previous value of _hgt_setpoint after noise filtering (m)
+	float _hgt_setpoint_prev;					///< previous value of _hgt_setpoint after noise filtering and rate limiting (m)
+	float _hgt_setpoint_adj;					///< demanded height used by the control loops after all filtering has been applied (m)
+	float _hgt_setpoint_adj_prev;				///< value of _hgt_setpoint_adj from previous frame (m)
+	float _hgt_rate_setpoint;					///< demanded climb rate tracked by the TECS algorithm
+
+	// vehicle physical limits
+	float _pitch_setpoint_unc;					///< pitch demand before limiting (rad)
+	float _STE_rate_max;						///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3)
+	float _STE_rate_min;						///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3)
+	float _throttle_setpoint_max;				///< normalised throttle upper limit
+	float _throttle_setpoint_min;				///< normalised throttle lower limit
+	float _pitch_setpoint_max;					///< pitch demand upper limit (rad)
+	float _pitch_setpoint_min;					///< pitch demand lower limit (rad)
+	float _throttle_slewrate;					///< throttle demand slew rate limit (1/sec)
+
+	// specific energy quantities
+	float _SPE_setpoint;						///< specific potential energy demand (m**2/sec**2)
+	float _SKE_setpoint;						///< specific kinetic energy demand (m**2/sec**2)
+	float _SPE_rate_setpoint;					///< specific potential energy rate demand (m**2/sec**3)
+	float _SKE_rate_setpoint;					///< specific kinetic energy rate demand (m**2/sec**3)
+	float _SPE_estimate;						///< specific potential energy estimate (m**2/sec**2)
+	float _SKE_estimate;						///< specific kinetic energy estimate (m**2/sec**2)
+	float _SPE_rate;							///< specific potential energy rate estimate (m**2/sec**3)
+	float _SKE_rate;							///< specific kinetic energy rate estimate (m**2/sec**3)
+
+	// specific energy error quantities
+	float _STE_error;							///< specific total energy error (m**2/sec**2)
+	float _STE_rate_error;						///< specific total energy rate error (m**2/sec**3)
+	float _SEB_error;							///< specific energy balance error (m**2/sec**2)
+	float _SEB_rate_error;						///< specific energy balance rate error (m**2/sec**3)
+
+	// time steps (non-fixed)
+	float _dt;									///< Time since last update of main TECS loop (sec)
+	static constexpr float DT_MIN = 0.001f;		///< minimum allowed value of _dt (sec)
+	static constexpr float DT_DEFAULT = 0.02f;	///< default value for _dt (sec)
+	static constexpr float DT_MAX = 1.0f;		///< max value of _dt allowed before a filter state reset is performed (sec)
+
+	// controller mode logic
+	bool _underspeed_detected;					///< true when an underspeed condition has been detected
+	bool _detect_underspeed_enabled;			///< true when underspeed detection is enabled
+	bool _uncommanded_descent_recovery;			///< true when a continuous descent caused by an unachievable airspeed demand has been detected
+	bool _climbout_mode_active;					///< true when in climbout mode
+	bool _airspeed_enabled;						///< true when airspeed use has been enabled
+	bool _states_initalized;					///< true when TECS states have been iniitalized
+	bool _in_air;								///< true when the vehicle is flying
+
+	/**
+	 * Update the airspeed internal state using a second order complementary filter
+	 */
+	void _update_speed_states(float airspeed_setpoint, float indicated_airspeed, float eas_to_tas);
+
+	/**
+	 * Update the desired airspeed
+	 */
+	void _update_speed_setpoint();
+
+	/**
+	 * Update the desired height
+	 */
+	void _update_height_setpoint(float desired, float state);
+
+	/**
+	 * Detect if the system is not capable of maintaining airspeed
+	 */
+	void _detect_underspeed(void);
+
+	/**
+	 * Update specific energy
+	 */
+	void _update_energy_estimates(void);
+
+	/**
+	 * Update throttle setpoint
+	 */
+	void _update_throttle_setpoint(float throttle_cruise, const math::Matrix<3, 3> &rotMat);
+
+	/**
+	 * Detect an uncommanded descent
+	 */
+	void _detect_uncommanded_descent(void);
+
+	/**
+	 * Update the pitch setpoint
+	 */
+	void _update_pitch_setpoint(void);
+
+	/**
+	 * Initialize the controller
+	 */
+	void _initialize_states(float pitch, float throttle_cruise, float baro_altitude, float min_pitch, float eas_to_tas);
+
+	/**
+	 * Calculate specific total energy rate limits
+	 */
+	void _update_STE_rate_lim(void);
+
+};