Checked in L1 position and TECS altitude control. Not test-flown in HIL or outdoors yet

3 files changed, 932 insertions, 0 deletions

diff --git a/src/lib/external_lgpl/module.mk b/src/lib/external_lgpl/module.mk
new file mode 100644
index 000000000..619a1a5df
--- /dev/null
+++ b/src/lib/external_lgpl/module.mk
@@ -0,0 +1,48 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. 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 PX4 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.
+#
+############################################################################
+
+#
+#  W A R N I N G: The contents of this directory are license-incompatible
+# 			with the rest of the codebase. Do NOT copy any parts of it
+#			into other folders.
+#
+#  Acknowledgements:
+#
+#			The algorithms in this folder have been developed by Paul Riseborough
+#			with support from Andrew Tridgell.
+#			Originally licensed as LGPL for APM. As this is built as library and
+#			linked, use of this code does not change the usability of PX4 in general
+#			or any of the license implications.
+#
+
+SRCS		 = 	tecs/TECS.cpp
diff --git a/src/lib/external_lgpl/tecs/TECS.cpp b/src/lib/external_lgpl/tecs/TECS.cpp
new file mode 100644
index 000000000..3f5355243
--- /dev/null
+++ b/src/lib/external_lgpl/tecs/TECS.cpp
@@ -0,0 +1,534 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+#include "TECS.h"
+#include <ecl/ecl.h>
+
+using namespace math;
+
+#ifndef CONSTANTS_ONE_G
+#define CONSTANTS_ONE_G GRAVITY
+#endif
+
+/**
+ * @file TECS.cpp
+ *
+ * @author Paul Riseborough
+ *
+ *  Written by Paul Riseborough 2013 to provide:
+ *  - Combined control of speed and height using throttle to control
+ *    total energy and pitch angle to control exchange of energy between
+ *    potential and kinetic.
+ *    Selectable speed or height priority modes when calculating pitch angle
+ *  - Fallback mode when no airspeed measurement is available that
+ *    sets throttle based on height rate demand and switches pitch angle control to
+ *    height priority
+ *  - Underspeed protection that demands maximum throttle and switches pitch angle control
+ *    to speed priority mode
+ *  - Relative ease of tuning through use of intuitive time constant, integrator and damping gains and the use
+ *    of easy to measure aircraft performance data
+ *
+ */
+
+void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth)
+{
+	// Implement third order complementary filter for height and height rate
+	// estimted height rate = _integ2_state
+	// estimated height     = _integ3_state
+	// 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
+
+	// Calculate time in seconds since last update
+	uint64_t now = ecl_absolute_time();
+	float DT = max((now - _update_50hz_last_usec), 0ULL) * 1.0e-6f;
+
+	// printf("dt: %10.6f baro alt: %6.2f eas: %6.2f R(0,0): %6.2f, R(1,1): %6.2f\naccel body: %6.2f %6.2f %6.2f\naccel earth: %6.2f %6.2f %6.2f\n",
+	// 	DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2),
+	// 	accel_earth(0), accel_earth(1), accel_earth(2));
+
+	if (DT > 1.0f) {
+		_integ3_state = baro_altitude;
+		_integ2_state = 0.0f;
+		_integ1_state = 0.0f;
+		DT            = 0.02f; // when first starting TECS, use a
+		// small time constant
+	}
+
+	_update_50hz_last_usec = now;
+	_EAS = airspeed;
+
+	// Get height acceleration
+	float hgt_ddot_mea = -(accel_earth(2) + CONSTANTS_ONE_G);
+	// Perform filter calculation using backwards Euler integration
+	// Coefficients selected to place all three filter poles at omega
+	float omega2 = _hgtCompFiltOmega * _hgtCompFiltOmega;
+	float hgt_err = baro_altitude - _integ3_state;
+	float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega;
+	_integ1_state = _integ1_state + integ1_input * DT;
+	float integ2_input = _integ1_state + hgt_ddot_mea + hgt_err * omega2 * 3.0f;
+	_integ2_state = _integ2_state + integ2_input * DT;
+	float integ3_input = _integ2_state + hgt_err * _hgtCompFiltOmega * 3.0f;
+
+	// If more than 1 second has elapsed since last update then reset the integrator state
+	// to the measured height
+	if (DT > 1.0f) {
+		_integ3_state = baro_altitude;
+
+	} else {
+		_integ3_state = _integ3_state + integ3_input * DT;
+	}
+
+	// Update and average speed rate of change
+	// Only required if airspeed is being measured and controlled
+	float temp = 0;
+
+	if (isfinite(airspeed) && airspeed_sensor_enabled()) {
+		// Get DCM
+		// Calculate speed rate of change
+		// XXX check
+		temp = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0);
+		// take 5 point moving average
+		//_vel_dot = _vdot_filter.apply(temp);
+		// XXX resolve this properly
+		_vel_dot = 0.9f * _vel_dot + 0.1f * temp;
+
+	} else {
+		_vel_dot = 0.0f;
+	}
+
+}
+
+void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
+			 float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS)
+{
+	// Calculate time in seconds since last update
+	uint64_t now = ecl_absolute_time();
+	float DT = max((now - _update_speed_last_usec), 0ULL) * 1.0e-6f;
+	_update_speed_last_usec = now;
+
+	// Convert equivalent airspeeds to true airspeeds
+
+	_EAS_dem = airspeed_demand;
+	_TAS_dem  = _EAS_dem * EAS2TAS;
+	_TASmax   = indicated_airspeed_max * EAS2TAS;
+	_TASmin   = indicated_airspeed_min * EAS2TAS;
+
+	// Reset states of time since last update is too large
+	if (DT > 1.0f) {
+		_integ5_state = (_EAS * EAS2TAS);
+		_integ4_state = 0.0f;
+		DT            = 0.1f; // when first starting TECS, use a
+		// small time constant
+	}
+
+	// Get airspeed or default to halfway between min and max if
+	// airspeed is not being used and set speed rate to zero
+	if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) {
+		// If no airspeed available use average of min and max
+		_EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max);
+
+	} else {
+		_EAS = indicated_airspeed;
+	}
+
+	// Implement a second order complementary filter to obtain a
+	// smoothed airspeed estimate
+	// airspeed estimate is held in _integ5_state
+	float aspdErr = (_EAS * EAS2TAS) - _integ5_state;
+	float integ4_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega;
+
+	// Prevent state from winding up
+	if (_integ5_state < 3.1f) {
+		integ4_input = max(integ4_input , 0.0f);
+	}
+
+	_integ4_state = _integ4_state + integ4_input * DT;
+	float integ5_input = _integ4_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f;
+	_integ5_state = _integ5_state + integ5_input * DT;
+	// limit the airspeed to a minimum of 3 m/s
+	_integ5_state = max(_integ5_state, 3.0f);
+
+}
+
+void TECS::_update_speed_demand(void)
+{
+	// Set the airspeed demand to the minimum value if an underspeed condition exists
+	// or a bad descent condition exists
+	// This will minimise the rate of descent resulting from an engine failure,
+	// enable the maximum climb rate to be achieved and prevent continued full power descent
+	// into the ground due to an unachievable airspeed value
+	if ((_badDescent) || (_underspeed)) {
+		_TAS_dem     = _TASmin;
+	}
+
+	// Constrain speed demand
+	_TAS_dem = constrain(_TAS_dem, _TASmin, _TASmax);
+
+	// calculate velocity rate limits based on physical performance limits
+	// provision to use a different rate limit if bad descent or underspeed condition exists
+	// Use 50% of maximum energy rate to allow margin for total energy contgroller
+	float velRateMax;
+	float velRateMin;
+
+	if ((_badDescent) || (_underspeed)) {
+		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+
+	} else {
+		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+	}
+
+	// Apply rate limit
+	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
+		_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
+		_TAS_rate_dem = velRateMax;
+
+	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
+		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
+		_TAS_rate_dem = velRateMin;
+
+	} else {
+		_TAS_dem_adj = _TAS_dem;
+		_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
+	}
+
+	// Constrain speed demand again to protect against bad values on initialisation.
+	_TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax);
+	_TAS_dem_last = _TAS_dem;
+}
+
+void TECS::_update_height_demand(float demand)
+{
+	// Apply 2 point moving average to demanded height
+	// This is required because height demand is only updated at 5Hz
+	_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
+	_hgt_dem_in_old = _hgt_dem;
+
+	// printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
+	// 	_maxClimbRate);
+
+	// 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;
+	_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
+	_hgt_dem_adj_last = _hgt_dem_adj;
+
+	// printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
+	// 	_hgt_rate_dem);
+}
+
+void TECS::_detect_underspeed(void)
+{
+	if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) {
+		_underspeed = true;
+
+	} else {
+		_underspeed = false;
+	}
+}
+
+void TECS::_update_energies(void)
+{
+	// Calculate specific energy demands
+	_SPE_dem = _hgt_dem_adj * CONSTANTS_ONE_G;
+	_SKE_dem = 0.5f * _TAS_dem_adj * _TAS_dem_adj;
+
+	// Calculate specific energy rate demands
+	_SPEdot_dem = _hgt_rate_dem * CONSTANTS_ONE_G;
+	_SKEdot_dem = _integ5_state * _TAS_rate_dem;
+
+	// Calculate specific energy
+	_SPE_est = _integ3_state * CONSTANTS_ONE_G;
+	_SKE_est = 0.5f * _integ5_state * _integ5_state;
+
+	// Calculate specific energy rate
+	_SPEdot = _integ2_state * CONSTANTS_ONE_G;
+	_SKEdot = _integ5_state * _vel_dot;
+}
+
+void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
+{
+	// Calculate total energy values
+	_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
+	float STEdot_dem = constrain((_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 / (_timeConst * (_STEdot_max - _STEdot_min));
+
+		// Calculate feed-forward throttle
+		float ff_throttle = 0;
+		float nomThr = throttle_cruise;
+		// 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(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
+		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
+
+		if (STEdot_dem >= 0) {
+			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
+
+		} else {
+			ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
+		}
+
+		// 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 (fabsf(_throttle_slewrate) < 0.01f) {
+			float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
+
+			_throttle_dem = constrain(_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 dduring climbout
+		_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
+
+		if (_climbOutDem) {
+			_integ6_state = integ_max;
+
+		} else {
+			_integ6_state = constrain(_integ6_state, integ_min, integ_max);
+		}
+
+		// Sum the components.
+		// Only use feed-forward component if airspeed is not being used
+		if (airspeed_sensor_enabled()) {
+			_throttle_dem = _throttle_dem + _integ6_state;
+
+		} else {
+			_throttle_dem = ff_throttle;
+		}
+	}
+
+	// Constrain throttle demand
+	_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
+}
+
+void TECS::_detect_bad_descent(void)
+{
+	// Detect a demanded airspeed too high for the aircraft to achieve. This will be
+	// evident by the the following conditions:
+	// 1) Underspeed protection not active
+	// 2) Specific total energy error > 200 (greater than ~20m height error)
+	// 3) Specific total energy reducing
+	// 4) throttle demand > 90%
+	// If these four conditions exist simultaneously, then the protection
+	// mode will be activated.
+	// Once active, the following condition are required to stay in the mode
+	// 1) Underspeed protection not active
+	// 2) Specific total energy error > 0
+	// This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
+	float STEdot = _SPEdot + _SKEdot;
+
+	if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
+		_badDescent = true;
+
+	} else {
+		_badDescent = false;
+	}
+}
+
+void 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
+	// or during takeoff/climbout where a minimum pitch angle is set to ensure height is gained. In this instance, if airspeed
+	// rises above the demanded value, the pitch angle will be increased by the TECS controller.
+	float SKE_weighting = constrain(_spdWeight, 0.0f, 2.0f);
+
+	if ((_underspeed || _climbOutDem) && airspeed_sensor_enabled()) {
+		SKE_weighting = 2.0f;
+
+	} else if (!airspeed_sensor_enabled()) {
+		SKE_weighting = 0.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
+	float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
+	float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
+	_integ7_state = constrain(_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(_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 TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad)
+{
+	// Initialise states and variables if DT > 1 second or in climbout
+	if (_DT > 1.0f) {
+		_integ6_state      = 0.0f;
+		_integ7_state      = 0.0f;
+		_last_throttle_dem = throttle_cruise;
+		_last_pitch_dem    = pitch;
+		_hgt_dem_adj_last  = baro_altitude;
+		_hgt_dem_adj       = _hgt_dem_adj_last;
+		_hgt_dem_prev      = _hgt_dem_adj_last;
+		_hgt_dem_in_old    = _hgt_dem_adj_last;
+		_TAS_dem_last      = _TAS_dem;
+		_TAS_dem_adj       = _TAS_dem;
+		_underspeed        = false;
+		_badDescent        = false;
+		_DT                = 0.1f; // when first starting TECS, use a
+		// small time constant
+
+	} else if (_climbOutDem) {
+		_PITCHminf          = ptchMinCO_rad;
+		_THRminf            = _THRmaxf - 0.01f;
+		_hgt_dem_adj_last  = baro_altitude;
+		_hgt_dem_adj       = _hgt_dem_adj_last;
+		_hgt_dem_prev      = _hgt_dem_adj_last;
+		_TAS_dem_last      = _TAS_dem;
+		_TAS_dem_adj       = _TAS_dem;
+		_underspeed        = false;
+		_badDescent 	   = false;
+	}
+}
+
+void TECS::_update_STE_rate_lim(void)
+{
+	// Calculate Specific Total Energy Rate Limits
+	// This is a tivial calculation at the moment but will get bigger once we start adding altitude effects
+	_STEdot_max = _maxClimbRate * CONSTANTS_ONE_G;
+	_STEdot_min = - _minSinkRate * CONSTANTS_ONE_G;
+}
+
+void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
+				 float throttle_min, float throttle_max, float throttle_cruise,
+				 float pitch_limit_min, float pitch_limit_max)
+{
+	// Calculate time in seconds since last update
+	uint64_t now = ecl_absolute_time();
+	_DT = max((now - _update_pitch_throttle_last_usec), 0ULL) * 1.0e-6f;
+	_update_pitch_throttle_last_usec = now;
+
+	// printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n",
+	// 	_DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max);
+
+	// Update the speed estimate using a 2nd order complementary filter
+	_update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS);
+
+	// Convert inputs
+	_THRmaxf  = throttle_max;
+	_THRminf  = throttle_min;
+	_PITCHmaxf = pitch_limit_max;
+	_PITCHminf = pitch_limit_min;
+	_climbOutDem = climbOutDem;
+
+	// initialise selected states and variables if DT > 1 second or in climbout
+	_initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO);
+
+	// Calculate Specific Total Energy Rate Limits
+	_update_STE_rate_lim();
+
+	// Calculate the speed demand
+	_update_speed_demand();
+
+	// Calculate the height demand
+	_update_height_demand(hgt_dem);
+
+	// Detect underspeed condition
+	_detect_underspeed();
+
+	// Calculate specific energy quantitiues
+	_update_energies();
+
+	// Calculate throttle demand
+	_update_throttle(throttle_cruise, rotMat);
+
+	// 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      = _integ3_state;
+	// log_tuning.dhgt_dem = _hgt_rate_dem;
+	// log_tuning.dhgt     = _integ2_state;
+	// 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;
+}
diff --git a/src/lib/external_lgpl/tecs/TECS.h b/src/lib/external_lgpl/tecs/TECS.h
new file mode 100644
index 000000000..373215698
--- /dev/null
+++ b/src/lib/external_lgpl/tecs/TECS.h
@@ -0,0 +1,350 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file    TECS.h
+/// @brief   Combined Total Energy Speed & Height Control.
+
+/*
+ *  Written by Paul Riseborough 2013 to provide:
+ *  - Combined control of speed and height using throttle to control
+ *    total energy and pitch angle to control exchange of energy between
+ *    potential and kinetic.
+ *    Selectable speed or height priority modes when calculating pitch angle
+ *  - Fallback mode when no airspeed measurement is available that
+ *    sets throttle based on height rate demand and switches pitch angle control to
+ *    height priority
+ *  - Underspeed protection that demands maximum throttle switches pitch angle control
+ *    to speed priority mode
+ *  - Relative ease of tuning through use of intuitive time constant, trim rate and damping parameters and the use
+ *    of easy to measure aircraft performance data
+ */
+
+#ifndef TECS_H
+#define TECS_H
+
+#include <mathlib/mathlib.h>
+#include <stdint.h>
+
+class __EXPORT TECS
+{
+public:
+	TECS() :
+
+		_airspeed_enabled(false),
+		_throttle_slewrate(0.0f),
+		_climbOutDem(false),
+		_hgt_dem_prev(0.0f),
+		_hgt_dem_adj_last(0.0f),
+		_hgt_dem_in_old(0.0f),
+		_TAS_dem_last(0.0f),
+		_TAS_dem_adj(0.0f),
+		_TAS_dem(0.0f),
+		_integ1_state(0.0f),
+		_integ2_state(0.0f),
+		_integ3_state(0.0f),
+		_integ4_state(0.0f),
+		_integ5_state(0.0f),
+		_integ6_state(0.0f),
+		_integ7_state(0.0f),
+		_pitch_dem(0.0f),
+		_last_pitch_dem(0.0f),
+		_SPE_dem(0.0f),
+		_SKE_dem(0.0f),
+		_SPEdot_dem(0.0f),
+		_SKEdot_dem(0.0f),
+		_SPE_est(0.0f),
+		_SKE_est(0.0f),
+		_SPEdot(0.0f),
+		_SKEdot(0.0f) {
+
+	}
+
+	bool airspeed_sensor_enabled() {
+		return _airspeed_enabled;
+	}
+
+	void enable_airspeed(bool enabled) {
+		_airspeed_enabled = enabled;
+	}
+
+	// Update of the estimated height and height rate internal state
+	// Update of the inertial speed rate internal state
+	// Should be called at 50Hz or greater
+	void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth);
+
+	// Update the control loop calculations
+	void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
+				   float throttle_min, float throttle_max, float throttle_cruise,
+				   float pitch_limit_min, float pitch_limit_max);
+	// demanded throttle in percentage
+	// should return 0 to 100
+	float get_throttle_demand(void) {
+		return _throttle_dem;
+	}
+	int32_t get_throttle_demand_percent(void) {
+		return get_throttle_demand();
+	}
+
+
+	float get_pitch_demand() { return _pitch_dem; }
+
+	// demanded pitch angle in centi-degrees
+	// should return between -9000 to +9000
+	int32_t get_pitch_demand_cd() { return int32_t(get_pitch_demand() * 5729.5781f);}
+
+	// Rate of change of velocity along X body axis in m/s^2
+	float get_VXdot(void) { return _vel_dot; }
+
+	// log data on internal state of the controller. Called at 10Hz
+	// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
+
+	// struct PACKED log_TECS_Tuning {
+	// 	LOG_PACKET_HEADER;
+	// 	float hgt;
+	// 	float dhgt;
+	// 	float hgt_dem;
+	// 	float dhgt_dem;
+	// 	float spd_dem;
+	// 	float spd;
+	// 	float dspd;
+	// 	float ithr;
+	// 	float iptch;
+	// 	float thr;
+	// 	float ptch;
+	// 	float dspd_dem;
+	// } log_tuning;
+
+	void set_time_const(float time_const) {
+		_timeConst = time_const;
+	}
+
+	void set_min_sink_rate(float rate) {
+		_minSinkRate = rate;
+	}
+
+	void set_max_sink_rate(float sink_rate) {
+		_maxSinkRate = sink_rate;
+	}
+
+	void set_max_climb_rate(float climb_rate) {
+		_maxClimbRate = climb_rate;
+	}
+
+	void set_throttle_damp(float throttle_damp) {
+		_thrDamp = throttle_damp;
+	}
+
+	void set_integrator_gain(float gain) {
+		_integGain = gain;
+	}
+
+	void set_vertical_accel_limit(float limit) {
+		_vertAccLim = limit;
+	}
+
+	void set_height_comp_filter_omega(float omega) {
+		_hgtCompFiltOmega = omega;
+	}
+
+	void set_speed_comp_filter_omega(float omega) {
+		_spdCompFiltOmega = omega;
+	}
+
+	void set_roll_throttle_compensation(float compensation) {
+		_rollComp = compensation;
+	}
+
+	void set_speed_weight(float weight) {
+		_spdWeight = weight;
+	}
+
+	void set_pitch_damping(float damping) {
+		_ptchDamp = 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;
+	}
+
+private:
+	// Last time update_50Hz was called
+	uint64_t _update_50hz_last_usec;
+
+	// Last time update_speed was called
+	uint64_t _update_speed_last_usec;
+
+	// Last time update_pitch_throttle was called
+	uint64_t _update_pitch_throttle_last_usec;
+
+	// TECS tuning parameters
+	float _hgtCompFiltOmega;
+	float _spdCompFiltOmega;
+	float _maxClimbRate;
+	float _minSinkRate;
+	float _maxSinkRate;
+	float _timeConst;
+	float _ptchDamp;
+	float _thrDamp;
+	float _integGain;
+	float _vertAccLim;
+	float _rollComp;
+	float _spdWeight;
+
+	// throttle demand in the range from 0.0 to 1.0
+	float _throttle_dem;
+
+	// pitch angle demand in radians
+	float _pitch_dem;
+
+	// Integrator state 1 - height filter second derivative
+	float _integ1_state;
+
+	// Integrator state 2 - height rate
+	float _integ2_state;
+
+	// Integrator state 3 - height
+	float _integ3_state;
+
+	// Integrator state 4 - airspeed filter first derivative
+	float _integ4_state;
+
+	// Integrator state 5 - true airspeed
+	float _integ5_state;
+
+	// Integrator state 6 - throttle integrator
+	float _integ6_state;
+
+	// Integrator state 6 - pitch integrator
+	float _integ7_state;
+
+	// throttle demand rate limiter state
+	float _last_throttle_dem;
+
+	// pitch demand rate limiter state
+	float _last_pitch_dem;
+
+	// Rate of change of speed along X axis
+	float _vel_dot;
+
+	// Equivalent airspeed
+	float _EAS;
+
+	// True airspeed limits
+	float _TASmax;
+	float _TASmin;
+
+	// Current and last true airspeed demand
+	float _TAS_dem;
+	float _TAS_dem_last;
+
+	// Equivalent airspeed demand
+	float _EAS_dem;
+
+	// height demands
+	float _hgt_dem;
+	float _hgt_dem_in_old;
+	float _hgt_dem_adj;
+	float _hgt_dem_adj_last;
+	float _hgt_rate_dem;
+	float _hgt_dem_prev;
+
+	// Speed demand after application of rate limiting
+	// This is the demand tracked by the TECS control loops
+	float _TAS_dem_adj;
+
+	// Speed rate demand after application of rate limiting
+	// This is the demand tracked by the TECS control loops
+	float _TAS_rate_dem;
+
+	// Total energy rate filter state
+	float _STEdotErrLast;
+
+	// Underspeed condition
+	bool _underspeed;
+
+	// Bad descent condition caused by unachievable airspeed demand
+	bool _badDescent;
+
+	// climbout mode
+	bool _climbOutDem;
+
+	// throttle demand before limiting
+	float _throttle_dem_unc;
+
+	// pitch demand before limiting
+	float _pitch_dem_unc;
+
+	// Maximum and minimum specific total energy rate limits
+	float _STEdot_max;
+	float _STEdot_min;
+
+	// Maximum and minimum floating point throttle limits
+	float _THRmaxf;
+	float _THRminf;
+
+	// Maximum and minimum floating point pitch limits
+	float _PITCHmaxf;
+	float _PITCHminf;
+
+	// Specific energy quantities
+	float _SPE_dem;
+	float _SKE_dem;
+	float _SPEdot_dem;
+	float _SKEdot_dem;
+	float _SPE_est;
+	float _SKE_est;
+	float _SPEdot;
+	float _SKEdot;
+
+	// Specific energy error quantities
+	float _STE_error;
+
+	// Time since last update of main TECS loop (seconds)
+	float _DT;
+
+	bool _airspeed_enabled;
+	float _throttle_slewrate;
+	float _indicated_airspeed_min;
+	float _indicated_airspeed_max;
+
+	// Update the airspeed internal state using a second order complementary filter
+	void _update_speed(float airspeed_demand, float indicated_airspeed,
+			   float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS);
+
+	// Update the demanded airspeed
+	void _update_speed_demand(void);
+
+	// Update the demanded height
+	void _update_height_demand(float demand);
+
+	// Detect an underspeed condition
+	void _detect_underspeed(void);
+
+	// Update Specific Energy Quantities
+	void _update_energies(void);
+
+	// Update Demanded Throttle
+	void _update_throttle(float throttle_cruise, const math::Dcm &rotMat);
+
+	// Detect Bad Descent
+	void _detect_bad_descent(void);
+
+	// Update Demanded Pitch Angle
+	void _update_pitch(void);
+
+	// Initialise states and variables
+	void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad);
+
+	// Calculate specific total energy rate limits
+	void _update_STE_rate_lim(void);
+
+};
+
+#endif //TECS_H