wip, fw attitude ctrl: split into attitude and rate, compiles, untested

7 files changed, 298 insertions, 84 deletions

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 9b86d6971..30a46b0e7 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -56,29 +56,9 @@ ECL_PitchController::ECL_PitchController() :
 {
 }
 
-float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float yaw_rate, 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 = dt_micros / 1000000;
-
-	/* 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;
@@ -106,20 +86,61 @@ 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;
-	/* /* Apply P controller: rate setpoint from current error and time constant */
-	float theta_dot_setpoint =  pitch_error / _tc;
 
-	/* Transform setpoint to body angular rates */
-	_rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian //XXX: use desired yaw_rate?
+	/*  Apply P controller: rate setpoint from current error and time constant */
+	float _rate_setpoint =  pitch_error / _tc;
 
 	/* add turn offset */
 	_rate_setpoint += turn_offset;
 
+	/* 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;
+		}
+
+	}
+
+	return _rate_setpoint;
+}
+
+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 = dt_micros / 1000000;
+
+	/* 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;
+	}
+
+	/* 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_rate_body = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
+	float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
 
-	_rate_error = _rate_setpoint - pitch_rate_body;
+	_rate_error = _bodyrate_setpoint - pitch_bodyrate;
 
 	float ilimit_scaled = 0.0f;
 
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
index 41aa48677..ef6129d02 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 yaw_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 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();
 
@@ -94,6 +100,10 @@ public:
 		return _rate_setpoint;
 	}
 
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 
 	uint64_t _last_run;
@@ -109,6 +119,7 @@ private:
 	float _integrator;
 	float _rate_error;
 	float _rate_setpoint;
+	float _bodyrate_setpoint;
 	float _max_deflection_rad;
 };
 
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 36186ce68..3afda3176 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -53,18 +53,38 @@ ECL_RollController::ECL_RollController() :
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
+	_bodyrate_setpoint(0.0f),
 	_max_deflection_rad(math::radians(45.0f))
 {
 
 }
 
-float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, float pitch, float yaw_rate,
-				  float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+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_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);
 	_last_run = ecl_absolute_time();
-
 	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
 
 	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
@@ -78,25 +98,15 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 		airspeed = airspeed_min;
 	}
 
-	float roll_error = roll_setpoint - roll;
-
-	/* Apply P controller */
-	float phi_dot_setpoint = roll_error / _tc;
 
 	/* Transform setpoint to body angular rates */
-	_rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian //XXX: use desired yaw_rate?
-
-	/* 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;
-	}
+	_bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian
 
 	/* Transform estimation to body angular rates */
-	float roll_rate_body = roll_rate - sinf(pitch) * yaw_rate; //jacobian
+	float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian
 
 	/* Calculate body angular rate error */
-	_rate_error = _rate_setpoint - roll_rate_body; //body angular rate error
+	_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
 
 	float ilimit_scaled = 0.0f;
 
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
index e19812ea8..f94db0dc7 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 pitch, float yaw_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();
 
@@ -90,6 +95,10 @@ public:
 		return _rate_setpoint;
 	}
 
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 	uint64_t _last_run;
 	float _tc;
@@ -102,6 +111,7 @@ private:
 	float _integrator;
 	float _rate_error;
 	float _rate_setpoint;
+	float _bodyrate_setpoint;
 	float _max_deflection_rad;
 };
 
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index 9601fa544..d2cd9cf04 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -47,37 +47,95 @@
 
 ECL_YawController::ECL_YawController() :
 	_last_run(0),
+	_tc(0.1f),
 	_last_output(0.0f),
+	_integrator(0.0f),
+	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
+	_bodyrate_setpoint(0.0f),
 	_max_deflection_rad(math::radians(45.0f))
 
 {
 
 }
 
-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_attitude(float roll, float pitch,
+		float speed_body_u, float speed_body_w,
+		float roll_rate_setpoint, float pitch_rate_setpoint)
+{
+	/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
+	_rate_setpoint = 0.0f;
+	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;
+	}
+
+	/* 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_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 = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
 
-//	float psi_dot = 0.0f;
-//		float denumerator = (speed_body[0] * cosf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[2] * sinf(att_sp->pitch_body));
-//		if(denumerator != 0.0f) {
-//			psi_dot = (speed_body[2] * phi_dot + 9.81f * sinf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[0] * theta_dot * sinf(att_sp->roll_body))
-//				/ (speed_body[0] * cosf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[2] * sinf(att_sp->pitch_body));
-//		}
+	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+	float k_i_rate = _k_i * _tc;
 
-	/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
-	_last_output = 0.0f;
-	float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
-	if(denumerator != 0.0f) { //XXX: floating point comparison
-		_last_output = (speed_body_w * roll_rate_desired + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_desired * sinf(roll)) / denumerator;
+	/* 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
+
+	float ilimit_scaled = 0.0f;
+
+	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+	float id = _rate_error * k_i_rate * dt * scaler;
+
+	/*
+	 * anti-windup: do not allow integrator to increase into the
+	 * wrong direction if actuator is at limit
+	 */
+	if (_last_output < -_max_deflection_rad) {
+		/* only allow motion to center: increase value */
+		id = math::max(id, 0.0f);
+	} else if (_last_output > _max_deflection_rad) {
+		/* only allow motion to center: decrease value */
+		id = math::min(id, 0.0f);
+	}
+
+	_integrator += id;
+	}
+
+	/* 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;
+
 	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
 }
 
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index fe0abb956..d5aaa617f 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,24 +51,69 @@
 #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_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_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_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;
+	}
+
 
+	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 _rate_setpoint;
+	float _tc;
+	float _k_p;
+	float _k_i;
+	float _k_d;
+	float _integrator_max;
+	float _max_rate;
+	float  _roll_ff;
 	float _last_output;
+	float _integrator;
+	float _rate_error;
+	float _rate_setpoint;
+	float _bodyrate_setpoint;
 	float _max_deflection_rad;
 
 };
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 8cb515dcc..422e94ba1 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/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 */
 
@@ -227,6 +231,11 @@ private:
 	void		vehicle_setpoint_poll();
 
 	/**
+	 * Check for global position updates.
+	 */
+	void		global_pos_poll();
+
+	/**
 	 * Shim for calling task_main from task_create.
 	 */
 	static void	task_main_trampoline(int argc, char *argv[]);
@@ -261,6 +270,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
 	_manual_sub(-1),
+	_global_pos_sub(-1),
 
 /* publications */
 	_rate_sp_pub(-1),
@@ -375,7 +385,7 @@ FixedwingAttitudeControl::parameters_update()
 	_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_p(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));
@@ -453,6 +463,18 @@ FixedwingAttitudeControl::vehicle_setpoint_poll()
 }
 
 void
+FixedwingAttitudeControl::global_pos_poll()
+{
+	/* check if there is a new setpoint */
+	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[])
 {
 	att_control::g_control->task_main();
@@ -476,6 +498,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 +574,8 @@ FixedwingAttitudeControl::task_main()
 
 			vehicle_manual_poll();
 
+			global_pos_poll();
+
 			/* lock integrator until control is started */
 			bool lock_integrator;
 
@@ -635,16 +660,41 @@ FixedwingAttitudeControl::task_main()
 					}
 				}
 
-				float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, _att.pitch, _att.yawspeed,
-								    airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+				/* Prepare speed_body_u and speed_body_w */
+				float speed_body_u = 0.0f;
+				float speed_body_w = 0.0f;
+				if(_att.R_valid) 	{
+					speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[0][1] * _global_pos.vy + _att.R[0][2] * _global_pos.vz;
+//					speed_body_v = _att.R[1][0] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[1][2] * _global_pos.vz;
+					speed_body_w = _att.R[2][0] * _global_pos.vx + _att.R[2][1] * _global_pos.vy + _att.R[2][2] * _global_pos.vz;
+				} else	{
+					warnx("Did not get a valid R\n");
+				}
+
+				/* Run attitude controllers */
+				_roll_ctrl.control_attitude(roll_sp, _att.roll);
+				_pitch_ctrl.control_attitude(roll_sp, _att.roll, _att.pitch, airspeed);
+				_yaw_ctrl.control_attitude(_att.roll, _att.pitch,
+						speed_body_u,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_rad = _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_rad)) ? roll_rad * actuator_scaling : 0.0f;
 
-				float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, _att.yawspeed, airspeed_scaling,
-								      lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+				float pitch_rad = _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_rad)) ? pitch_rad * actuator_scaling : 0.0f;
 
-				float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
-								  _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+				float yaw_rad = _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_rad)) ? yaw_rad * actuator_scaling : 0.0f;
 
 				/* throttle passed through */