18 files changed, 978 insertions, 165 deletions

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 2eb58abd6..b66d1dba5 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -45,39 +45,29 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 ECL_PitchController::ECL_PitchController() :
 	_last_run(0),
+	_tc(0.1f),
+	_k_p(0.0f),
+	_k_i(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate_pos(0.0f),
+	_max_rate_neg(0.0f),
+	_roll_ff(0.0f),
 	_last_output(0.0f),
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_max_deflection_rad(math::radians(45.0f))
+	_bodyrate_setpoint(0.0f)
 {
 }
 
-float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
-				   bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
 {
-	/* get the usual dt estimate */
-	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
-	_last_run = ecl_absolute_time();
-	float dt = (float)dt_micros * 1e-6f;
-
-	/* lock integral for long intervals */
-	if (dt_micros > 500000)
-		lock_integrator = true;
 
-	float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_i_rate = _k_i * _tc;
-
-	/* input conditioning */
-	if (!isfinite(airspeed)) {
-		/* airspeed is NaN, +- INF or not available, pick center of band */
-		airspeed = 0.5f * (airspeed_min + airspeed_max);
-	} else if (airspeed < airspeed_min) {
-		airspeed = airspeed_min;
-	}
 
 	/* flying inverted (wings upside down) ? */
 	bool inverted = false;
@@ -100,34 +90,78 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 	}
 
 	/* calculate the offset in the rate resulting from rolling  */
+	//xxx needs explanation and conversion to body angular rates or should be removed
 	float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
 				tanf(roll) * sinf(roll)) * _roll_ff;
 	if (inverted)
 		turn_offset = -turn_offset;
 
+	/* Calculate the error */
 	float pitch_error = pitch_setpoint - pitch;
-	/* rate setpoint from current error and time constant */
-	_rate_setpoint = pitch_error / _tc;
+
+	/*  Apply P controller: rate setpoint from current error and time constant */
+	_rate_setpoint =  pitch_error / _tc;
 
 	/* add turn offset */
 	_rate_setpoint += turn_offset;
 
-	_rate_error = _rate_setpoint - pitch_rate;
+	/* limit the rate */ //XXX: move to body angluar rates
+	if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) {
+		if (_rate_setpoint > 0.0f) {
+			_rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint;
+		} else {
+			_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
+		}
 
-	float ilimit_scaled = _integrator_max * scaler;
+	}
 
-	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+	return _rate_setpoint;
+}
 
-		float id = _rate_error * k_i_rate * dt * scaler;
+float ECL_PitchController::control_bodyrate(float roll, float pitch,
+		float pitch_rate, float yaw_rate,
+		float yaw_rate_setpoint,
+		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
+{
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
+
+	/* lock integral for long intervals */
+	if (dt_micros > 500000)
+		lock_integrator = true;
+
+//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+	float k_ff = 0;
+
+	/* input conditioning */
+	if (!isfinite(airspeed)) {
+		/* airspeed is NaN, +- INF or not available, pick center of band */
+		airspeed = 0.5f * (airspeed_min + airspeed_max);
+	} else if (airspeed < airspeed_min) {
+		airspeed = airspeed_min;
+	}
+
+	/* Transform setpoint to body angular rates */
+	_bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian
+
+	/* Transform estimation to body angular rates */
+	float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
+
+	_rate_error = _bodyrate_setpoint - pitch_bodyrate;
+
+	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+		float id = _rate_error * dt;
 
 		/*
-		 * anti-windup: do not allow integrator to increase into the
-		 * wrong direction if actuator is at limit
+		 * anti-windup: do not allow integrator to increase if actuator is at limit
 		 */
-		if (_last_output < -_max_deflection_rad) {
+		if (_last_output < -1.0f) {
 			/* only allow motion to center: increase value */
 			id = math::max(id, 0.0f);
-		} else if (_last_output > _max_deflection_rad) {
+		} else if (_last_output > 1.0f) {
 			/* only allow motion to center: decrease value */
 			id = math::min(id, 0.0f);
 		}
@@ -136,11 +170,14 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 	}
 
 	/* integrator limit */
-	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
-	/* store non-limited output */
-	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler;
-
-	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
+//	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
+//	warnx("roll: _last_output %.4f", (double)_last_output);
+	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
 void ECL_PitchController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
index 1e6cec6a1..30a82a86a 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -36,6 +36,7 @@
  * Definition of a simple orthogonal pitch PID controller.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  *
  * Acknowledgements:
  *
@@ -51,13 +52,18 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-class __EXPORT ECL_PitchController
+class __EXPORT ECL_PitchController //XXX: create controller superclass
 {
 public:
 	ECL_PitchController();
 
-	float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f,
-		      bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
+	float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed);
+
+
+	float control_bodyrate(float roll, float pitch,
+			float pitch_rate, float yaw_rate,
+			float yaw_rate_setpoint,
+			float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
 
 	void reset_integrator();
 
@@ -67,21 +73,27 @@ public:
 	void set_k_p(float k_p) {
 		_k_p = k_p;
 	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
-	void set_k_d(float k_d) {
-		_k_d = k_d;
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
 	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
+
 	void set_max_rate_pos(float max_rate_pos) {
 		_max_rate_pos = max_rate_pos;
 	}
+
 	void set_max_rate_neg(float max_rate_neg) {
 		_max_rate_neg = max_rate_neg;
 	}
+
 	void set_roll_ff(float roll_ff) {
 		_roll_ff = roll_ff;
 	}
@@ -94,13 +106,17 @@ public:
 		return _rate_setpoint;
 	}
 
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 
 	uint64_t _last_run;
 	float _tc;
 	float _k_p;
 	float _k_i;
-	float _k_d;
+	float _k_ff;
 	float _integrator_max;
 	float _max_rate_pos;
 	float _max_rate_neg;
@@ -109,7 +125,7 @@ private:
 	float _integrator;
 	float _rate_error;
 	float _rate_setpoint;
-	float _max_deflection_rad;
+	float _bodyrate_setpoint;
 };
 
 #endif // ECL_PITCH_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 0b1ffa7a4..9e7d35f68 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -45,21 +45,46 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 ECL_RollController::ECL_RollController() :
 	_last_run(0),
 	_tc(0.1f),
+	_k_p(0.0f),
+	_k_i(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate(0.0f),
 	_last_output(0.0f),
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_max_deflection_rad(math::radians(45.0f))
+	_bodyrate_setpoint(0.0f)
 {
+}
+
+float ECL_RollController::control_attitude(float roll_setpoint, float roll)
+{
+
+	/* Calculate error */
+	float roll_error = roll_setpoint - roll;
+
+	/* Apply P controller */
+	_rate_setpoint = roll_error / _tc;
+
+	/* limit the rate */ //XXX: move to body angluar rates
+	if (_max_rate > 0.01f) {
+		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+	}
 
+	return _rate_setpoint;
 }
 
-float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate,
-				  float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+float ECL_RollController::control_bodyrate(float pitch,
+		float roll_rate, float yaw_rate,
+		float yaw_rate_setpoint,
+		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
 {
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
@@ -70,10 +95,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_i_rate = _k_i * _tc;
+//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+	float k_ff = 0; //xxx: param
 
 	/* input conditioning */
+//	warnx("airspeed pre %.4f", (double)airspeed);
 	if (!isfinite(airspeed)) {
 		/* airspeed is NaN, +- INF or not available, pick center of band */
 		airspeed = 0.5f * (airspeed_min + airspeed_max);
@@ -81,32 +107,27 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 		airspeed = airspeed_min;
 	}
 
-	float roll_error = roll_setpoint - roll;
-	_rate_setpoint = roll_error / _tc;
-
-	/* limit the rate */
-	if (_max_rate > 0.01f) {
-		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
-		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
-	}
 
-	_rate_error = _rate_setpoint - roll_rate;
+	/* Transform setpoint to body angular rates */
+	_bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian
 
+	/* Transform estimation to body angular rates */
+	float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian
 
-	float ilimit_scaled = _integrator_max * scaler;
+	/* Calculate body angular rate error */
+	_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
 
-	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
 
-		float id = _rate_error * k_i_rate * dt * scaler;
+		float id = _rate_error * dt;
 
 		/*
-		 * anti-windup: do not allow integrator to increase into the
-		 * wrong direction if actuator is at limit
+		 * anti-windup: do not allow integrator to increase if actuator is at limit
 		 */
-		if (_last_output < -_max_deflection_rad) {
+		if (_last_output < -1.0f) {
 			/* only allow motion to center: increase value */
 			id = math::max(id, 0.0f);
-		} else if (_last_output > _max_deflection_rad) {
+		} else if (_last_output > 1.0f) {
 			/* only allow motion to center: decrease value */
 			id = math::min(id, 0.0f);
 		}
@@ -115,11 +136,14 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	}
 
 	/* integrator limit */
-	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
-	/* store non-limited output */
-	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
 
-	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
 void ECL_RollController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
index 0d4ea9333..92c64b95f 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -36,6 +36,7 @@
  * Definition of a simple orthogonal roll PID controller.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  *
  * Acknowledgements:
  *
@@ -51,13 +52,17 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-class __EXPORT ECL_RollController
+class __EXPORT ECL_RollController //XXX: create controller superclass
 {
 public:
 	ECL_RollController();
 
-	float control(float roll_setpoint, float roll, float roll_rate,
-		      float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
+	float control_attitude(float roll_setpoint, float roll);
+
+	float control_bodyrate(float pitch,
+			float roll_rate, float yaw_rate,
+			float yaw_rate_setpoint,
+			float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
 
 	void reset_integrator();
 
@@ -66,18 +71,23 @@ public:
 			_tc = time_constant;
 		}
 	}
+
 	void set_k_p(float k_p) {
 		_k_p = k_p;
 	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
-	void set_k_d(float k_d) {
-		_k_d = k_d;
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
 	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
+
 	void set_max_rate(float max_rate) {
 		_max_rate = max_rate;
 	}
@@ -90,19 +100,23 @@ public:
 		return _rate_setpoint;
 	}
 
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 	uint64_t _last_run;
 	float _tc;
 	float _k_p;
 	float _k_i;
-	float _k_d;
+	float _k_ff;
 	float _integrator_max;
 	float _max_rate;
 	float _last_output;
 	float _integrator;
 	float _rate_error;
 	float _rate_setpoint;
-	float _max_deflection_rad;
+	float _bodyrate_setpoint;
 };
 
 #endif // ECL_ROLL_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index b786acf24..5e2200727 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -44,29 +44,125 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 ECL_YawController::ECL_YawController() :
 	_last_run(0),
-	_last_error(0.0f),
+	_k_p(0.0f),
+	_k_i(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate(0.0f),
 	_last_output(0.0f),
-	_last_rate_hp_out(0.0f),
-	_last_rate_hp_in(0.0f),
-	_k_d_last(0.0f),
-	_integrator(0.0f)
+	_integrator(0.0f),
+	_rate_error(0.0f),
+	_rate_setpoint(0.0f),
+	_bodyrate_setpoint(0.0f),
+	_coordinated_min_speed(1.0f)
 {
+}
+
+float ECL_YawController::control_attitude(float roll, float pitch,
+		float speed_body_u, float speed_body_v, float speed_body_w,
+		float roll_rate_setpoint, float pitch_rate_setpoint)
+{
+//	static int counter = 0;
+	/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
+	_rate_setpoint = 0.0f;
+	if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
+		float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
+		if(denumerator != 0.0f) { //XXX: floating point comparison
+			_rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
+//			warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
+		}
+
+//		if(counter % 20 == 0) {
+//			warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch));
+//		}
+	}
+
+	/* limit the rate */ //XXX: move to body angluar rates
+	if (_max_rate > 0.01f) {
+	_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+	_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+	}
+
 
+//	counter++;
+
+	if(!isfinite(_rate_setpoint)){
+		warnx("yaw rate sepoint not finite");
+		_rate_setpoint = 0.0f;
+	}
+
+	return _rate_setpoint;
 }
 
-float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator,
-				 float airspeed_min, float airspeed_max, float aspeed)
+float ECL_YawController::control_bodyrate(float roll, float pitch,
+		float pitch_rate, float yaw_rate,
+		float pitch_rate_setpoint,
+		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
 {
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
+
+	/* lock integral for long intervals */
+	if (dt_micros > 500000)
+		lock_integrator = true;
+
+
+//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+	float k_ff = 0;
+
+
+	/* input conditioning */
+	if (!isfinite(airspeed)) {
+	/* airspeed is NaN, +- INF or not available, pick center of band */
+	airspeed = 0.5f * (airspeed_min + airspeed_max);
+	} else if (airspeed < airspeed_min) {
+	airspeed = airspeed_min;
+	}
+
+
+	/* Transform setpoint to body angular rates */
+	_bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian
+
+	/* Transform estimation to body angular rates */
+	float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian
+
+	/* Calculate body angular rate error */
+	_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
+
+	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+	float id = _rate_error * dt;
+
+	/*
+	 * anti-windup: do not allow integrator to increase if actuator is at limit
+	 */
+	if (_last_output < -1.0f) {
+		/* only allow motion to center: increase value */
+		id = math::max(id, 0.0f);
+	} else if (_last_output > 1.0f) {
+		/* only allow motion to center: decrease value */
+		id = math::min(id, 0.0f);
+	}
+
+	_integrator += id;
+	}
+
+	/* integrator limit */
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
 
-	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
 
-	return 0.0f;
+	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
 void ECL_YawController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index 66b227918..03f3202d0 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -35,6 +35,15 @@
  * @file ecl_yaw_controller.h
  * Definition of a simple orthogonal coordinated turn yaw PID controller.
  *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
  */
 #ifndef ECL_YAW_CONTROLLER_H
 #define ECL_YAW_CONTROLLER_H
@@ -42,47 +51,73 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-class __EXPORT ECL_YawController
+class __EXPORT ECL_YawController //XXX: create controller superclass
 {
 public:
 	ECL_YawController();
 
-	float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false,
-		      float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f));
+	float control_attitude(float roll, float pitch,
+			float speed_body_u, float speed_body_v, float speed_body_w,
+			float roll_rate_setpoint, float pitch_rate_setpoint);
+
+	float control_bodyrate(	float roll, float pitch,
+			float pitch_rate, float yaw_rate,
+			float pitch_rate_setpoint,
+			float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator);
 
 	void reset_integrator();
 
-	void set_k_side(float k_a) {
-		_k_side = k_a;
+	void set_k_p(float k_p) {
+			_k_p = k_p;
 	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
-	void set_k_d(float k_d) {
-		_k_d = k_d;
-	}
-	void set_k_roll_ff(float k_roll_ff) {
-		_k_roll_ff = k_roll_ff;
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
 	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
 
+	void set_max_rate(float max_rate) {
+		_max_rate = max_rate;
+	}
+
+	void set_coordinated_min_speed(float coordinated_min_speed) {
+		_coordinated_min_speed = coordinated_min_speed;
+	}
+
+
+	float get_rate_error() {
+		return _rate_error;
+	}
+
+	float get_desired_rate() {
+		return _rate_setpoint;
+	}
+
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 	uint64_t _last_run;
-
-	float _k_side;
+	float _k_p;
 	float _k_i;
-	float _k_d;
-	float _k_roll_ff;
+	float _k_ff;
 	float _integrator_max;
-
-	float _last_error;
+	float _max_rate;
+	float  _roll_ff;
 	float _last_output;
-	float _last_rate_hp_out;
-	float _last_rate_hp_in;
-	float _k_d_last;
 	float _integrator;
+	float _rate_error;
+	float _rate_setpoint;
+	float _bodyrate_setpoint;
+	float _coordinated_min_speed;
 
 };
 
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index 27d76f959..11def2371 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -70,7 +70,7 @@ float ECL_L1_Pos_Controller::target_bearing()
 float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
 {
 	/* following [2], switching on L1 distance */
-	return math::max(wp_radius, _L1_distance);
+	return math::min(wp_radius, _L1_distance);
 }
 
 bool ECL_L1_Pos_Controller::reached_loiter_target(void)
@@ -280,7 +280,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
 	 */
 
 	// XXX check switch over
-	if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) |
+	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;
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 1d5c85699..5a56dce65 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -2,6 +2,7 @@
 
 #include "tecs.h"
 #include <ecl/ecl.h>
+#include <systemlib/err.h>
 
 using namespace math;
 
@@ -168,64 +169,88 @@ void TECS::_update_speed_demand(void)
 	// 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;
-	}
+//	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;
+//	}
+
+	_TAS_dem_adj = _TAS_dem;
+	_TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now
 
 	// 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;
+//	_TAS_dem_last = _TAS_dem;
+
+//	warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f",
+//			(double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent, _underspeed, velRateMin);
+//	warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f,  _badDescent %u , _underspeed %u",
+//			(double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state,  _badDescent , _underspeed);
 }
 
-void TECS::_update_height_demand(float demand)
+void TECS::_update_height_demand(float demand, float state)
 {
-	// 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);
+//	// 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);
+
+	_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
+	_hgt_dem_adj_last = _hgt_dem_adj;
 
+	_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
 	// Limit height rate of change
-	if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
-		_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
+	if (_hgt_rate_dem > _maxClimbRate) {
+		_hgt_rate_dem = _maxClimbRate;
 
-	} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
-		_hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
+	} else if (_hgt_rate_dem < -_maxSinkRate) {
+		_hgt_rate_dem = -_maxSinkRate;
 	}
 
-	_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);
+	//warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj);
 }
 
 void TECS::_detect_underspeed(void)
@@ -285,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
 		// 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);
+		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f);
 
 		if (STEdot_dem >= 0) {
 			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
@@ -353,14 +378,18 @@ void TECS::_detect_bad_descent(void)
 	// 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;
-	}
+//	float STEdot = _SPEdot + _SKEdot;
+//
+//	if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
+//
+//		warnx("bad descent detected: _STE_error %.1f, STEdot %.1f, _throttle_dem %.1f", _STE_error, STEdot, _throttle_dem);
+//		_badDescent = true;
+//
+//	} else {
+//		_badDescent = false;
+//	}
+
+	_badDescent = false;
 }
 
 void TECS::_update_pitch(void)
@@ -508,7 +537,7 @@ void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float bar
 	_update_speed_demand();
 
 	// Calculate the height demand
-	_update_height_demand(hgt_dem);
+	_update_height_demand(hgt_dem, baro_altitude);
 
 	// Detect underspeed condition
 	_detect_underspeed();
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index f8f832ed7..4fc009da9 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -180,6 +180,14 @@ public:
 		_indicated_airspeed_max = airspeed;
 	}
 
+	void set_heightrate_p(float heightrate_p) {
+		_heightrate_p = heightrate_p;
+	}
+
+	void set_speedrate_p(float speedrate_p) {
+		_speedrate_p = speedrate_p;
+	}
+
 private:
 	// Last time update_50Hz was called
 	uint64_t _update_50hz_last_usec;
@@ -203,6 +211,8 @@ private:
 	float _vertAccLim;
 	float _rollComp;
 	float _spdWeight;
+	float _heightrate_p;
+	float _speedrate_p;
 
 	// throttle demand in the range from 0.0 to 1.0
 	float _throttle_dem;
@@ -329,7 +339,7 @@ private:
 	void _update_speed_demand(void);
 
 	// Update the demanded height
-	void _update_height_demand(float demand);
+	void _update_height_demand(float demand, float state);
 
 	// Detect an underspeed condition
 	void _detect_underspeed(void);
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 43105fdba..08fe2b696 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -387,6 +387,45 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
 	return return_value;
 }
 
+__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
+	                                          double lat_next, double lon_next, float alt_next,
+	                                          float *dist_xy, float *dist_z)
+{
+	double current_x_rad = lat_next / 180.0 * M_PI;
+	double current_y_rad = lon_next / 180.0 * M_PI;
+	double x_rad = lat_now / 180.0 * M_PI;
+	double y_rad = lon_now / 180.0 * M_PI;
+
+	double d_lat = x_rad - current_x_rad;
+	double d_lon = y_rad - current_y_rad;
+
+	double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad);
+	double c = 2 * atan2(sqrt(a), sqrt(1 - a));
+
+	float dxy = CONSTANTS_RADIUS_OF_EARTH * c;
+	float dz = alt_now - alt_next;
+
+	*dist_xy = fabsf(dxy);
+	*dist_z = fabsf(dz);
+
+	return sqrtf(dxy * dxy + dz * dz);
+}
+
+
+__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
+	                                           float x_next, float y_next, float z_next,
+	                                           float *dist_xy, float *dist_z)
+{
+	float dx = x_now - x_next;
+	float dy = y_now - y_next;
+	float dz = z_now - z_next;
+
+	*dist_xy = sqrtf(dx * dx + dy * dy);
+	*dist_z = fabsf(dz);
+
+	return sqrtf(dx * dx + dy * dy + dz * dz);
+}
+
 __EXPORT float _wrap_pi(float bearing)
 {
 	/* value is inf or NaN */
@@ -464,5 +503,3 @@ __EXPORT float _wrap_360(float bearing)
 
 	return bearing;
 }
-
-
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 123ff80f1..5f4bce698 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -47,6 +47,9 @@
 
 #pragma once
 
+#include "uORB/topics/fence.h"
+#include "uORB/topics/vehicle_global_position.h"
+
 __BEGIN_DECLS
 
 #include <stdbool.h>
@@ -121,6 +124,20 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
 __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
 		float radius, float arc_start_bearing, float arc_sweep);
 
+/*
+ * Calculate distance in global frame
+ */
+__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
+	                                          double lat_next, double lon_next, float alt_next,
+	                                          float *dist_xy, float *dist_z);
+
+/*
+ * Calculate distance in local frame (NED)
+ */
+__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
+	                                           float x_next, float y_next, float z_next,
+	                                           float *dist_xy, float *dist_z);
+
 __EXPORT float _wrap_180(float bearing);
 __EXPORT float _wrap_360(float bearing);
 __EXPORT float _wrap_pi(float bearing);
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
new file mode 100644
index 000000000..d5c759b17
--- /dev/null
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   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 documentation4 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 CatapultLaunchMethod.cpp
+ * Catpult Launch detection
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "CatapultLaunchMethod.h"
+#include <systemlib/err.h>
+
+CatapultLaunchMethod::CatapultLaunchMethod() :
+	last_timestamp(0),
+	integrator(0.0f),
+	launchDetected(false),
+	threshold_accel(NULL, "LAUN_CAT_A", false),
+	threshold_time(NULL, "LAUN_CAT_T", false)
+{
+
+}
+
+CatapultLaunchMethod::~CatapultLaunchMethod() {
+
+}
+
+void CatapultLaunchMethod::update(float accel_x)
+{
+	float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
+	last_timestamp = hrt_absolute_time();
+
+	if (accel_x > threshold_accel.get()) {
+		integrator += accel_x * dt;
+//		warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
+//				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
+		if (integrator > threshold_accel.get() * threshold_time.get()) {
+			launchDetected = true;
+		}
+
+	} else {
+//		warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
+//				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
+		/* reset integrator */
+		integrator = 0.0f;
+		launchDetected = false;
+	}
+
+}
+
+bool CatapultLaunchMethod::getLaunchDetected()
+{
+	return launchDetected;
+}
+
+void CatapultLaunchMethod::updateParams()
+{
+	threshold_accel.update();
+	threshold_time.update();
+}
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
new file mode 100644
index 000000000..e943f11e9
--- /dev/null
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   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 documentation4 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 CatapultLaunchMethod.h
+ * Catpult Launch detection
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef CATAPULTLAUNCHMETHOD_H_
+#define CATAPULTLAUNCHMETHOD_H_
+
+#include "LaunchMethod.h"
+
+#include <drivers/drv_hrt.h>
+#include <controllib/block/BlockParam.hpp>
+
+class CatapultLaunchMethod : public LaunchMethod
+{
+public:
+	CatapultLaunchMethod();
+	~CatapultLaunchMethod();
+
+	void update(float accel_x);
+	bool getLaunchDetected();
+	void updateParams();
+
+private:
+	hrt_abstime last_timestamp;
+//	float threshold_accel_raw;
+//	float threshold_time;
+	float integrator;
+	bool launchDetected;
+
+	control::BlockParamFloat threshold_accel;
+	control::BlockParamFloat threshold_time;
+
+};
+
+#endif /* CATAPULTLAUNCHMETHOD_H_ */
diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
new file mode 100644
index 000000000..67b32ad82
--- /dev/null
+++ b/src/lib/launchdetection/LaunchDetector.cpp
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   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 launchDetection.cpp
+ * Auto Detection for different launch methods (e.g. catapult)
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "LaunchDetector.h"
+#include "CatapultLaunchMethod.h"
+#include <systemlib/err.h>
+
+LaunchDetector::LaunchDetector() :
+	launchdetection_on(NULL, "LAUN_ALL_ON", false),
+	throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
+{
+	/* init all detectors */
+	launchMethods[0] = new CatapultLaunchMethod();
+
+
+	/* update all parameters of all detectors */
+	updateParams();
+}
+
+LaunchDetector::~LaunchDetector()
+{
+
+}
+
+void LaunchDetector::update(float accel_x)
+{
+	if (launchdetection_on.get() == 1) {
+		for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+			launchMethods[i]->update(accel_x);
+		}
+	}
+}
+
+bool LaunchDetector::getLaunchDetected()
+{
+	if (launchdetection_on.get() == 1) {
+		for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+			if(launchMethods[i]->getLaunchDetected()) {
+				return true;
+			}
+		}
+	}
+
+	return false;
+}
+
+void LaunchDetector::updateParams() {
+
+	warnx(" LaunchDetector::updateParams()");
+	launchdetection_on.update();
+	throttlePreTakeoff.update();
+
+	for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+		launchMethods[i]->updateParams();
+		warnx("updating component %d", i);
+	}
+
+	warnx(" LaunchDetector::updateParams() ended");
+}
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
new file mode 100644
index 000000000..7c2ff826c
--- /dev/null
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   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 documentation4 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 LaunchDetector.h
+ * Auto Detection for different launch methods (e.g. catapult)
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef LAUNCHDETECTOR_H
+#define LAUNCHDETECTOR_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "LaunchMethod.h"
+
+#include <controllib/block/BlockParam.hpp>
+
+class __EXPORT LaunchDetector
+{
+public:
+	LaunchDetector();
+	~LaunchDetector();
+
+	void update(float accel_x);
+	bool getLaunchDetected();
+	void updateParams();
+	bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
+
+	float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
+
+//	virtual bool getLaunchDetected();
+protected:
+private:
+	LaunchMethod* launchMethods[1];
+	control::BlockParamInt launchdetection_on;
+	control::BlockParamFloat throttlePreTakeoff;
+
+
+};
+
+
+#endif // LAUNCHDETECTOR_H
diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h
new file mode 100644
index 000000000..bfb5f8cb4
--- /dev/null
+++ b/src/lib/launchdetection/LaunchMethod.h
@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   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 documentation4 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 LaunchMethod.h
+ * Base class for different launch methods
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef LAUNCHMETHOD_H_
+#define LAUNCHMETHOD_H_
+
+class LaunchMethod
+{
+public:
+	virtual void update(float accel_x) = 0;
+	virtual bool getLaunchDetected() = 0;
+	virtual void updateParams() = 0;
+protected:
+private:
+};
+
+#endif /* LAUNCHMETHOD_H_ */
diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c
new file mode 100644
index 000000000..63a8981aa
--- /dev/null
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 launchdetection_params.c
+ *
+ * Parameters for launchdetection
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Launch detection parameters, accessible via MAVLink
+ *
+ */
+
+/* Catapult Launch detection */
+
+// @DisplayName		Switch to enable launchdetection
+// @Description		if set to 1 launchdetection is enabled
+// @Range		0 or 1
+PARAM_DEFINE_INT32(LAUN_ALL_ON, 0);
+
+// @DisplayName		Catapult Accelerometer Threshold
+// @Description		LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
+// @Range		> 0
+PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
+
+// @DisplayName		Catapult Time Threshold
+// @Description		LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
+// @Range		> 0, in seconds
+PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
+
+// @DisplayName		Throttle setting while detecting the launch
+// @Description		The throttle is set to this value while the system is waiting for the takeoff
+// @Range		0 to 1
+PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f);
diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk
new file mode 100644
index 000000000..13648b74c
--- /dev/null
+++ b/src/lib/launchdetection/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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 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.
+#
+############################################################################
+
+#
+# Launchdetection Library
+#
+
+SRCS		 = 	LaunchDetector.cpp \
+			CatapultLaunchMethod.cpp \
+			launchdetection_params.c