Merge remote-tracking branch 'px4/mtecs' into navigator_rewrite

14 files changed, 300 insertions, 43 deletions

diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 810e008ae..e4bc6fecf 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -52,7 +52,6 @@ MODULES		+= systemcmds/pwm
 MODULES		+= systemcmds/esc_calib
 MODULES		+= systemcmds/reboot
 MODULES		+= systemcmds/top
-MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/dumpfile
@@ -67,12 +66,11 @@ MODULES		+= modules/mavlink
 MODULES		+= modules/gpio_led
 
 #
-# Estimation modules (EKF/ SO3 / other filters)
+# Estimation modules (EKF / other filters)
 #
 MODULES		+= modules/attitude_estimator_ekf
 MODULES		+= modules/ekf_att_pos_estimator
 MODULES		+= modules/position_estimator_inav
-#MODULES		+= examples/flow_position_estimator
 
 #
 # Vehicle Control
@@ -81,8 +79,6 @@ MODULES		+= modules/fw_pos_control_l1
 MODULES		+= modules/fw_att_control
 MODULES		+= modules/mc_att_control
 MODULES		+= modules/mc_pos_control
-#MODULES		+= examples/flow_position_control
-#MODULES		+= examples/flow_speed_control
 
 #
 # Logging
diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk
index de0174e37..d92f0bb45 100644
--- a/src/lib/launchdetection/module.mk
+++ b/src/lib/launchdetection/module.mk
@@ -38,3 +38,5 @@
 SRCS		 = 	LaunchDetector.cpp \
 			CatapultLaunchMethod.cpp \
 			launchdetection_params.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 25272051b..abe272e56 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -74,6 +74,7 @@
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/home_position.h>
+#include <uORB/topics/wind_estimate.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
@@ -158,6 +159,7 @@ private:
 	orb_advert_t	_global_pos_pub;		/**< global position */
 	orb_advert_t	_local_pos_pub;			/**< position in local frame */
 	orb_advert_t	_estimator_status_pub;		/**< status of the estimator */
+	orb_advert_t	_wind_pub;			/**< wind estimate */
 
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
 	struct gyro_report				_gyro;
@@ -169,6 +171,7 @@ private:
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
 	struct vehicle_local_position_s			_local_pos;		/**< local vehicle position */
 	struct vehicle_gps_position_s			_gps;			/**< GPS position */
+	struct wind_estimate_s				_wind;			/**< Wind estimate */
 
 	struct gyro_scale				_gyro_offsets;
 	struct accel_scale				_accel_offsets;
@@ -316,6 +319,7 @@ FixedwingEstimator::FixedwingEstimator() :
 	_global_pos_pub(-1),
 	_local_pos_pub(-1),
 	_estimator_status_pub(-1),
+	_wind_pub(-1),
 
 	_att({}),
 	_gyro({}),
@@ -327,6 +331,7 @@ FixedwingEstimator::FixedwingEstimator() :
 	_global_pos({}),
 	_local_pos({}),
 	_gps({}),
+	_wind({}),
 
 	_gyro_offsets({}),
 	_accel_offsets({}),
@@ -1351,13 +1356,31 @@ FixedwingEstimator::task_main()
 
 				/* lazily publish the global position only once available */
 				if (_global_pos_pub > 0) {
-					/* publish the attitude setpoint */
+					/* publish the global position */
 					orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
 
 				} else {
 					/* advertise and publish */
 					_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
 				}
+
+				if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
+					_wind.timestamp = _global_pos.timestamp;
+					_wind.windspeed_north = _ekf->states[14];
+					_wind.windspeed_east = _ekf->states[15];
+					_wind.covariance_north = 0.0f; // XXX get form filter
+					_wind.covariance_east = 0.0f;
+
+					/* lazily publish the wind estimate only once available */
+					if (_wind_pub > 0) {
+						/* publish the wind estimate */
+						orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
+
+					} else {
+						/* advertise and publish */
+						_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
+					}
+				}
 			}
 
 		}
@@ -1419,9 +1442,11 @@ FixedwingEstimator::print_status()
 	printf("states (vel m/s)     [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
 	printf("states (pos m)      [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
 	printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
-	printf("states (wind)      [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
-	printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
-	printf("states (body mag)  [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
+	printf("states (accel offs)   [14]: %8.4f\n", (double)_ekf->states[13]);
+	printf("states (wind)      [15-16]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]);
+	printf("states (earth mag) [17-19]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]);
+	printf("states (body mag)  [20-22]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]);
+	printf("states (terrain)      [23]: %8.4f\n", (double)_ekf->states[22]);
 	printf("states: %s %s %s %s %s %s %s %s %s %s\n",
 	       (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
 	       (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp
index 5db1adbb3..1320b4668 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator.cpp
@@ -2042,10 +2042,10 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max)
     float ret;
     if (val > max) {
         ret = max;
-        ekf_debug("> max: %8.4f, val: %8.4f", max, val);
+        ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val);
     } else if (val < min) {
         ret = min;
-        ekf_debug("< min: %8.4f, val: %8.4f", min, val);
+        ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val);
     } else {
         ret = val;
     }
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 31b3b3066..070d7377a 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
@@ -380,7 +380,7 @@ private:
 			bool climbout_mode, float climbout_pitch_min_rad,
 			float altitude,
 			const math::Vector<3> &ground_speed,
-			fwPosctrl::mTecs::tecs_mode mode = fwPosctrl::mTecs::TECS_MODE_NORMAL);
+			tecs_mode mode = TECS_MODE_NORMAL);
 
 };
 
@@ -835,7 +835,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
 	math::Vector<3> accel_earth = _R_nb * accel_body;
 
-	_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
+	if (!_mTecs.getEnabled()) {
+		_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
+	}
+
 	float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
 
 	/* no throttle limit as default */
@@ -851,6 +854,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			/* reset integrators */
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
+				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
 			}
 		}
 
@@ -1008,7 +1012,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						0.0f, throttle_max, throttle_land,
 						false, flare_pitch_angle_rad,
 						_pos_sp_triplet.current.alt + relative_alt, ground_speed,
-						land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND);
+						land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
 
 				if (!land_noreturn_vertical) {
 					mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
@@ -1101,7 +1105,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 							math::radians(10.0f)),
 							_global_pos.alt,
 							ground_speed,
-							fwPosctrl::mTecs::TECS_MODE_TAKEOFF);
+							TECS_MODE_TAKEOFF);
 
 					/* 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));
@@ -1439,16 +1443,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		bool climbout_mode, float climbout_pitch_min_rad,
 		float altitude,
 		const math::Vector<3> &ground_speed,
-		fwPosctrl::mTecs::tecs_mode mode)
+		tecs_mode mode)
 {
 	if (_mTecs.getEnabled()) {
+		/* Using mtecs library: prepare arguments for mtecs call */
 		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, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode);
+		fwPosctrl::mTecs::LimitOverride limitOverride;
+		if (climbout_mode) {
+			limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
+		} else {
+			limitOverride.disablePitchMinOverride();
+		}
+		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
+				limitOverride);
 	} else {
+		/* Using tecs library */
 		_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
 					    _airspeed.indicated_airspeed_m_s, eas2tas,
 					    climbout_mode, climbout_pitch_min_rad,
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 087e1b476..32f9f19ca 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -79,7 +79,8 @@ mTecs::~mTecs()
 {
 }
 
-int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode)
+int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
+		float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
 	if (!isfinite(flightPathAngle) || !isfinite(altitude) ||
@@ -105,10 +106,12 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 
 
 	/* use flightpath angle setpoint for total energy control */
-	return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode);
+	return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed,
+			airspeedSp, mode, limitOverride);
 }
 
-int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode)
+int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
+		float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
 	if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
@@ -135,10 +138,12 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 
 
 	/* use longitudinal acceleration setpoint for total energy control */
-	return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode);
+	return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed,
+			accelerationLongitudinalSp, mode, limitOverride);
 }
 
-int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode)
+int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed,
+		float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
 	if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
@@ -180,8 +185,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 				(double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2);
 	}
 
-	/* Check airspeed: if below safe value switch to underspeed mode */
-	if (airspeed < _airspeedMin.get()) {
+	/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
+	if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeed < _airspeedMin.get()) {
 		mode = TECS_MODE_UNDERSPEED;
 	}
 
@@ -202,6 +207,16 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 		outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
 	}
 
+	/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
+	 * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
+	 * is running) */
+	bool limitApplied = limitOverride.applyOverride(outputLimiterThrottle == NULL ?
+			_controlTotalEnergy.getOutputLimiter() :
+			*outputLimiterThrottle,
+			outputLimiterPitch == NULL ?
+			_controlEnergyDistribution.getOutputLimiter() :
+			*outputLimiterPitch);
+
 	/* Write part of the status message */
 	_status.airspeedDerivativeSp = airspeedDerivativeSp;
 	_status.airspeedDerivative = airspeedDerivative;
@@ -209,6 +224,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	_status.totalEnergyRate = totalEnergyRate;
 	_status.energyDistributionRateSp = energyDistributionRateSp;
 	_status.energyDistributionRate = energyDistributionRate;
+	_status.mode = mode;
 
 	/** update control blocks **/
 	/* update total energy rate control block */
@@ -245,6 +261,12 @@ void mTecs::resetIntegrators()
 	_firstIterationAfterReset = true;
 }
 
+void mTecs::resetDerivatives(float airspeed)
+{
+	_airspeedDerivative.setU(airspeed);
+}
+
+
 void mTecs::updateTimeMeasurement()
 {
 	if (!_dtCalculated) {
@@ -280,5 +302,29 @@ void mTecs::debug(const char *fmt, ...) {
 	debug_print(fmt, args);
 }
 
-} /* namespace fwPosctrl */
+bool mTecs::LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
+		BlockOutputLimiter &outputLimiterPitch)
+{
+	bool ret = false;
 
+	if (overrideThrottleMinEnabled)	{
+		outputLimiterThrottle.setMin(overrideThrottleMin);
+		ret = true;
+	}
+	if (overrideThrottleMaxEnabled)	{
+		outputLimiterThrottle.setMax(overrideThrottleMax);
+		ret = true;
+	}
+	if (overridePitchMinEnabled)	{
+		outputLimiterPitch.setMin(overridePitchMin);
+		ret = true;
+	}
+	if (overridePitchMaxEnabled)	{
+		outputLimiterPitch.setMax(overridePitchMax);
+		ret = true;
+	}
+
+	return ret;
+}
+
+} /* namespace fwPosctrl */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index 376d39698..ddd6583e8 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -60,34 +60,89 @@ public:
 	mTecs();
 	virtual ~mTecs();
 
-	typedef enum {
-		TECS_MODE_NORMAL,
-		TECS_MODE_UNDERSPEED,
-		TECS_MODE_TAKEOFF,
-		TECS_MODE_LAND,
-		TECS_MODE_LAND_THROTTLELIM
-	} tecs_mode;
+	/* A small class which provides helper fucntions to override control output limits which are usually set by
+	 * parameters in special cases
+	 */
+	class LimitOverride
+	{
+	public:
+		LimitOverride() :
+			overrideThrottleMinEnabled(false),
+			overrideThrottleMaxEnabled(false),
+			overridePitchMinEnabled(false),
+			overridePitchMaxEnabled(false)
+		{};
+
+		~LimitOverride() {};
+
+		/*
+		 * Override the limits of the outputlimiter instances given by the arguments with the limits saved in
+		 * this class (if enabled)
+		 * @return true if the limit was applied
+		 */
+		bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
+				BlockOutputLimiter &outputLimiterPitch);
+
+		/* Functions to enable or disable the override */
+		void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled,
+				&overrideThrottleMin, value); }
+		void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
+		void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled,
+				&overrideThrottleMax, value); }
+		void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
+		void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled,
+				&overridePitchMin, value); }
+		void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
+		void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled,
+				&overridePitchMax, value); }
+		void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
+
+	protected:
+		bool overrideThrottleMinEnabled;
+		float overrideThrottleMin;
+		bool overrideThrottleMaxEnabled;
+		float overrideThrottleMax;
+		bool overridePitchMinEnabled;
+		float overridePitchMin; //in degrees (replaces param values)
+		bool overridePitchMaxEnabled;
+		float overridePitchMax; //in degrees (replaces param values)
+
+		/* Enable a specific limit override */
+		void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value;
+		};
+		/* Disable a specific limit override */
+		void disable(bool *flag) { *flag = false; };
+
+
+	};
 
 	/*
 	 * Control in altitude setpoint and speed mode
 	 */
-	int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode);
+	int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
+			float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
 
 	/*
 	 * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode
 	 */
-	int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode);
+	int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
+			float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
 
 	/*
 	 * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
 	 */
-	int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode);
+	int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed,
+			float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride);
 
 	/*
 	 * Reset all integrators
 	 */
 	void resetIntegrators();
 
+	/*
+	 * Reset all derivative calculations
+	 */
+	void resetDerivatives(float airspeed);
 
 	/* Accessors */
 	bool getEnabled() {return _mTecsEnabled.get() > 0;}
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
index a7acd95de..e4e405227 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -89,6 +89,8 @@ public:
 	bool isAngularLimit() {return _isAngularLimit ;}
 	float getMin() { return _min.get(); }
 	float getMax() { return _max.get(); }
+	void setMin(float value) { _min.set(value); }
+	void setMax(float value) { _max.set(value); }
 protected:
 //attributes
 	bool _isAngularLimit;
@@ -96,7 +98,6 @@ protected:
 	control::BlockParamFloat _max;
 };
 
-typedef
 
 /* A combination of feed forward, P and I gain using the output limiter*/
 class BlockFFPILimited: public SuperBlock
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
index 591257611..39514b223 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -58,7 +58,8 @@
 PARAM_DEFINE_INT32(MT_ENABLED, 1);
 
 /**
- * Total Energy Rate Control FF
+ * Total Energy Rate Control Feedforward
+ * Maps the total energy rate setpoint to the throttle setpoint
  *
  * @min 0.0
  * @max 10.0
@@ -68,6 +69,7 @@ PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f);
 
 /**
  * Total Energy Rate Control P
+ * Maps the total energy rate error to the throttle setpoint
  *
  * @min 0.0
  * @max 10.0
@@ -77,6 +79,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f);
 
 /**
  * Total Energy Rate Control I
+ * Maps the integrated total energy rate to the throttle setpoint
  *
  * @min 0.0
  * @max 10.0
@@ -85,7 +88,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f);
 PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f);
 
 /**
- * Total Energy Rate Control OFF (Cruise throttle)
+ * Total Energy Rate Control Offset (Cruise throttle sp)
  *
  * @min 0.0
  * @max 10.0
@@ -94,7 +97,8 @@ PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f);
 PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f);
 
 /**
- * Energy Distribution Rate Control FF
+ * Energy Distribution Rate Control Feedforward
+ * Maps the energy distribution rate setpoint to the pitch setpoint
  *
  * @min 0.0
  * @max 10.0
@@ -104,6 +108,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.1f);
 
 /**
  * Energy Distribution Rate Control P
+ * Maps the energy distribution rate error to the pitch setpoint
  *
  * @min 0.0
  * @max 10.0
@@ -113,6 +118,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f);
 
 /**
  * Energy Distribution Rate Control I
+ * Maps the integrated energy distribution rate error to the pitch setpoint
  *
  * @min 0.0
  * @max 10.0
@@ -122,7 +128,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f);
 
 
 /**
- * Total Energy Distribution OFF (Cruise pitch sp)
+ * Total Energy Distribution Offset (Cruise pitch sp)
  *
  * @min 0.0
  * @max 10.0
@@ -170,6 +176,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
 
 /**
  * P gain for the altitude control
+ * Maps the altitude error to the flight path angle setpoint
  *
  * @min 0.0f
  * @max 10.0f
@@ -179,6 +186,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f);
 
 /**
  * D gain for the altitude control
+ * Maps the change of altitude error to the flight path angle setpoint
  *
  * @min 0.0f
  * @max 10.0f
@@ -187,7 +195,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f);
 PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f);
 
 /**
- * Lowpass for FPA error derivative (see MT_FPA_D)
+ * Lowpass for FPA error derivative calculation (see MT_FPA_D)
  *
  * @group mTECS
  */
@@ -217,6 +225,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
 
 /**
  * P gain for the airspeed control
+ * Maps the airspeed error to the acceleration setpoint
  *
  * @min 0.0f
  * @max 10.0f
@@ -241,7 +250,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f);
 PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
 
 /**
- * Airspeed derivative lowpass
+ * Airspeed derivative calculation lowpass
  *
  * @group mTECS
  */
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index dadfe40b2..c8d19af01 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -87,6 +87,7 @@
 #include <uORB/topics/tecs_status.h>
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/servorail_status.h>
+#include <uORB/topics/wind_estimate.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -943,6 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct tecs_status_s tecs_status;
 		struct system_power_s system_power;
 		struct servorail_status_s servorail_status;
+		struct wind_estimate_s wind_estimate;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -982,6 +984,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GS1A_s log_GS1A;
 			struct log_GS1B_s log_GS1B;
 			struct log_TECS_s log_TECS;
+			struct log_WIND_s log_WIND;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1016,6 +1019,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int tecs_status_sub;
 		int system_power_sub;
 		int servorail_status_sub;
+		int wind_sub;
 	} subs;
 
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -1044,6 +1048,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
 	subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
 	subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
+	subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
+	/* we need to rate-limit wind, as we do not need the full update rate */
+	orb_set_interval(subs.wind_sub, 90);
 
 	thread_running = true;
 
@@ -1508,9 +1515,20 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate;
 			log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp;
 			log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate;
+			log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode;
 			LOGBUFFER_WRITE_AND_COUNT(TECS);
 		}
 
+		/* --- WIND ESTIMATE --- */
+		if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) {
+			log_msg.msg_type = LOG_WIND_MSG;
+			log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north;
+			log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east;
+			log_msg.body.log_WIND.cov_x = buf.wind_estimate.covariance_north;
+			log_msg.body.log_WIND.cov_y = buf.wind_estimate.covariance_east;
+			LOGBUFFER_WRITE_AND_COUNT(WIND);
+		}
+
 		/* signal the other thread new data, but not yet unlock */
 		if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 			/* only request write if several packets can be written at once */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 8a0a0ad45..a874351b3 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -362,6 +362,17 @@ struct log_TECS_s {
 	float totalEnergyRate;
 	float energyDistributionRateSp;
 	float energyDistributionRate;
+
+	uint8_t mode;
+};
+
+/* --- WIND - WIND ESTIMATE --- */
+#define LOG_WIND_MSG 31
+struct log_WIND_s {
+	float x;
+	float y;
+	float cov_x;
+	float cov_y;
 };
 
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
@@ -419,7 +430,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(TECS, "ffffffffffff",	"AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR"),
+	LOG_FORMAT(TECS, "ffffffffffffB",	"AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"),
+	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index bae46e14d..7c3bb0009 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -203,3 +203,5 @@ ORB_DEFINE(estimator_status, struct estimator_status_report);
 #include "topics/tecs_status.h"
 ORB_DEFINE(tecs_status, struct tecs_status_s);
 
+#include "topics/wind_estimate.h"
+ORB_DEFINE(wind_estimate, struct wind_estimate_s);
diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h
index f3d33ec20..fc530b295 100644
--- a/src/modules/uORB/topics/tecs_status.h
+++ b/src/modules/uORB/topics/tecs_status.h
@@ -50,6 +50,14 @@
  * @{
  */
 
+typedef enum {
+	TECS_MODE_NORMAL,
+	TECS_MODE_UNDERSPEED,
+	TECS_MODE_TAKEOFF,
+	TECS_MODE_LAND,
+	TECS_MODE_LAND_THROTTLELIM
+} tecs_mode;
+
  /**
  * Internal values of the (m)TECS fixed wing speed alnd altitude control system
  */
@@ -69,6 +77,8 @@ struct tecs_status_s {
 	float totalEnergyRate;
 	float energyDistributionRateSp;
 	float energyDistributionRate;
+
+	tecs_mode mode;
 };
 
 /**
diff --git a/src/modules/uORB/topics/wind_estimate.h b/src/modules/uORB/topics/wind_estimate.h
new file mode 100644
index 000000000..58333a64f
--- /dev/null
+++ b/src/modules/uORB/topics/wind_estimate.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file wind_estimate.h
+ *
+ * Wind estimate topic topic
+ *
+ */
+
+#ifndef TOPIC_WIND_ESTIMATE_H
+#define TOPIC_WIND_ESTIMATE_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/** Wind estimate */
+struct wind_estimate_s {
+
+	uint64_t	timestamp;		/**< Microseconds since system boot */
+	float		windspeed_north;	/**< Wind component in north / X direction */
+	float		windspeed_east;		/**< Wind component in east / Y direction */
+	float		covariance_north;	/**< Uncertainty - set to zero (no uncertainty) if not estimated */
+	float		covariance_east;	/**< Uncertainty - set to zero (no uncertainty) if not estimated */
+};
+
+/**
+ * @}
+ */
+
+ORB_DECLARE(wind_estimate);
+
+#endif
\ No newline at end of file