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

13 files changed, 1501 insertions, 8 deletions

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index d0b5fcab7..44b339ce5 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -12,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name APL nor the names of its contributors may be
+ * 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.
  *
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
index 02ca62dad..7fbfd6fbc 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -12,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name APL nor the names of its contributors may be
+ * 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.
  *
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 6de30fc33..f3a8585ae 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -12,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name APL nor the names of its contributors may be
+ * 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.
  *
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
index 7fbcdf4b8..3427b67c2 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -12,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name APL nor the names of its contributors may be
+ * 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.
  *
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index a0f901e71..2b7429996 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -12,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name APL nor the names of its contributors may be
+ * 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.
  *
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index cfaa6c233..66b227918 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -12,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name APL nor the names of its contributors may be
+ * 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.
  *
diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h
index 2bd76ce97..e0f207696 100644
--- a/src/lib/ecl/ecl.h
+++ b/src/lib/ecl/ecl.h
@@ -38,6 +38,7 @@
  */
 
 #include <drivers/drv_hrt.h>
+#include <geo/geo.h>
 
 #define ecl_absolute_time hrt_absolute_time
 #define ecl_elapsed_time hrt_elapsed_time
\ No newline at end of file
diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.cpp b/src/lib/ecl/l1/ecl_l1_pos_control.cpp
new file mode 100644
index 000000000..533409217
--- /dev/null
+++ b/src/lib/ecl/l1/ecl_l1_pos_control.cpp
@@ -0,0 +1,323 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 vector_ABove copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the vector_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 MERCHANTvector_ABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIvector_ABLE 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 LIvector_ABILITY, WHETHER IN CONTRACT, STRICT
+ * LIvector_ABILITY, 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 ecl_l1_pos_control.h
+ * Implementation of L1 position control.
+ * Authors and acknowledgements in header.
+ *
+ */
+
+#include "ecl_l1_pos_control.h"
+
+float ECL_L1_Pos_Control::nav_roll()
+{
+	float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
+	ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f);
+	return ret;
+}
+
+float ECL_L1_Pos_Control::nav_lateral_acceleration_demand()
+{
+	return _lateral_accel;
+}
+
+float ECL_L1_Pos_Control::nav_bearing()
+{
+	return _wrap_pi(_nav_bearing);
+}
+
+float ECL_L1_Pos_Control::bearing_error()
+{
+	return _bearing_error;
+}
+
+float ECL_L1_Pos_Control::target_bearing()
+{
+	return _target_bearing;
+}
+
+float ECL_L1_Pos_Control::switch_distance(float wp_radius)
+{
+	return math::min(wp_radius, _L1_distance);
+}
+
+bool ECL_L1_Pos_Control::reached_loiter_target(void)
+{
+	return _circle_mode;
+}
+
+float ECL_L1_Pos_Control::crosstrack_error(void)
+{
+	return _crosstrack_error;
+}
+
+void ECL_L1_Pos_Control::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
+				       const math::Vector2f &ground_speed_vector)
+{
+
+	float eta;
+	float xtrack_vel;
+	float ltrack_vel;
+
+	/* get the direction between the last (visited) and next waypoint */
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY());
+
+	/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
+	float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
+
+	/* calculate the L1 length required for the desired period */
+	_L1_distance = _L1_ratio * ground_speed;
+
+	/* calculate vector from A to B */
+	math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
+
+	/*
+	 * check if waypoints are on top of each other. If yes,
+	 * skip A and directly continue to B
+	 */
+	if (vector_AB.length() < 1.0e-6f) {
+		vector_AB = get_local_planar_vector(vector_curr_position, vector_B);
+	}
+
+	vector_AB.normalize();
+
+	/* calculate the vector from waypoint A to the aircraft */
+	math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+
+	/* calculate crosstrack error (output only) */
+	_crosstrack_error = vector_AB % vector_A_to_airplane;
+
+	/*
+	 * If the current position is in a +-135 degree angle behind waypoint A
+	 * and further away from A than the L1 distance, then A becomes the L1 point.
+	 * If the aircraft is already between A and B normal L1 logic is applied.
+	 */
+	float distance_A_to_airplane = vector_A_to_airplane.length();
+	float alongTrackDist = vector_A_to_airplane * vector_AB;
+
+	if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) {
+
+		/* calculate eta to fly to waypoint A */
+
+		/* unit vector from waypoint A to current position */
+		math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+		/* velocity across / orthogonal to line */
+		xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
+		/* velocity along line */
+		ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
+		eta = atan2f(xtrack_vel, ltrack_vel);
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+
+	} else {
+
+		/* calculate eta to fly along the line between A and B */
+
+		/* velocity across / orthogonal to line */
+		xtrack_vel = ground_speed_vector % vector_AB;
+		/* velocity along line */
+		ltrack_vel = ground_speed_vector * vector_AB;
+		/* calculate eta2 (angle of velocity vector relative to line) */
+		float eta2 = atan2f(xtrack_vel, ltrack_vel);
+		/* calculate eta1 (angle to L1 point) */
+		float xtrackErr = vector_A_to_airplane % vector_AB;
+		float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f);
+		/* limit output to 45 degrees */
+		sine_eta1 = math::constrain(sine_eta1, -M_PI_F / 4.0f, +M_PI_F / 4.0f);
+		float eta1 = asinf(sine_eta1);
+		eta = eta1 + eta2;
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1;
+	}
+
+	/* limit angle to +-90 degrees */
+	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
+	_lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
+
+	/* flying to waypoints, not circling them */
+	_circle_mode = false;
+
+	/* the bearing angle, in NED frame */
+	_bearing_error = eta;
+}
+
+void ECL_L1_Pos_Control::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
+				       const math::Vector2f &ground_speed_vector)
+{
+
+	/* calculate the gains for the PD loop (circle tracking) */
+	float omega = (2.0f * M_PI_F / _L1_period);
+	float K_crosstrack = omega * omega;
+	float K_velocity = 2.0f * _L1_damping * omega;
+
+	/* update bearing to next waypoint */
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY());
+
+	/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
+	float ground_speed = math::max(ground_speed_vector.length() , 0.1f);
+
+	/* calculate the L1 length required for the desired period */
+	_L1_distance = _L1_ratio * ground_speed;
+
+	/* calculate the vector from waypoint A to current position */
+	math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+
+	/* store the normalized vector from waypoint A to current position */
+	math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
+
+	/* calculate eta angle towards the loiter center */
+
+	/* velocity across / orthogonal to line from waypoint to current position */
+	float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector;
+	/* velocity along line from waypoint to current position */
+	float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit);
+	float eta = atan2f(xtrack_vel_center, ltrack_vel_center);
+	/* limit eta to 90 degrees */
+	eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f);
+
+	/* calculate the lateral acceleration to capture the center point */
+	float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
+
+	/* for PD control: Calculate radial position and velocity errors */
+
+	/* radial velocity error */
+	float xtrack_vel_circle = -ltrack_vel_center;
+	/* radial distance from the loiter circle (not center) */
+	float xtrack_err_circle = vector_A_to_airplane.length() - radius;
+
+	/* cross track error for feedback */
+	_crosstrack_error = xtrack_err_circle;
+
+	/* calculate PD update to circle waypoint */
+	float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity);
+
+	/* calculate velocity on circle / along tangent */
+	float tangent_vel = xtrack_vel_center * loiter_direction;
+
+	/* prevent PD output from turning the wrong way */
+	if (tangent_vel < 0.0f) {
+		lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd , 0.0f);
+	}
+
+	/* calculate centripetal acceleration setpoint */
+	float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius) , (radius + xtrack_err_circle));
+
+	/* add PD control on circle and centripetal acceleration for total circle command */
+	float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal);
+
+	/*
+	 * Switch between circle (loiter) and capture (towards waypoint center) mode when
+	 * the commands switch over. Only fly towards waypoint if outside the circle.
+	 */
+
+	// XXX check switch over
+	if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) |
+		(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
+		_lateral_accel = lateral_accel_sp_center;
+		_circle_mode = false;
+		/* angle between requested and current velocity vector */
+		_bearing_error = eta;
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+
+	} else {
+		_lateral_accel = lateral_accel_sp_circle;
+		_circle_mode = true;
+		_bearing_error = 0.0f;
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+	}
+}
+
+
+void ECL_L1_Pos_Control::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector)
+{
+
+	float eta;
+
+	/* 
+	 * As the commanded heading is the only reference
+	 * (and no crosstrack correction occurs),
+	 * target and navigation bearing become the same
+	 */
+	_target_bearing = _nav_bearing = _wrap_pi(navigation_heading);
+	eta = _target_bearing - _wrap_pi(current_heading);
+	eta = _wrap_pi(eta);
+
+	/* consequently the bearing error is exactly eta: */
+	_bearing_error = eta; 
+
+	/* ground speed is the length of the ground speed vector */
+	float ground_speed = ground_speed_vector.length();
+
+	/* adjust L1 distance to keep constant frequency */
+	_L1_distance = ground_speed / _heading_omega;
+	float omega_vel = ground_speed * _heading_omega;
+
+	/* not circling a waypoint */
+	_circle_mode = false;
+
+	/* navigating heading means by definition no crosstrack error */
+	_crosstrack_error = 0;
+
+	/* limit eta to 90 degrees */
+	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
+	_lateral_accel = 2.0f * sinf(eta) * omega_vel;
+}
+
+void ECL_L1_Pos_Control::navigate_level_flight(float current_heading)
+{
+	/* reset all heading / error measures resulting in zero roll */
+	_target_bearing = current_heading;
+	_nav_bearing = current_heading;
+	_bearing_error = 0;
+	_crosstrack_error = 0;
+	_lateral_accel = 0;
+
+	/* not circling a waypoint when flying level */
+	_circle_mode = false;
+
+}
+
+
+math::Vector2f ECL_L1_Pos_Control::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const
+{
+	math::Vector2f out;
+
+	out.setX(math::radians((target.getX() - origin.getX())));
+	out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX()))));
+
+	return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
+}
+
diff --git a/src/lib/ecl/l1/ecl_l1_pos_control.h b/src/lib/ecl/l1/ecl_l1_pos_control.h
new file mode 100644
index 000000000..661651f08
--- /dev/null
+++ b/src/lib/ecl/l1/ecl_l1_pos_control.h
@@ -0,0 +1,236 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 ecl_l1_pos_control.h
+ * Implementation of L1 position control.
+ *
+ * @author Lorenz Meier
+ *
+ * Acknowledgements and References:
+ *
+ * Original publication:
+ *    S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
+ *    Proceedings of the AIAA Guidance, Navigation and Control
+ *    Conference, Aug 2004. AIAA-2004-4900.
+ *
+ * Original navigation logic modified by Paul Riseborough 2013:
+ *  - Explicit control over frequency and damping
+ *  - Explicit control over track capture angle
+ *  - Ability to use loiter radius smaller than L1 length
+ *  - Modified to use PD control for circle tracking to enable loiter radius less than L1 length
+ *  - Modified to enable period and damping of guidance loop to be set explicitly
+ *  - Modified to provide explicit control over capture angle
+ *
+ */
+
+#ifndef ECL_L1_POS_CONTROL_H
+#define ECL_L1_POS_CONTROL_H
+
+#include <mathlib/mathlib.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+
+/**
+ * L1 Nonlinear Guidance Logic
+ */
+class __EXPORT ECL_L1_Pos_Control
+{
+public:
+	ECL_L1_Pos_Control() {
+		_L1_period = 25;
+		_L1_damping = 0.75f;
+	}
+
+	/**
+	 * The current target bearing
+	 *
+	 * @return bearing angle (-pi..pi, in NED frame)
+	 */
+	float nav_bearing();
+
+	/**
+	 * Get lateral acceleration demand.
+	 *
+	 * @return Lateral acceleration in m/s^2
+	 */
+	float nav_lateral_acceleration_demand();
+
+	// return the heading error angle +ve to left of track
+
+
+	/**
+	 * Heading error.
+	 *
+	 * The heading error is either compared to the current track
+	 * or to the tangent of the current loiter radius.
+	 */
+	float bearing_error();
+
+
+	/**
+	 * Bearing from aircraft to current target.
+	 *
+	 * @return bearing angle (-pi..pi, in NED frame)
+	 */
+	float target_bearing();
+
+
+	/**
+	 * Get roll angle setpoint for fixed wing.
+	 *
+	 * @return Roll angle (in NED frame)
+	 */
+	float nav_roll();
+
+
+	/**
+	 * Get the current crosstrack error.
+	 *
+	 * @return Crosstrack error in meters.
+	 */
+	float crosstrack_error();
+
+
+	/**
+	 * Returns true if the loiter waypoint has been reached
+	 */
+	bool reached_loiter_target();
+
+
+	/**
+	 * Get the switch distance
+	 * 
+	 * This is the distance at which the system will
+	 * switch to the next waypoint. This depends on the
+	 * period and damping
+	 *
+	 * @param waypoint_switch_radius The switching radius the waypoint has set.
+	 */
+	float switch_distance(float waypoint_switch_radius);
+
+
+	/**
+	 * Navigate between two waypoints
+	 *
+	 * Calling this function with two waypoints results in the
+	 * control outputs to fly to the line segment defined by
+	 * the points and once captured following the line segment.
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
+			   const math::Vector2f &ground_speed);
+
+
+	/**
+	 * Navigate on an orbit around a loiter waypoint.
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
+			   const math::Vector2f &ground_speed_vector);
+
+
+	/**
+	 * Navigate on a fixed bearing.
+	 *
+	 * This only holds a certain direction and does not perform cross
+	 * track correction.
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed);
+
+
+	/**
+	 * Keep the wings level
+	 */
+	void navigate_level_flight(float current_heading);
+
+
+	/**
+	 * Set the L1 period.
+	 */
+	void set_l1_period(float period) {
+		_L1_period = period;
+		_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
+		/* calculate normalized frequency for heading tracking */
+		_heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period;
+	}
+
+
+	/**
+	 * Set the L1 damping factor.
+	 *
+	 * The original publication recommends a default of sqrt(2) / 2 = 0.707
+	 */
+	void set_l1_damping(float damping) {
+		_L1_damping = damping;
+		_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
+		_K_L1 = 4.0f * _L1_damping * _L1_damping;
+	}
+
+private:
+
+	float _lateral_accel;		///< Lateral acceleration setpoint in m/s^2
+	float _L1_distance;			///< L1 lead distance, defined by period and damping
+	bool _circle_mode;		///< flag for loiter mode
+	float _nav_bearing;		///< bearing to L1 reference point
+	float _bearing_error;		///< bearing error
+	float _crosstrack_error;	///< crosstrack error in meters
+	float _target_bearing;		///< the heading setpoint
+
+	float _L1_period;		///< L1 tracking period in seconds
+	float _L1_damping;		///< L1 damping ratio
+	float _L1_ratio;		///< L1 ratio for navigation
+	float _K_L1;			///< L1 control gain for _L1_damping
+	float _heading_omega;		///< Normalized frequency
+
+	/**
+	 * Convert a 2D vector from WGS84 to planar coordinates.
+	 *
+	 * This converts from latitude and longitude to planar
+	 * coordinates with (0,0) being at the position of ref and
+	 * returns a vector in meters towards wp.
+	 *
+	 * @param ref The reference position in WGS84 coordinates
+	 * @param wp The point to convert to into the local coordinates, in WGS84 coordinates
+	 * @return The vector in meters pointing from the reference position to the coordinates
+	 */
+	math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const;
+
+};
+
+
+#endif /* ECL_L1_POS_CONTROL_H */
diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk
index 19610872e..e8bca3c76 100644
--- a/src/lib/ecl/module.mk
+++ b/src/lib/ecl/module.mk
@@ -36,5 +36,6 @@
 #
 
 SRCS		 = 	attitude_fw/ecl_pitch_controller.cpp \
-				attitude_fw/ecl_roll_controller.cpp \
-				attitude_fw/ecl_yaw_controller.cpp
+			attitude_fw/ecl_roll_controller.cpp \
+			attitude_fw/ecl_yaw_controller.cpp \
+			l1/ecl_l1_pos_control.cpp
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