Merge remote-tracking branch 'thomasgubler_private/fw_autoland_att_tecs' into navigator_wip_merge_test

Conflicts:
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp

@@ -45,39 +45,30 @@
 #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_d(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;
@@ -105,29 +96,72 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 	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()

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,30 @@ 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,6 +109,10 @@ public:
 		return _rate_setpoint;
 	}
 
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 
 	uint64_t _last_run;
@@ -101,6 +120,7 @@ private:
 	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 +129,7 @@ private:
 	float _integrator;
 	float _rate_error;
 	float _rate_setpoint;
-	float _max_deflection_rad;
+	float _bodyrate_setpoint;
 };
 
 #endif // ECL_PITCH_CONTROLLER_H

src/lib/ecl/attitude_fw/ecl_roll_controller.cpp

@@ -45,21 +45,47 @@
 #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_d(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 +96,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 +108,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 +137,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()

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,27 @@ 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 +104,24 @@ 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

src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp

@@ -44,29 +44,127 @@
 #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_d(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate(0.0f),
+	_roll_ff(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()

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,82 @@
 #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_k_roll_ff(float roll_ff) {
+		_roll_ff = roll_ff;
+	}
+
+	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;
 
 };
 

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)

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;
 
@@ -199,33 +200,47 @@ void TECS::_update_speed_demand(void)
 	_TAS_dem_last = _TAS_dem;
 }
 
-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)
@@ -299,7 +314,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
 
 		// Rate limit PD + FF throttle
 		// Calculate the throttle increment from the specified slew time
-		if (fabsf(_throttle_slewrate) < 0.01f) {
+		if (fabsf(_throttle_slewrate) > 0.01f) {
 			float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
 
 			_throttle_dem = constrain(_throttle_dem,
@@ -500,7 +515,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();

src/lib/external_lgpl/tecs/tecs.h

@@ -180,6 +180,10 @@ public:
 		_indicated_airspeed_max = airspeed;
 	}
 
+	void set_heightrate_p(float heightrate_p) {
+		_heightrate_p = heightrate_p;
+	}
+
 private:
 	// Last time update_50Hz was called
 	uint64_t _update_50hz_last_usec;
@@ -203,6 +207,7 @@ private:
 	float _vertAccLim;
 	float _rollComp;
 	float _spdWeight;
+	float _heightrate_p;
 
 	// throttle demand in the range from 0.0 to 1.0
 	float _throttle_dem;
@@ -329,7 +334,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);

src/modules/commander/commander.cpp

@@ -1539,7 +1539,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 			// TODO AUTO_LAND handling
 			if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
 				/* don't switch to other states until takeoff not completed */
-				if (local_pos->z > -takeoff_alt || status->condition_landed) {
+				// XXX: only respect the condition_landed when the local position is actually valid
+				if (status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
 					return TRANSITION_NOT_CHANGED;
 				}
 			}

src/modules/commander/state_machine_helper.cpp

@@ -239,8 +239,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 		case MAIN_STATE_SEATBELT:
 
 			/* need at minimum altitude estimate */
-			if (current_state->condition_local_altitude_valid ||
-				current_state->condition_global_position_valid) {
+			if (!current_state->is_rotary_wing ||
+				(current_state->condition_local_altitude_valid ||
+				current_state->condition_global_position_valid)) {
 				ret = TRANSITION_CHANGED;
 			}
 

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -37,6 +37,7 @@
  * Implementation of a generic attitude controller based on classic orthogonal PIDs.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  *
  */
 
@@ -62,6 +63,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_global_position.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/pid/pid.h>
@@ -106,26 +108,28 @@ private:
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
 	int		_control_task;			/**< task handle for sensor task */
 
-	int		_att_sub;			/**< vehicle attitude subscription */
-	int		_accel_sub;			/**< accelerometer subscription */
+	int		_att_sub;				/**< vehicle attitude subscription */
+	int		_accel_sub;				/**< accelerometer subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
-	int		_vcontrol_mode_sub;			/**< vehicle status subscription */
-	int 		_params_sub;			/**< notification of parameter updates */
-	int 		_manual_sub;			/**< notification of manual control updates */
+	int		_vcontrol_mode_sub;		/**< vehicle status subscription */
+	int 	_params_sub;			/**< notification of parameter updates */
+	int 	_manual_sub;			/**< notification of manual control updates */
+	int		_global_pos_sub;		/**< global position subscription */
 
 	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint point */
 	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
 
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct accel_report				_accel;			/**< body frame accelerations */
+	struct vehicle_attitude_s				_att;			/**< vehicle attitude */
+	struct accel_report						_accel;			/**< body frame accelerations */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct airspeed_s				_airspeed;		/**< airspeed */
-	struct vehicle_control_mode_s			_vcontrol_mode;		/**< vehicle control mode */
-	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
+	struct airspeed_s						_airspeed;		/**< airspeed */
+	struct vehicle_control_mode_s			_vcontrol_mode;	/**< vehicle control mode */
+	struct actuator_controls_s				_actuators;		/**< actuator control inputs */
+	struct vehicle_global_position_s		_global_pos;	/**< global position */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -137,6 +141,7 @@ private:
 		float p_p;
 		float p_d;
 		float p_i;
+		float p_ff;
 		float p_rmax_pos;
 		float p_rmax_neg;
 		float p_integrator_max;
@@ -144,13 +149,17 @@ private:
 		float r_p;
 		float r_d;
 		float r_i;
+		float r_ff;
 		float r_integrator_max;
 		float r_rmax;
 		float y_p;
 		float y_i;
 		float y_d;
+		float y_ff;
 		float y_roll_feedforward;
 		float y_integrator_max;
+		float y_coordinated_min_speed;
+		float y_rmax;
 
 		float airspeed_min;
 		float airspeed_trim;
@@ -163,6 +172,7 @@ private:
 		param_t p_p;
 		param_t p_d;
 		param_t p_i;
+		param_t p_ff;
 		param_t p_rmax_pos;
 		param_t p_rmax_neg;
 		param_t p_integrator_max;
@@ -170,13 +180,17 @@ private:
 		param_t r_p;
 		param_t r_d;
 		param_t r_i;
+		param_t r_ff;
 		param_t r_integrator_max;
 		param_t r_rmax;
 		param_t y_p;
 		param_t y_i;
 		param_t y_d;
+		param_t y_ff;
 		param_t y_roll_feedforward;
 		param_t y_integrator_max;
+		param_t y_coordinated_min_speed;
+		param_t y_rmax;
 
 		param_t airspeed_min;
 		param_t airspeed_trim;
@@ -226,6 +240,11 @@ private:
 	 */
 	void		vehicle_setpoint_poll();
 
+	/**
+	 * Check for global position updates.
+	 */
+	void		global_pos_poll();
+
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
@@ -261,6 +280,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
 	_manual_sub(-1),
+	_global_pos_sub(-1),
 
 /* publications */
 	_rate_sp_pub(-1),
@@ -273,31 +293,48 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_setpoint_valid(false),
 	_airspeed_valid(false)
 {
+	/* safely initialize structs */
+	vehicle_attitude_s			_att = {0};
+	accel_report				_accel = {0};
+	vehicle_attitude_setpoint_s		_att_sp = {0};
+	manual_control_setpoint_s		_manual = {0};
+	airspeed_s				_airspeed = {0};
+	vehicle_control_mode_s			_vcontrol_mode = {0};
+	actuator_controls_s			_actuators = {0};
+	vehicle_global_position_s		_global_pos = {0};
+
+
 	_parameter_handles.tconst = param_find("FW_ATT_TC");
-	_parameter_handles.p_p = param_find("FW_P_P");
-	_parameter_handles.p_d = param_find("FW_P_D");
-	_parameter_handles.p_i = param_find("FW_P_I");
+	_parameter_handles.p_p = param_find("FW_PR_P");
+	_parameter_handles.p_d = param_find("FW_PR_D");
+	_parameter_handles.p_i = param_find("FW_PR_I");
+	_parameter_handles.p_ff = param_find("FW_PR_FF");
 	_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
 	_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
-	_parameter_handles.p_integrator_max = param_find("FW_P_IMAX");
+	_parameter_handles.p_integrator_max = param_find("FW_PR_IMAX");
 	_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
 
-	_parameter_handles.r_p = param_find("FW_R_P");
-	_parameter_handles.r_d = param_find("FW_R_D");
-	_parameter_handles.r_i = param_find("FW_R_I");
-	_parameter_handles.r_integrator_max = param_find("FW_R_IMAX");
+	_parameter_handles.r_p = param_find("FW_RR_P");
+	_parameter_handles.r_d = param_find("FW_RR_D");
+	_parameter_handles.r_i = param_find("FW_RR_I");
+	_parameter_handles.r_ff = param_find("FW_RR_FF");
+	_parameter_handles.r_integrator_max = param_find("FW_RR_IMAX");
 	_parameter_handles.r_rmax = param_find("FW_R_RMAX");
 
-	_parameter_handles.y_p = param_find("FW_Y_P");
-	_parameter_handles.y_i = param_find("FW_Y_I");
-	_parameter_handles.y_d = param_find("FW_Y_D");
+	_parameter_handles.y_p = param_find("FW_YR_P");
+	_parameter_handles.y_i = param_find("FW_YR_I");
+	_parameter_handles.y_d = param_find("FW_YR_D");
+	_parameter_handles.y_ff = param_find("FW_YR_FF");
 	_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
-	_parameter_handles.y_integrator_max = param_find("FW_Y_IMAX");
+	_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
+	_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
 
 	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
 	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
 
+	_parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -335,6 +372,7 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.p_p, &(_parameters.p_p));
 	param_get(_parameter_handles.p_d, &(_parameters.p_d));
 	param_get(_parameter_handles.p_i, &(_parameters.p_i));
+	param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
 	param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
 	param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
 	param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
@@ -343,14 +381,19 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.r_p, &(_parameters.r_p));
 	param_get(_parameter_handles.r_d, &(_parameters.r_d));
 	param_get(_parameter_handles.r_i, &(_parameters.r_i));
+	param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
+
 	param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
 	param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
 
 	param_get(_parameter_handles.y_p, &(_parameters.y_p));
 	param_get(_parameter_handles.y_i, &(_parameters.y_i));
 	param_get(_parameter_handles.y_d, &(_parameters.y_d));
+	param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
 	param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward));
 	param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
+	param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
+	param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
 
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
@@ -358,28 +401,33 @@ FixedwingAttitudeControl::parameters_update()
 
 	/* pitch control parameters */
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
-	_pitch_ctrl.set_k_p(math::radians(_parameters.p_p));
-	_pitch_ctrl.set_k_i(math::radians(_parameters.p_i));
-	_pitch_ctrl.set_k_d(math::radians(_parameters.p_d));
-	_pitch_ctrl.set_integrator_max(math::radians(_parameters.p_integrator_max));
+	_pitch_ctrl.set_k_p(_parameters.p_p);
+	_pitch_ctrl.set_k_i(_parameters.p_i);
+	_pitch_ctrl.set_k_d(_parameters.p_d);
+	_pitch_ctrl.set_k_ff(_parameters.p_ff);
+	_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
 	_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
 	_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
-	_pitch_ctrl.set_roll_ff(math::radians(_parameters.p_roll_feedforward));
+	_pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward);
 
 	/* roll control parameters */
 	_roll_ctrl.set_time_constant(_parameters.tconst);
-	_roll_ctrl.set_k_p(math::radians(_parameters.r_p));
-	_roll_ctrl.set_k_i(math::radians(_parameters.r_i));
-	_roll_ctrl.set_k_d(math::radians(_parameters.r_d));
-	_roll_ctrl.set_integrator_max(math::radians(_parameters.r_integrator_max));
+	_roll_ctrl.set_k_p(_parameters.r_p);
+	_roll_ctrl.set_k_i(_parameters.r_i);
+	_roll_ctrl.set_k_d(_parameters.r_d);
+	_roll_ctrl.set_k_ff(_parameters.r_ff);
+	_roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
 	_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
 
 	/* yaw control parameters */
-	_yaw_ctrl.set_k_side(math::radians(_parameters.y_p));
-	_yaw_ctrl.set_k_i(math::radians(_parameters.y_i));
-	_yaw_ctrl.set_k_d(math::radians(_parameters.y_d));
-	_yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward));
-	_yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max));
+	_yaw_ctrl.set_k_p(_parameters.y_p);
+	_yaw_ctrl.set_k_i(_parameters.y_i);
+	_yaw_ctrl.set_k_d(_parameters.y_d);
+	_yaw_ctrl.set_k_ff(_parameters.y_ff);
+	_yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward);
+	_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
+	_yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed);
+	_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
 
 	return OK;
 }
@@ -421,6 +469,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
 
 	if (airspeed_updated) {
 		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+//		warnx("airspeed poll: ind: %.4f,  true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s);
 		return true;
 	}
 
@@ -452,6 +501,18 @@ FixedwingAttitudeControl::vehicle_setpoint_poll()
 	}
 }
 
+void
+FixedwingAttitudeControl::global_pos_poll()
+{
+	/* check if there is a new global position */
+	bool global_pos_updated;
+	orb_check(_global_pos_sub, &global_pos_updated);
+
+	if (global_pos_updated) {
+		orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+	}
+}
+
 void
 FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
 {
@@ -476,6 +537,7 @@ FixedwingAttitudeControl::task_main()
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vcontrol_mode_sub, 200);
@@ -551,6 +613,8 @@ FixedwingAttitudeControl::task_main()
 
 			vehicle_manual_poll();
 
+			global_pos_poll();
+
 			/* lock integrator until control is started */
 			bool lock_integrator;
 
@@ -565,18 +629,15 @@ FixedwingAttitudeControl::task_main()
 
 			if (_vcontrol_mode.flag_control_attitude_enabled) {
 
-				/* scale from radians to normalized -1 .. 1 range */
-				const float actuator_scaling = 1.0f / (M_PI_F / 4.0f);
-
 				/* scale around tuning airspeed */
 
 				float airspeed;
 
 				/* if airspeed is smaller than min, the sensor is not giving good readings */
 				if (!_airspeed_valid ||
-				    (_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) ||
+				    (_airspeed.indicated_airspeed_m_s < 0.1f * _parameters.airspeed_min) ||
 				    !isfinite(_airspeed.indicated_airspeed_m_s)) {
-					airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
+					airspeed = _parameters.airspeed_trim;
 
 				} else {
 					airspeed = _airspeed.indicated_airspeed_m_s;
@@ -585,7 +646,8 @@ FixedwingAttitudeControl::task_main()
 				float airspeed_scaling = _parameters.airspeed_trim / airspeed;
 				//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
 
-				float roll_sp, pitch_sp;
+				float roll_sp = 0.0f;
+				float pitch_sp = 0.0f;
 				float throttle_sp = 0.0f;
 
 				if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
@@ -635,46 +697,86 @@ FixedwingAttitudeControl::task_main()
 					}
 				}
 
-				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
+				/* Prepare speed_body_u and speed_body_w */
+				float speed_body_u = 0.0f;
+				float speed_body_v = 0.0f;
+				float speed_body_w = 0.0f;
+				if(_att.R_valid) 	{
+					speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz;
+					speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz;
+					speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz;
+				} else	{
+					warnx("Did not get a valid R\n");
+				}
 
-					float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
-									    airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-					_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
+				/* Run attitude controllers */
+				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
+					_roll_ctrl.control_attitude(roll_sp, _att.roll);
+					_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
+					_yaw_ctrl.control_attitude(_att.roll, _att.pitch,
+							speed_body_u, speed_body_v, speed_body_w,
+							_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
+
+					/* Run attitude RATE controllers which need the desired attitudes from above */
+					float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
+							_att.rollspeed, _att.yawspeed,
+							_yaw_ctrl.get_desired_rate(),
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f;
+					if (!isfinite(roll_u)) {
+						warnx("roll_u %.4f", roll_u);
+					}
 
-					float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
-									      lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-					_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
+					float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
+							_att.pitchspeed, _att.yawspeed,
+							_yaw_ctrl.get_desired_rate(),
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f;
+					if (!isfinite(pitch_u)) {
+						warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
+								pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body);
+					}
 
-					float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
-									  _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-					_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
+					float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
+							_att.pitchspeed, _att.yawspeed,
+							_pitch_ctrl.get_desired_rate(),
+							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f;
+					if (!isfinite(yaw_u)) {
+						warnx("yaw_u %.4f", yaw_u);
+					}
 
 					/* throttle passed through */
 					_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+					if (!isfinite(throttle_sp)) {
+						warnx("throttle_sp %.4f", throttle_sp);
+					}
+				} else {
+					warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp);
+				}
 
-					// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
-					// 			airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
-					// 			_actuators.control[2], _actuators.control[3]);
+				// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
+				// 			airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
+				// 			_actuators.control[2], _actuators.control[3]);
 
-					/*
-					 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
-					 * only once available
-					 */
-					vehicle_rates_setpoint_s rates_sp;
-					rates_sp.roll = _roll_ctrl.get_desired_rate();
-					rates_sp.pitch = _pitch_ctrl.get_desired_rate();
-					rates_sp.yaw = 0.0f; // XXX not yet implemented
+				/*
+				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
+				 * only once available
+				 */
+				vehicle_rates_setpoint_s rates_sp;
+				rates_sp.roll = _roll_ctrl.get_desired_rate();
+				rates_sp.pitch = _pitch_ctrl.get_desired_rate();
+				rates_sp.yaw = _yaw_ctrl.get_desired_rate();
 
-					rates_sp.timestamp = hrt_absolute_time();
+				rates_sp.timestamp = hrt_absolute_time();
 
-					if (_rate_sp_pub > 0) {
-						/* publish the attitude setpoint */
-						orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
+				if (_rate_sp_pub > 0) {
+					/* publish the attitude setpoint */
+					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
 
-					} else {
-						/* advertise and publish */
-						_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
-					}
+				} else {
+					/* advertise and publish */
+					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
 				}
 
 			} else {

src/modules/fw_att_control/fw_att_control_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+f *   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
@@ -44,11 +44,13 @@
 
 #include <systemlib/param/param.h>
 
+//XXX resolve unclear naming of paramters: FW_P_P --> FW_PR_P
+
 /*
  * Controller parameters, accessible via MAVLink
  *
  */
-
+//xxx: update descriptions
 // @DisplayName		Attitude Time Constant
 // @Description		This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
 // @Range		0.4 to 1.0 seconds, in tens of seconds
@@ -57,33 +59,33 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
 // @DisplayName		Proportional gain.
 // @Description		This defines how much the elevator input will be commanded dependend on the current pitch error.
 // @Range		10 to 200, 1 increments
-PARAM_DEFINE_FLOAT(FW_P_P, 40.0f);
+PARAM_DEFINE_FLOAT(FW_PR_P, 0.3f);
 
 // @DisplayName		Damping gain.
 // @Description		This gain damps the airframe pitch rate. In particular relevant for flying wings.
 // @Range		0.0 to 10.0, 0.1 increments
-PARAM_DEFINE_FLOAT(FW_P_D, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PR_D, 0.0f); //xxx: remove
 
 // @DisplayName		Integrator gain.
 // @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
 // @Range		0 to 50.0
-PARAM_DEFINE_FLOAT(FW_P_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PR_I, 0.05f);
 
 // @DisplayName		Maximum positive / up pitch rate.
 // @Description		This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
 // @Range		0 to 90.0 degrees per seconds, in 1 increments
-PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
+PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f);
 
 // @DisplayName		Maximum negative / down pitch rate.
 // @Description		This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
 // @Range		0 to 90.0 degrees per seconds, in 1 increments
-PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
+PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
 
 // @DisplayName		Pitch Integrator Anti-Windup
 // @Description		This limits the range in degrees the integrator can wind up to.
 // @Range		0.0 to 45.0
 // @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f);
+PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
 
 // @DisplayName		Roll feedforward gain.
 // @Description		This compensates during turns and ensures the nose stays level.
@@ -97,27 +99,27 @@ PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f);
 // @Range		10.0 200.0
 // @Increment		10.0
 // @User		User
-PARAM_DEFINE_FLOAT(FW_R_P, 40.0f);
+PARAM_DEFINE_FLOAT(FW_RR_P, 0.5f);
 
 // @DisplayName		Damping Gain
 // @Description		Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence.
 // @Range		0.0 10.0
 // @Increment		1.0
 // @User		User
-PARAM_DEFINE_FLOAT(FW_R_D, 0.0f);
+PARAM_DEFINE_FLOAT(FW_RR_D, 0.0f); //xxx: remove
 
 // @DisplayName		Integrator Gain
 // @Description		This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors.
 // @Range		0.0 100.0
 // @Increment		5.0
 // @User		User
-PARAM_DEFINE_FLOAT(FW_R_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_RR_I, 0.05f);
 
 // @DisplayName		Roll Integrator Anti-Windup
 // @Description		This limits the range in degrees the integrator can wind up to.
 // @Range		0.0 to 45.0
 // @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f);
+PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
 
 // @DisplayName		Maximum Roll Rate
 // @Description		This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
@@ -126,11 +128,17 @@ PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f);
 PARAM_DEFINE_FLOAT(FW_R_RMAX, 60);
 
 
-PARAM_DEFINE_FLOAT(FW_Y_P, 0);
-PARAM_DEFINE_FLOAT(FW_Y_I, 0);
-PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f);
-PARAM_DEFINE_FLOAT(FW_Y_D, 0);
-PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 1);
+PARAM_DEFINE_FLOAT(FW_YR_P, 0.5);
+PARAM_DEFINE_FLOAT(FW_YR_I, 0.05);
+PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
+PARAM_DEFINE_FLOAT(FW_YR_D, 0); //xxx: remove
+PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0);
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f);
 PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f);
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f);
+PARAM_DEFINE_FLOAT(FW_Y_RMAX, 60);
+PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1.0f);
+
+PARAM_DEFINE_FLOAT(FW_RR_FF, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PR_FF, 0.0f);
+PARAM_DEFINE_FLOAT(FW_YR_FF, 0.0f);

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -83,6 +83,7 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
+#include <mavlink/mavlink_log.h>
 
 #include <ecl/l1/ecl_l1_pos_controller.h>
 #include <external_lgpl/tecs/tecs.h>
@@ -94,6 +95,8 @@
  */
 extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
 
+static int mavlink_fd;
+
 class FixedwingPositionControl
 {
 public:
@@ -160,7 +163,13 @@ private:
 
 	/* land states */
 	/* not in non-abort mode for landing yet */
-	bool land_noreturn;
+	bool land_noreturn_horizontal;
+	bool land_noreturn_vertical;
+	bool land_stayonground;
+	bool land_motor_lim;
+	bool land_onslope;
+
+	float flare_curve_alt_last;
 	/* heading hold */
 	float target_bearing;
 
@@ -206,6 +215,15 @@ private:
 		float throttle_land_max;
 
 		float loiter_hold_radius;
+
+		float heightrate_p;
+
+		float land_slope_angle;
+		float land_slope_length;
+		float land_H1_virt;
+		float land_flare_alt_relative;
+		float land_thrust_lim_horizontal_distance;
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -240,6 +258,15 @@ private:
 		param_t throttle_land_max;
 
 		param_t loiter_hold_radius;
+
+		param_t heightrate_p;
+
+		param_t land_slope_angle;
+		param_t land_slope_length;
+		param_t land_H1_virt;
+		param_t land_flare_alt_relative;
+		param_t land_thrust_lim_horizontal_distance;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -279,6 +306,11 @@ private:
 	 */
 	void		vehicle_setpoint_poll();
 
+	/**
+	 * Get Altitude on the landing glide slope
+	 */
+	float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement);
+
 	/**
 	 * Control position.
 	 */
@@ -286,7 +318,7 @@ private:
 					 const struct mission_item_triplet_s &_mission_item_triplet);
 
 	float calculate_target_airspeed(float airspeed_demand);
-	void calculate_gndspeed_undershoot();
+	void calculate_gndspeed_undershoot(const math::Vector2f &current_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet);
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -338,8 +370,30 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_airspeed_valid(false),
 	_groundspeed_undershoot(0.0f),
 	_global_pos_valid(false),
-	land_noreturn(false)
+	land_noreturn_horizontal(false),
+	land_noreturn_vertical(false),
+	land_stayonground(false),
+	land_motor_lim(false),
+	land_onslope(false),
+	flare_curve_alt_last(0.0f)
 {
+
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	/* safely initialize structs */
+	vehicle_attitude_s			_att = {0};
+	vehicle_attitude_setpoint_s		_att_sp = {0};
+	navigation_capabilities_s		_nav_capabilities = {0};
+	manual_control_setpoint_s		_manual = {0};
+	airspeed_s				_airspeed = {0};
+	vehicle_control_mode_s			_control_mode = {0};
+	vehicle_global_position_s		_global_pos = {0};
+	mission_item_triplet_s			_mission_item_triplet = {0};
+	accel_report				_accel = {0};
+
+
+
+
 	_nav_capabilities.turn_distance = 0.0f;
 
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
@@ -358,6 +412,12 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
 	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 
+	_parameter_handles.land_slope_angle = param_find("FW_LND_ANG");
+	_parameter_handles.land_slope_length = param_find("FW_LND_SLLR");
+	_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
+	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
+	_parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST");
+
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.min_sink_rate = 			param_find("FW_T_SINK_MIN");
 	_parameter_handles.max_sink_rate =			param_find("FW_T_SINK_MAX");
@@ -370,6 +430,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.roll_throttle_compensation = 	param_find("FW_T_RLL2THR");
 	_parameter_handles.speed_weight = 			param_find("FW_T_SPDWEIGHT");
 	_parameter_handles.pitch_damping = 			param_find("FW_T_PTCH_DAMP");
+	_parameter_handles.heightrate_p =			param_find("FW_T_HRATE_P");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -435,6 +496,14 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
 	param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
 
+	param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
+
+	param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
+	param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
+	param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
+	param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
+	param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance));
+
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
 	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
@@ -447,12 +516,13 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
 	_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
 	_tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
-	_tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation));
+	_tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation);
 	_tecs.set_speed_weight(_parameters.speed_weight);
 	_tecs.set_pitch_damping(_parameters.pitch_damping);
 	_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
 	_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
 	_tecs.set_max_climb_rate(_parameters.max_climb_rate);
+	_tecs.set_heightrate_p(_parameters.heightrate_p);
 
 	/* sanity check parameters */
 	if (_parameters.airspeed_max < _parameters.airspeed_min ||
@@ -595,17 +665,29 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
 }
 
 void
-FixedwingPositionControl::calculate_gndspeed_undershoot()
+FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f &current_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet)
 {
 
 	if (_global_pos_valid) {
-		/* get ground speed vector */
-		math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy);
 
-		/* rotate with current attitude */
+		/* rotate ground speed vector with current attitude */
 		math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
 		yaw_vector.normalize();
-		float ground_speed_body = yaw_vector * ground_speed_vector;
+		float ground_speed_body = yaw_vector * ground_speed;
+
+		/* 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;
+		float delta_altitude = 0.0f;
+		if (mission_item_triplet.previous_valid) {
+			distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon);
+			delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude;
+		} else {
+			distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), mission_item_triplet.current.lat, mission_item_triplet.current.lon);
+			delta_altitude = mission_item_triplet.current.altitude -  _global_pos.alt;
+		}
+
+		float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance));
+
 
 		/*
 		 * Ground speed undershoot is the amount of ground velocity not reached
@@ -616,20 +698,25 @@ FixedwingPositionControl::calculate_gndspeed_undershoot()
 		 * not exceeded) travels towards a waypoint (and is not pushed more and more away
 		 * by wind). Not countering this would lead to a fly-away.
 		 */
-		_groundspeed_undershoot = math::max(_parameters.airspeed_min - ground_speed_body, 0.0f);
+		_groundspeed_undershoot = math::max(ground_speed_desired - ground_speed_body, 0.0f);
 
 	} else {
 		_groundspeed_undershoot = 0;
 	}
 }
 
+float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement)
+{
+	return (wp_distance - horizontal_displacement) * tanf(landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative
+}
+
 bool
 FixedwingPositionControl::control_position(const math::Vector2f &current_position, const math::Vector2f &ground_speed,
 		const struct mission_item_triplet_s &mission_item_triplet)
 {
 	bool setpoint = true;
 
-	calculate_gndspeed_undershoot();
+	calculate_gndspeed_undershoot(current_position, ground_speed, mission_item_triplet);
 
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
@@ -659,11 +746,15 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 		_tecs.set_speed_weight(_parameters.speed_weight);
 
 		/* execute navigation once we have a setpoint */
-		if (_setpoint_valid) {
+		if (_setpoint_valid && _control_mode.flag_control_auto_enabled) {
 
 			/* current waypoint (the one currently heading for) */
 			math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
 
+			/* current waypoint (the one currently heading for) */
+			math::Vector2f curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
+
+
 			/* previous waypoint */
 			math::Vector2f prev_wp;
 
@@ -700,7 +791,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 			} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
 				/* waypoint is a plain navigation waypoint */
-				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
 				_att_sp.roll_body = _l1_control.nav_roll();
 				_att_sp.yaw_body = _l1_control.nav_bearing();
 
@@ -715,7 +806,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 				   mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
 
 				/* waypoint is a loiter waypoint */
-				_l1_control.navigate_loiter(next_wp, current_position, mission_item_triplet.current.loiter_radius,
+				_l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius,
 							  mission_item_triplet.current.loiter_direction, ground_speed);
 				_att_sp.roll_body = _l1_control.nav_roll();
 				_att_sp.yaw_body = _l1_control.nav_bearing();
@@ -728,11 +819,12 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 			} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
 
+				/* Horizontal landing control */
 				/* switch to heading hold for the last meters, continue heading hold after */
-
-				float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY());
+				float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY());
 				//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
-				if (wp_distance < 15.0f || land_noreturn) {
+				const float heading_hold_distance = 15.0f;
+				if (wp_distance < heading_hold_distance || land_noreturn_horizontal) {
 
 					/* heading hold, along the line connecting this and the last waypoint */
 					
@@ -741,75 +833,133 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 					// 	target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
 					// } else {
 
-					if (!land_noreturn)
+					if (!land_noreturn_horizontal) //set target_bearing in first occurrence
 						target_bearing = _att.yaw;
 					//}
 
-					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
+//					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);
 
-					if (altitude_error > -5.0f)
-						land_noreturn = true;
+					land_noreturn_horizontal = true;
 
 				} else {
 
 					/* normal navigation */
-					_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+					_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
 				}
 
-				/* do not go down too early */
-				if (wp_distance > 50.0f) {
-					altitude_error = (_mission_item_triplet.current.altitude + 25.0f) - _global_pos.alt;
-				}
-
-
 				_att_sp.roll_body = _l1_control.nav_roll();
 				_att_sp.yaw_body = _l1_control.nav_bearing();
 
+
+				/* Vertical landing control */
+				//xxx: using the tecs altitude controller for slope control for now
+
+//				/* do not go down too early */
+//				if (wp_distance > 50.0f) {
+//					altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
+//				}
 				/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
 				// XXX this could make a great param
 
-				float flare_angle_rad = math::radians(10.0f);//math::radians(mission_item_triplet.current.param1)
+				float flare_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1)
 				float land_pitch_min = math::radians(5.0f);
 				float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
-				float airspeed_land = _parameters.airspeed_min;
-				float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
-
-				if (altitude_error > -4.0f) {
+				float airspeed_land = 1.3f * _parameters.airspeed_min;
+				float airspeed_approach = 1.3f * _parameters.airspeed_min;
+
+				float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle);
+				float flare_relative_alt = _parameters.land_flare_alt_relative;
+				float motor_lim_horizontal_distance = _parameters.land_thrust_lim_horizontal_distance;//be generous here as we currently have to rely on the user input for the waypoint
+				float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length;
+				float H1 = _parameters.land_H1_virt;
+				float H0 = flare_relative_alt + H1;
+				float d1 = flare_relative_alt/tanf(landing_slope_angle_rad);
+				float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0));
+				float flare_length = - logf(H1/H0) * flare_constant;//d1+20.0f;//-logf(0.01f/flare_relative_alt);
+				float horizontal_slope_displacement = (flare_length - d1);
+				float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
+				float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
+
+				if ( (_global_pos.alt < _mission_item_triplet.current.altitude + flare_relative_alt) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
 
 					/* land with minimal speed */
 
-					/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
-					_tecs.set_speed_weight(2.0f);
-
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
-								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    false, flare_angle_rad,
-								    0.0f, _parameters.throttle_max, throttle_land,
-								    math::radians(-10.0f), math::radians(15.0f));
+//					/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
+//					_tecs.set_speed_weight(2.0f);
 
 					/* kill the throttle if param requests it */
-					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
+					throttle_max = _parameters.throttle_max;
+
+					 if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) {
+						throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
+						if (!land_motor_lim) {
+							land_motor_lim  = true;
+							mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, limit throttle");
+						}
+
+					 }
+
+					float flare_curve_alt =   _mission_item_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1;
+
+					/* avoid climbout */
+					if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
+					{
+						flare_curve_alt = mission_item_triplet.current.altitude;
+						land_stayonground = true;
+					}
+
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land),
+													    _airspeed.indicated_airspeed_m_s, eas2tas,
+													    false, flare_angle_rad,
+													    0.0f, throttle_max, throttle_land,
+													    flare_angle_rad,  math::radians(15.0f));
 
 					/* 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));
+					if (!land_noreturn_vertical) {
+						mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare");
+						land_noreturn_vertical = true;
+					}
+					//warnx("Landing:  flare, _global_pos.alt  %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
 
-				} else if (wp_distance < 60.0f && altitude_error > -20.0f) {
+					flare_curve_alt_last = flare_curve_alt;
 
-					/* minimize speed to approach speed */
+				} else if (wp_distance < L_wp_distance) {
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(airspeed_approach),
+					/* minimize speed to approach speed, stay on landing slope */
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach),
 								    _airspeed.indicated_airspeed_m_s, eas2tas,
 								    false, flare_angle_rad,
 								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+					//warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length);
+
+					if (!land_onslope) {
+
+						mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, on slope");
+						land_onslope = true;
+					}
 
 				} else {
 
-					/* normal cruise speed */
+					 /* intersect glide slope:
+					 * if current position is higher or within 10m of slope follow the glide slope
+					 * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
+					 * */
+					float altitude_desired = _global_pos.alt;
+					if (_global_pos.alt > landing_slope_alt_desired - 10.0f) {
+						/* stay on slope */
+						altitude_desired = landing_slope_alt_desired;
+						//warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement);
+					} else {
+						/* continue horizontally */
+						altitude_desired =  math::max(_global_pos.alt, L_altitude);
+						//warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance);
+					}
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach),
 								    _airspeed.indicated_airspeed_m_s, eas2tas,
 								    false, math::radians(_parameters.pitch_limit_min),
 								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@@ -818,12 +968,12 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 			} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
 
-				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
 				_att_sp.roll_body = _l1_control.nav_roll();
 				_att_sp.yaw_body = _l1_control.nav_bearing();
 
 				/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
-				if (altitude_error > 10.0f) {
+				if (altitude_error > 15.0f) {
 
 					/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
 					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
@@ -903,7 +1053,11 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 		/* reset land state */
 		if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) {
-			land_noreturn = false;
+			land_noreturn_horizontal = false;
+			land_noreturn_vertical = false;
+			land_stayonground = false;
+			land_motor_lim = false;
+			land_onslope = false;
 		}
 
 		if (was_circle_mode && !_l1_control.circle_mode()) {

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -111,3 +111,11 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
 
 
 PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
+
+PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
+
+PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f);
+PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
+PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
+PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
+PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f);

src/modules/mavlink/missionlib.c

@@ -276,7 +276,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 			next_setpoint_index = index + 1;
 		}
 
-		while (next_setpoint_index < wpm->size - 1) {
+		while (next_setpoint_index < wpm->size) {
 
 			if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
 					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||