mtecs

6 files changed, 766 insertions, 33 deletions

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index d6c706b3b..1e8447aed 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -90,6 +90,7 @@
 #include <ecl/l1/ecl_l1_pos_controller.h>
 #include <external_lgpl/tecs/tecs.h>
 #include "landingslope.h"
+#include "mtecs/mTecs.h"
 
 
 /**
@@ -198,6 +199,8 @@ private:
 
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
+	fwPosctrl::mTecs				_mTecs;
+	bool						_was_pos_control_mode;
 
 	struct {
 		float l1_period;
@@ -331,11 +334,11 @@ private:
 	/**
 	 * Control position.
 	 */
-	bool		control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed,
+	bool		control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed,
 					 const struct position_setpoint_triplet_s &_pos_sp_triplet);
 
 	float calculate_target_airspeed(float airspeed_demand);
-	void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet);
+	void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet);
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -363,7 +366,8 @@ private:
 	void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
 			float pitch_min_rad, float pitch_max_rad,
 			float throttle_min, float throttle_max, float throttle_cruise,
-			bool climbout_mode, float climbout_pitch_min_rad);
+			bool climbout_mode, float climbout_pitch_min_rad,
+			const math::Vector<3> &ground_speed);
 
 };
 
@@ -426,7 +430,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_control_mode(),
 	_global_pos(),
 	_pos_sp_triplet(),
-	_sensor_combined()
+	_sensor_combined(),
+	_mTecs(),
+	_was_pos_control_mode(false)
 {
 	_nav_capabilities.turn_distance = 0.0f;
 
@@ -583,6 +589,9 @@ FixedwingPositionControl::parameters_update()
 	/* Update Launch Detector Parameters */
 	launchDetector.updateParams();
 
+	/* Update the mTecs */
+	_mTecs.updateParams();
+
 	return OK;
 }
 
@@ -714,7 +723,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
 }
 
 void
-FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
+FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 
 	if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
@@ -722,7 +731,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c
 		/* rotate ground speed vector with current attitude */
 		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
 		yaw_vector.normalize();
-		float ground_speed_body = yaw_vector * ground_speed;
+		float ground_speed_body = yaw_vector * ground_speed_2d;
 
 		/* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/
 		float distance = 0.0f;
@@ -764,12 +773,13 @@ void FixedwingPositionControl::navigation_capabilities_publish()
 }
 
 bool
-FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed,
+FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
 		const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 	bool setpoint = true;
 
-	calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet);
+	math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
+	calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
 
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
@@ -792,6 +802,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	// else integrators should be constantly reset.
 	if (_control_mode.flag_control_position_enabled) {
 
+		if (!_was_pos_control_mode) {
+			/* reset integrators */
+			if (_mTecs.getEnabled()) {
+				_mTecs.resetIntegrators();
+			}
+		}
+
+		_was_pos_control_mode = true;
+
 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
 
@@ -824,27 +843,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) {
 			/* waypoint is a plain navigation waypoint */
-			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
+			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
 			tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
 						math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-						false, math::radians(_parameters.pitch_limit_min));
+						false, math::radians(_parameters.pitch_limit_min), ground_speed);
 
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
 
 			/* waypoint is a loiter waypoint */
 			_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
-						  pos_sp_triplet.current.loiter_direction, ground_speed);
+						  pos_sp_triplet.current.loiter_direction, ground_speed_2d);
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
 			tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
 						math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-						false, math::radians(_parameters.pitch_limit_min));
+						false, math::radians(_parameters.pitch_limit_min), ground_speed);
 
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
 
@@ -869,7 +888,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 //					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
 
-				_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
+				_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d);
 
 				/* limit roll motion to prevent wings from touching the ground first */
 				_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
@@ -879,7 +898,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			} else {
 
 				/* normal navigation */
-				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
 			}
 
 			_att_sp.roll_body = _l1_control.nav_roll();
@@ -939,7 +958,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				tecs_update_pitch_throttle(flare_curve_alt, calculate_target_airspeed(airspeed_land), eas2tas,
 						flare_pitch_angle_rad, math::radians(15.0f),
 						0.0f, throttle_max, throttle_land,
-						false, flare_pitch_angle_rad);
+						false, flare_pitch_angle_rad, ground_speed);
 
 				if (!land_noreturn_vertical) {
 					mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
@@ -971,7 +990,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				tecs_update_pitch_throttle(altitude_desired, calculate_target_airspeed(airspeed_approach), eas2tas,
 						math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-						false, math::radians(_parameters.pitch_limit_min));
+						false, math::radians(_parameters.pitch_limit_min), ground_speed);
 			}
 
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
@@ -998,7 +1017,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				}
 			}
 
-			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
+			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
@@ -1012,7 +1031,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas,
 							math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 							_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)));
+							true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), ground_speed);
 
 					/* limit roll motion to ensure enough lift */
 					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
@@ -1021,7 +1040,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
 								math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 								_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								false, math::radians(_parameters.pitch_limit_min));
+								false, math::radians(_parameters.pitch_limit_min), ground_speed);
 				}
 
 			} else {
@@ -1055,6 +1074,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	} else if (0/* easy mode enabled */) {
 
+		_was_pos_control_mode = false;
+
 		/** EASY FLIGHT **/
 
 		if (0/* switched from another mode to easy */) {
@@ -1082,16 +1103,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					  (_parameters.airspeed_max - _parameters.airspeed_min) *
 					  _manual.throttle;
 
-		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
+		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed_2d);
 		_att_sp.roll_body = _l1_control.nav_roll();
 		_att_sp.yaw_body = _l1_control.nav_bearing();
 		tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas,
 					math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 					_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-					false, math::radians(_parameters.pitch_limit_min));
+					false, math::radians(_parameters.pitch_limit_min), ground_speed);
 
 	} else if (0/* seatbelt mode enabled */) {
 
+		_was_pos_control_mode = false;
+
 		/** SEATBELT FLIGHT **/
 
 		if (0/* switched from another mode to seatbelt */) {
@@ -1124,16 +1147,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			climb_out = true;
 		}
 
-		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
+		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed_2d);
 		_att_sp.roll_body =	_manual.roll;
 		_att_sp.yaw_body =	_manual.yaw;
 		tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas,
 					math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 					_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-					climb_out, math::radians(_parameters.pitch_limit_min));
+					climb_out, math::radians(_parameters.pitch_limit_min), ground_speed);
 
 	} else {
 
+		_was_pos_control_mode = false;
+
 		/** MANUAL FLIGHT **/
 
 		/* no flight mode applies, do not publish an attitude setpoint */
@@ -1150,9 +1175,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
 	}
 	else {
-		_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
+		_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max);
 	}
-	_att_sp.pitch_body = _tecs.get_pitch_demand();
+	_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
 
 	if (_control_mode.flag_control_position_enabled) {
 		last_manual = false;
@@ -1264,7 +1289,7 @@ FixedwingPositionControl::task_main()
 			vehicle_airspeed_poll();
 			// vehicle_baro_poll();
 
-			math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
+			math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e,  _global_pos.vel_d);
 			math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
 
 			/*
@@ -1329,13 +1354,25 @@ void FixedwingPositionControl::reset_landing_state()
 void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
 		float pitch_min_rad, float pitch_max_rad,
 		float throttle_min, float throttle_max, float throttle_cruise,
-		bool climbout_mode, float climbout_pitch_min_rad)
+		bool climbout_mode, float climbout_pitch_min_rad,
+		const math::Vector<3> &ground_speed)
 {
-	_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp,
-				    _airspeed.indicated_airspeed_m_s, eas2tas,
-				    climbout_mode, climbout_pitch_min_rad,
-				    throttle_min, throttle_max, throttle_cruise,
-				    pitch_min_rad, pitch_max_rad);
+	if (_mTecs.getEnabled()) {
+
+		float flightPathAngle = 0.0f;
+		float ground_speed_length = ground_speed.length();
+		if (ground_speed_length > FLT_EPSILON) {
+			flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
+		}
+		_mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp);
+
+	} else {
+		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp,
+					    _airspeed.indicated_airspeed_m_s, eas2tas,
+					    climbout_mode, climbout_pitch_min_rad,
+					    throttle_min, throttle_max, throttle_cruise,
+					    pitch_min_rad, pitch_max_rad);
+	}
 }
 
 int
diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk
index cf419ec7e..5d752437f 100644
--- a/src/modules/fw_pos_control_l1/module.mk
+++ b/src/modules/fw_pos_control_l1/module.mk
@@ -39,4 +39,6 @@ MODULE_COMMAND	= fw_pos_control_l1
 
 SRCS		= fw_pos_control_l1_main.cpp \
 		  fw_pos_control_l1_params.c \
-		  landingslope.cpp
+		  landingslope.cpp \
+		  mtecs/mTecs.cpp\
+		  mtecs/mTecs_params.c
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
new file mode 100644
index 000000000..ad108e7ee
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+
+/**
+ * @file mTecs.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "mTecs.h"
+
+#include <lib/geo/geo.h>
+#include <systemlib/err.h>
+
+namespace fwPosctrl {
+
+mTecs::mTecs() :
+	SuperBlock(NULL, "MT"),
+	/* Parameters */
+	_mTecsEnabled(this, "ENABLED"),
+	/* control blocks */
+	_controlTotalEnergy(this, "THR"),
+	_controlEnergyDistribution(this, "PIT", true),
+	_controlAltitude(this, "FPA", true),
+	_controlAirSpeed(this, "ACC"),
+	_airspeedDerivative(this, "AD"),
+	_throttleSp(0.0f),
+	_pitchSp(0.0f),
+	timestampLastIteration(hrt_absolute_time()),
+	_firstIterationAfterReset(true)
+{
+}
+
+mTecs::~mTecs()
+{
+}
+
+void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp)
+{
+	warnx("***");
+	float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude);
+	warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp);
+	updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp);
+}
+
+void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp) {
+
+	float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed);
+	warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp);
+	updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp);
+}
+
+void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp)
+{
+	/* time measurement */
+	float deltaTSeconds = 0.0f;
+	if (!_firstIterationAfterReset) {
+		hrt_abstime timestampNow = hrt_absolute_time();
+		deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f;
+		timestampLastIteration = timestampNow;
+	}
+	setDt(deltaTSeconds);
+
+	/* update parameters first */
+	updateParams();
+
+	/* calculate values (energies) */
+	float flightPathAngleError = flightPathAngleSp - flightPathAngle;
+	float airspeedDerivative = 0.0f;
+	if(_airspeedDerivative.getDt() > 0.0f) {
+		airspeedDerivative = _airspeedDerivative.update(airspeed);
+	}
+	float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G;
+	float airspeedSpDerivative = accelerationLongitudinalSp;
+	float airspeedSpDerivativeNorm = airspeedSpDerivative / CONSTANTS_ONE_G;
+	float airspeedSpDerivativeNormError = airspeedSpDerivativeNorm - airspeedDerivativeNorm;
+
+	float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm;
+	float totalEnergyRateError = flightPathAngleError + airspeedSpDerivativeNormError;
+	float totalEnergyRateSp = flightPathAngleSp + airspeedSpDerivativeNorm;
+	float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate;
+
+	float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm;
+	float energyDistributionRateError = flightPathAngleError - airspeedSpDerivativeNormError;
+	float energyDistributionRateSp = flightPathAngleSp - airspeedSpDerivativeNorm;
+	float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate;
+
+	warnx("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f",
+			(double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm);
+	warnx("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f",
+			(double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2);
+
+	/** update control blocks **/
+	/* update total energy rate control block */
+	_throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError);
+
+	/* update energy distribution rate control block */
+	_pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError);
+
+	warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f",
+			(double)_throttleSp, (double)_pitchSp,
+			(double)flightPathAngleSp, (double)flightPathAngle,
+			(double)accelerationLongitudinalSp, (double)airspeedDerivative);
+
+
+	/* clean up */
+	_firstIterationAfterReset = false;
+
+}
+
+void mTecs::resetIntegrators()
+{
+	_controlTotalEnergy.getIntegral().setY(0.0f);
+	_controlEnergyDistribution.getIntegral().setY(0.0f);
+	timestampLastIteration = hrt_absolute_time();
+	_firstIterationAfterReset = true;
+}
+
+} /* namespace fwPosctrl */
+
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
new file mode 100644
index 000000000..5877a8312
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+
+/**
+ * @file mTecs.h
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+
+#ifndef MTECS_H_
+#define MTECS_H_
+
+#include "mTecs_blocks.h"
+
+#include <controllib/block/BlockParam.hpp>
+#include <drivers/drv_hrt.h>
+
+namespace fwPosctrl
+{
+
+/* Main class of the mTecs */
+class mTecs : public control::SuperBlock
+{
+public:
+	mTecs();
+	virtual ~mTecs();
+
+	/*
+	 * Control in altitude setpoint and speed mode
+	 */
+	void updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp);
+
+	/*
+	 * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode
+	 */
+	void updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp);
+
+	/*
+	 * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
+	 */
+	void updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp);
+
+	/*
+	 * Reset all integrators
+	 */
+	void resetIntegrators();
+
+
+	/* Accessors */
+	bool getEnabled() {return _mTecsEnabled.get() > 0;}
+	float getThrottleSetpoint() {return _throttleSp;}
+	float getPitchSetpoint() {return _pitchSp;}
+
+protected:
+	/* parameters */
+	control::BlockParamInt _mTecsEnabled;		/**< 1 if mTecs is enabled */
+
+	/* control blocks */
+	BlockFFPILimited _controlTotalEnergy;		/**< FFPI controller for total energy control: output is throttle */
+	BlockFFPILimited _controlEnergyDistribution;	/**< FFPI controller for energy distribution control: output is pitch */
+	BlockPLimited	_controlAltitude;		/**< P controller for altitude: output is the flight path angle setpoint */
+	BlockPLimited	_controlAirSpeed;			/**< P controller for airspeed: output is acceleration setpoint */
+
+	/* Other calculation Blocks */
+	control::BlockDerivative _airspeedDerivative;
+
+	/* Output setpoints */
+	float _throttleSp;				/**< Throttle Setpoint from 0 to 1 */
+	float _pitchSp;					/**< Pitch Setpoint from -pi to pi */
+
+	/* Time measurements */
+	hrt_abstime timestampLastIteration;		/**< Saves the result of hrt_absolute_time() of the last iteration */
+
+	bool _firstIterationAfterReset;		/**< True during the first iteration after a reset */
+
+};
+
+} /* namespace fwPosctrl */
+
+#endif /* MTECS_H_ */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
new file mode 100644
index 000000000..7dd5c7c2e
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -0,0 +1,198 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+
+/**
+ * @file mTecs_blocks.h
+ *
+ * Custom blocks for the mTecs
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#pragma once
+
+#include <controllib/blocks.hpp>
+#include <systemlib/err.h>
+
+namespace fwPosctrl
+{
+
+using namespace control;
+
+/* Integrator without limit */
+class BlockIntegralNoLimit: public SuperBlock
+{
+public:
+// methods
+	BlockIntegralNoLimit(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_y(0) {};
+	virtual ~BlockIntegralNoLimit() {};
+	float update(float input) {
+		setY(getY() + input * getDt());
+		return getY();
+	};
+// accessors
+	float getY() {return _y;}
+	void setY(float y) {_y = y;}
+protected:
+// attributes
+	float _y; /**< previous output */
+};
+
+/* An block which can be used to limit the output */
+class BlockOutputLimiter: public SuperBlock
+{
+public:
+// methods
+	BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+		SuperBlock(parent, name),
+		_isAngularLimit(isAngularLimit),
+		_min(this, "MIN"),
+		_max(this, "MAX")
+	{};
+	virtual ~BlockOutputLimiter() {};
+	bool limit(float& value, float& difference) {
+		float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
+		float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
+		char name[blockNameLengthMax];
+		getName(name, blockNameLengthMax);
+		warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u",
+				 name,(double)minimum,(double)maximum,  (double)getMin(), (double)getMax(), isAngularLimit());
+		if (value < minimum) {
+			difference = value - minimum;
+			value = minimum;
+			return true;
+		} else if (value > maximum) {
+			difference = value - maximum;
+			value = maximum;
+			return true;
+		}
+		return false;
+	}
+//accessor:
+	bool isAngularLimit() {return _isAngularLimit ;}
+	float getMin() { return _min.get(); }
+	float getMax() { return _max.get(); }
+protected:
+//attributes
+	bool _isAngularLimit;
+	control::BlockParamFloat _min;
+	control::BlockParamFloat _max;
+};
+
+typedef
+
+/* A combination of feed forward, P and I gain  using the output limiter*/
+class BlockFFPILimited: public SuperBlock
+{
+public:
+// methods
+	BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+		SuperBlock(parent, name),
+		_integral(this, "I"),
+		_kFF(this, "FF"),
+		_kP(this, "P"),
+		_kI(this, "I"),
+		_offset(this, "OFF"),
+		_outputLimiter(this, "", isAngularLimit)
+	{};
+	virtual ~BlockFFPILimited() {};
+	float update(float inputValue, float inputError) {
+		float difference = 0.0f;
+		float integralYPrevious = _integral.getY();
+		float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);
+		char name[blockNameLengthMax];
+		getName(name, blockNameLengthMax);
+		warnx("%s output %.2f getKFF() %.2f, inputValue  %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name,
+				(double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt());
+		if(!getOutputLimiter().limit(output, difference) &&
+			(((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) ||
+			((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) {
+				getIntegral().setY(integralYPrevious);
+		}
+		warnx("%s output limited %.2f",
+				 name,(double)output);
+		return output;
+	}
+// accessors
+	BlockIntegralNoLimit &getIntegral() { return _integral; }
+	float getKFF() { return _kFF.get(); }
+	float getKP() { return _kP.get(); }
+	float getKI() { return _kI.get(); }
+	float getOffset() { return _offset.get(); }
+	BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
+private:
+	BlockIntegralNoLimit _integral;
+	BlockParamFloat _kFF;
+	BlockParamFloat _kP;
+	BlockParamFloat _kI;
+	BlockParamFloat _offset;
+	BlockOutputLimiter _outputLimiter;
+};
+
+/* A combination of P gain and output limiter */
+class BlockPLimited: public SuperBlock
+{
+public:
+// methods
+	BlockPLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+		SuperBlock(parent, name),
+		_kP(this, "P"),
+		_outputLimiter(this, "", isAngularLimit)
+	{};
+	virtual ~BlockPLimited() {};
+	float update(float input) {
+		float difference = 0.0f;
+		float output = getKP() * input;
+		char name[blockNameLengthMax];
+		getName(name, blockNameLengthMax);
+		warnx("%s output %.2f _kP.get() %.2f, input",
+				 name,(double)output, (double)_kP.get(), (double)input);
+		getOutputLimiter().limit(output, difference);
+		warnx("%s output limited %.2f",
+				 name,(double)output);
+		return output;
+	}
+// accessors
+	BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
+	float getKP() { return _kP.get(); }
+private:
+	control::BlockParamFloat _kP;
+	BlockOutputLimiter _outputLimiter;
+};
+
+}
+
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
new file mode 100644
index 000000000..76ed95303
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -0,0 +1,231 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+
+/**
+ * @file mTecs_params.c
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+/*
+ * Controller parameters, accessible via MAVLink
+ */
+
+/**
+ * mTECS enabled
+ *
+ * Set to 1 to enable mTECS
+ *
+ * @min 0
+ * @max 1
+ * @group mTECS
+ */
+PARAM_DEFINE_INT32(MT_ENABLED, 0);
+
+/**
+ * Total Energy Rate Control FF
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_FF, 1.0f);
+
+/**
+ * Total Energy Rate Control P
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f);
+
+/**
+ * Total Energy Rate Control I
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_I, 0.0f);
+
+/**
+ * Total Energy Rate Control OFF (Cruise throttle)
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f);
+
+/**
+ * Energy Distribution Rate Control FF
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_FF, 1.0f);
+
+/**
+ * Energy Distribution Rate Control P
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_P, 0.1f);
+
+/**
+ * Energy Distribution Rate Control I
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_I, 0.0f);
+
+
+/**
+ * Total Energy Distribution OFF (Cruise pitch sp)
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_OFF, 0.0f);
+
+/**
+ * Minimal Throttle Setpoint
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_MIN, 0.0f);
+
+/**
+ * Maximal Throttle Setpoint
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_MAX, 1.0f);
+
+/**
+ * Minimal Pitch Setpoint in Degrees
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
+
+/**
+ * Maximal Pitch Setpoint in Degrees
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_MAX, 45.0f);
+
+/**
+ * P gain for the altitude control
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_P, 0.1f);
+
+/**
+ * Minimal flight path angle setpoint
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_MIN, -30.0f);
+
+/**
+ * Maximal flight path angle setpoint
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
+
+
+/**
+ * P gain for the airspeed control
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_P, 0.1f);
+
+/**
+ * Minimal acceleration (air)
+ *
+ * @unit m/s^2
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_MIN, 0.0f);
+
+/**
+ * Maximal acceleration (air)
+ *
+* @unit m/s^2
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
+
+/**
+ * Airspeed derivative lowpass
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f);