diff --git a/CMakeLists.txt b/CMakeLists.txt
index ffd1e1737a..742c50bee4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -61,3 +61,4 @@ px4_add_module(
 	DEPENDS
 		platforms__common
 	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/EKF/geo.cpp b/EKF/geo.cpp
index d59a35ba16..7fca38888c 100644
--- a/EKF/geo.cpp
+++ b/EKF/geo.cpp
@@ -826,4 +826,4 @@ float _wrap_360(float bearing)
 
 	return bearing;
 }
-#endif //POSIX_SHARED
+#endif //POSIX_SHARED
\ No newline at end of file
diff --git a/attitude_fw/ecl_controller.cpp b/attitude_fw/ecl_controller.cpp
index d00850638f..213b601838 100644
--- a/attitude_fw/ecl_controller.cpp
+++ b/attitude_fw/ecl_controller.cpp
@@ -48,6 +48,25 @@
 
 #include "ecl_controller.h"
 
+#include <stdio.h>
+#include <mathlib/mathlib.h>
+
+ECL_Controller::ECL_Controller(const char *name) :
+	_last_run(0),
+	_tc(0.1f),
+	_k_p(0.0f),
+	_k_i(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate(0.0f),
+	_last_output(0.0f),
+	_integrator(0.0f),
+	_rate_error(0.0f),
+	_rate_setpoint(0.0f),
+	_bodyrate_setpoint(0.0f)
+{
+}
+
 void ECL_Controller::reset_integrator()
 {
 	_integrator = 0.0f;
@@ -104,7 +123,7 @@ float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float m
 {
 	float airspeed_result = airspeed;
 
-	if (!ISFINITE(airspeed)) {
+	if (!PX4_ISFINITE(airspeed)) {
 		/* airspeed is NaN, +- INF or not available, pick center of band */
 		airspeed_result = 0.5f * (minspeed + maxspeed);
 
diff --git a/attitude_fw/ecl_controller.h b/attitude_fw/ecl_controller.h
index 183745e7f3..fe8909dd63 100644
--- a/attitude_fw/ecl_controller.h
+++ b/attitude_fw/ecl_controller.h
@@ -48,8 +48,9 @@
 
 #pragma once
 
-#include "ecl/ecl.h"
-#include "mathlib/mathlib.h"
+#include <stdbool.h>
+#include <stdint.h>
+#include <systemlib/perf_counter.h>
 
 struct ECL_ControlData {
 	float roll;
@@ -76,7 +77,7 @@ struct ECL_ControlData {
 class __EXPORT ECL_Controller
 {
 public:
-	ECL_Controller() = default;
+	ECL_Controller(const char *name);
 	~ECL_Controller() = default;
 
 	virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0;
@@ -90,7 +91,7 @@ public:
 	void set_k_ff(float k_ff);
 	void set_integrator_max(float max);
 	void set_max_rate(float max_rate);
-	void set_bodyrate_setpoint(float rate) { _bodyrate_setpoint = rate; }
+	void set_bodyrate_setpoint(float rate) {_bodyrate_setpoint = rate;}
 
 	/* Getters */
 	float get_rate_error();
@@ -100,22 +101,17 @@ public:
 	void reset_integrator();
 
 protected:
-	uint64_t _last_run{0};
-
-	float _tc{0.5f};
-	float _k_p{0.0f};
-	float _k_i{0.0f};
-	float _k_ff{0.0f};
-
-	float _integrator_max{0.0f};
-	float _integrator{0.0f};
-
-	float _rate_setpoint{0.0f};
-	float _bodyrate_setpoint{0.0f};
-
-	float _max_rate{0.0f};
-	float _rate_error{0.0f};
-	float _last_output{0.0f};
-
+	uint64_t _last_run;
+	float _tc;
+	float _k_p;
+	float _k_i;
+	float _k_ff;
+	float _integrator_max;
+	float _max_rate;
+	float _last_output;
+	float _integrator;
+	float _rate_error;
+	float _rate_setpoint;
+	float _bodyrate_setpoint;
 	float constrain_airspeed(float airspeed, float minspeed, float maxspeed);
 };
diff --git a/attitude_fw/ecl_pitch_controller.cpp b/attitude_fw/ecl_pitch_controller.cpp
index b5ab5df6eb..8fa2930c9c 100644
--- a/attitude_fw/ecl_pitch_controller.cpp
+++ b/attitude_fw/ecl_pitch_controller.cpp
@@ -35,20 +35,34 @@
  * @file ecl_pitch_controller.cpp
  * Implementation of a simple orthogonal pitch PID controller.
  *
- * Authors and acknowledgments in header.
+ * Authors and acknowledgements in header.
  */
 
 #include "ecl_pitch_controller.h"
+#include <math.h>
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
+
+ECL_PitchController::ECL_PitchController() :
+	ECL_Controller("pitch"),
+	_max_rate_neg(0.0f),
+	_roll_ff(0.0f)
+{
+}
 
 float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
 {
-	/* Do not calculate control signal with bad inputs */
-	if (!(ISFINITE(ctl_data.pitch_setpoint) &&
-	      ISFINITE(ctl_data.roll) &&
-	      ISFINITE(ctl_data.pitch) &&
-	      ISFINITE(ctl_data.airspeed))) {
 
-		ECL_WARN("not controlling pitch");
+	/* Do not calculate control signal with bad inputs */
+	if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
+	      PX4_ISFINITE(ctl_data.roll) &&
+	      PX4_ISFINITE(ctl_data.pitch) &&
+	      PX4_ISFINITE(ctl_data.airspeed))) {
+		warnx("not controlling pitch");
 		return _rate_setpoint;
 	}
 
@@ -59,13 +73,14 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
 	_rate_setpoint =  pitch_error / _tc;
 
 	/* limit the rate */
-	if (_max_rate >= 0.0f && _max_rate_neg >= 0.0f) {
+	if (_max_rate > 0.01f && _max_rate_neg > 0.01f) {
 		if (_rate_setpoint > 0.0f) {
 			_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
 
 		} else {
 			_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
 		}
+
 	}
 
 	return _rate_setpoint;
@@ -74,22 +89,21 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
 float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(ISFINITE(ctl_data.roll) &&
-	      ISFINITE(ctl_data.pitch) &&
-	      ISFINITE(ctl_data.body_y_rate) &&
-	      ISFINITE(ctl_data.body_z_rate) &&
-	      ISFINITE(ctl_data.yaw_rate_setpoint) &&
-	      ISFINITE(ctl_data.airspeed_min) &&
-	      ISFINITE(ctl_data.airspeed_max) &&
-	      ISFINITE(ctl_data.scaler))) {
-
+	if (!(PX4_ISFINITE(ctl_data.roll) &&
+	      PX4_ISFINITE(ctl_data.pitch) &&
+	      PX4_ISFINITE(ctl_data.body_y_rate) &&
+	      PX4_ISFINITE(ctl_data.body_z_rate) &&
+	      PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
+	      PX4_ISFINITE(ctl_data.airspeed_min) &&
+	      PX4_ISFINITE(ctl_data.airspeed_max) &&
+	      PX4_ISFINITE(ctl_data.scaler))) {
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
 
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
-	float dt = dt_micros * 1e-6f;
+	float dt = (float)dt_micros * 1e-6f;
 
 	/* lock integral for long intervals */
 	bool lock_integrator = ctl_data.lock_integrator;
diff --git a/attitude_fw/ecl_pitch_controller.h b/attitude_fw/ecl_pitch_controller.h
index 2e3a8c672d..d6ae88e66c 100644
--- a/attitude_fw/ecl_pitch_controller.h
+++ b/attitude_fw/ecl_pitch_controller.h
@@ -49,17 +49,21 @@
 #ifndef ECL_PITCH_CONTROLLER_H
 #define ECL_PITCH_CONTROLLER_H
 
+#include <stdbool.h>
+#include <stdint.h>
+
 #include "ecl_controller.h"
 
-class __EXPORT ECL_PitchController final : public ECL_Controller
+class __EXPORT ECL_PitchController :
+	public ECL_Controller
 {
 public:
-	ECL_PitchController() = default;
+	ECL_PitchController();
 	~ECL_PitchController() = default;
 
-	float control_attitude(const struct ECL_ControlData &ctl_data) override;
-	float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
-	float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
+	float control_attitude(const struct ECL_ControlData &ctl_data);
+	float control_euler_rate(const struct ECL_ControlData &ctl_data);
+	float control_bodyrate(const struct ECL_ControlData &ctl_data);
 
 	/* Additional Setters */
 	void set_max_rate_pos(float max_rate_pos)
@@ -72,8 +76,14 @@ public:
 		_max_rate_neg = max_rate_neg;
 	}
 
+	void set_roll_ff(float roll_ff)
+	{
+		_roll_ff = roll_ff;
+	}
+
 protected:
-	float _max_rate_neg{0.0f};
+	float _max_rate_neg;
+	float _roll_ff;
 };
 
 #endif // ECL_PITCH_CONTROLLER_H
diff --git a/attitude_fw/ecl_roll_controller.cpp b/attitude_fw/ecl_roll_controller.cpp
index 6e139a9989..12d2480643 100644
--- a/attitude_fw/ecl_roll_controller.cpp
+++ b/attitude_fw/ecl_roll_controller.cpp
@@ -35,15 +35,27 @@
  * @file ecl_roll_controller.cpp
  * Implementation of a simple orthogonal roll PID controller.
  *
- * Authors and acknowledgments in header.
+ * Authors and acknowledgements in header.
  */
 
+#include <ecl/ecl.h>
 #include "ecl_roll_controller.h"
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
+
+ECL_RollController::ECL_RollController() :
+	ECL_Controller("roll")
+{
+}
 
 float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(ISFINITE(ctl_data.roll_setpoint) && ISFINITE(ctl_data.roll))) {
+	if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) {
 		return _rate_setpoint;
 	}
 
@@ -55,7 +67,7 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat
 
 	/* limit the rate */ //XXX: move to body angluar rates
 
-	if (_max_rate >= 0.0f) {
+	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;
 	}
@@ -66,21 +78,20 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat
 float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(ISFINITE(ctl_data.pitch) &&
-	      ISFINITE(ctl_data.body_x_rate) &&
-	      ISFINITE(ctl_data.body_z_rate) &&
-	      ISFINITE(ctl_data.yaw_rate_setpoint) &&
-	      ISFINITE(ctl_data.airspeed_min) &&
-	      ISFINITE(ctl_data.airspeed_max) &&
-	      ISFINITE(ctl_data.scaler))) {
-
+	if (!(PX4_ISFINITE(ctl_data.pitch) &&
+	      PX4_ISFINITE(ctl_data.body_x_rate) &&
+	      PX4_ISFINITE(ctl_data.body_z_rate) &&
+	      PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
+	      PX4_ISFINITE(ctl_data.airspeed_min) &&
+	      PX4_ISFINITE(ctl_data.airspeed_max) &&
+	      PX4_ISFINITE(ctl_data.scaler))) {
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
 
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
-	float dt = dt_micros * 1e-6f;
+	float dt = (float)dt_micros * 1e-6f;
 
 	/* lock integral for long intervals */
 	bool lock_integrator = ctl_data.lock_integrator;
@@ -129,4 +140,6 @@ float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_d
 	_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
 
 	return control_bodyrate(ctl_data);
+
 }
+
diff --git a/attitude_fw/ecl_roll_controller.h b/attitude_fw/ecl_roll_controller.h
index ccb30f0b4b..cb127ef029 100644
--- a/attitude_fw/ecl_roll_controller.h
+++ b/attitude_fw/ecl_roll_controller.h
@@ -49,17 +49,21 @@
 #ifndef ECL_ROLL_CONTROLLER_H
 #define ECL_ROLL_CONTROLLER_H
 
+#include <stdbool.h>
+#include <stdint.h>
+
 #include "ecl_controller.h"
 
-class __EXPORT ECL_RollController final : public ECL_Controller
+class __EXPORT ECL_RollController :
+	public ECL_Controller
 {
 public:
-	ECL_RollController() = default;
+	ECL_RollController();
 	~ECL_RollController() = default;
 
-	float control_attitude(const struct ECL_ControlData &ctl_data) override;
-	float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
-	float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
+	float control_attitude(const struct ECL_ControlData &ctl_data);
+	float control_euler_rate(const struct ECL_ControlData &ctl_data);
+	float control_bodyrate(const struct ECL_ControlData &ctl_data);
 };
 
 #endif // ECL_ROLL_CONTROLLER_H
diff --git a/attitude_fw/ecl_wheel_controller.cpp b/attitude_fw/ecl_wheel_controller.cpp
index 396571520f..527af899fe 100644
--- a/attitude_fw/ecl_wheel_controller.cpp
+++ b/attitude_fw/ecl_wheel_controller.cpp
@@ -35,26 +35,36 @@
  * @file ecl_wheel_controller.cpp
  * Implementation of a simple PID wheel controller for heading tracking.
  *
- * Authors and acknowledgments in header.
+ * Authors and acknowledgements in header.
  */
 
 #include "ecl_wheel_controller.h"
-
+#include <stdint.h>
+#include <float.h>
 #include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
+#include <ecl/ecl.h>
+
+ECL_WheelController::ECL_WheelController() :
+	ECL_Controller("wheel")
+{
+}
 
 float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(ISFINITE(ctl_data.body_z_rate) &&
-	      ISFINITE(ctl_data.groundspeed) &&
-	      ISFINITE(ctl_data.groundspeed_scaler))) {
+	if (!(PX4_ISFINITE(ctl_data.body_z_rate) &&
+	      PX4_ISFINITE(ctl_data.groundspeed) &&
+	      PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
 
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
-	float dt = dt_micros * 1e-6f;
+	float dt = (float)dt_micros * 1e-6f;
 
 	/* lock integral for long intervals */
 	bool lock_integrator = ctl_data.lock_integrator;
@@ -99,11 +109,12 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
+
 float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(ISFINITE(ctl_data.yaw_setpoint) &&
-	      ISFINITE(ctl_data.yaw))) {
+	if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&
+	      PX4_ISFINITE(ctl_data.yaw))) {
 		return _rate_setpoint;
 	}
 
@@ -114,13 +125,14 @@ float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_da
 	_rate_setpoint =  yaw_error / _tc;
 
 	/* limit the rate */
-	if (_max_rate >= 0.0f) {
+	if (_max_rate > 0.01f) {
 		if (_rate_setpoint > 0.0f) {
 			_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
 
 		} else {
 			_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
 		}
+
 	}
 
 	return _rate_setpoint;
diff --git a/attitude_fw/ecl_wheel_controller.h b/attitude_fw/ecl_wheel_controller.h
index f741f83d09..1c84aa1399 100644
--- a/attitude_fw/ecl_wheel_controller.h
+++ b/attitude_fw/ecl_wheel_controller.h
@@ -49,17 +49,23 @@
 #ifndef ECL_HEADING_CONTROLLER_H
 #define ECL_HEADING_CONTROLLER_H
 
+#include <stdbool.h>
+#include <stdint.h>
+
 #include "ecl_controller.h"
 
-class __EXPORT ECL_WheelController final : public ECL_Controller
+class __EXPORT ECL_WheelController :
+	public ECL_Controller
 {
 public:
-	ECL_WheelController() = default;
+	ECL_WheelController();
 	~ECL_WheelController() = default;
 
-	float control_attitude(const struct ECL_ControlData &ctl_data) override;
-	float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
-	float control_euler_rate(const struct ECL_ControlData &ctl_data) override { return 0; }
+	float control_attitude(const struct ECL_ControlData &ctl_data);
+
+	float control_bodyrate(const struct ECL_ControlData &ctl_data);
+
+	float control_euler_rate(const struct ECL_ControlData &ctl_data) {return 0;}
 };
 
 #endif // ECL_HEADING_CONTROLLER_H
diff --git a/attitude_fw/ecl_yaw_controller.cpp b/attitude_fw/ecl_yaw_controller.cpp
index 12e12e2025..52c1bab4c5 100644
--- a/attitude_fw/ecl_yaw_controller.cpp
+++ b/attitude_fw/ecl_yaw_controller.cpp
@@ -35,18 +35,54 @@
  * @file ecl_yaw_controller.cpp
  * Implementation of a simple orthogonal coordinated turn yaw PID controller.
  *
- * Authors and acknowledgments in header.
+ * Authors and acknowledgements in header.
  */
 
 #include "ecl_yaw_controller.h"
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
+#include <ecl/ecl.h>
+
+ECL_YawController::ECL_YawController() :
+	ECL_Controller("yaw"),
+	_coordinated_min_speed(1.0f),
+	_max_rate(0.0f), /* disable by default */
+	_coordinated_method(0)
+{
+}
 
 float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
+{
+	switch (_coordinated_method) {
+	case COORD_METHOD_OPEN:
+		return control_attitude_impl_openloop(ctl_data);
+
+	case COORD_METHOD_CLOSEACC:
+		return control_attitude_impl_accclosedloop(ctl_data);
+
+	default:
+		static hrt_abstime last_print = 0;
+
+		if (ecl_elapsed_time(&last_print) > 5e6) {
+			warnx("invalid param setting FW_YCO_METHOD");
+			last_print = ecl_absolute_time();
+		}
+	}
+
+	return _rate_setpoint;
+}
+
+float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(ISFINITE(ctl_data.roll) &&
-	      ISFINITE(ctl_data.pitch) &&
-	      ISFINITE(ctl_data.roll_rate_setpoint) &&
-	      ISFINITE(ctl_data.pitch_rate_setpoint))) {
+	if (!(PX4_ISFINITE(ctl_data.roll) &&
+	      PX4_ISFINITE(ctl_data.pitch) &&
+	      PX4_ISFINITE(ctl_data.roll_rate_setpoint) &&
+	      PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) {
 		return _rate_setpoint;
 	}
 
@@ -84,12 +120,12 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data
 
 	/* limit the rate */ //XXX: move to body angluar rates
 
-	if (_max_rate >= 0.0f) {
+	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;
 	}
 
-	if (!ISFINITE(_rate_setpoint)) {
+	if (!PX4_ISFINITE(_rate_setpoint)) {
 		warnx("yaw rate sepoint not finite");
 		_rate_setpoint = 0.0f;
 	}
@@ -100,17 +136,17 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data
 float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(ISFINITE(ctl_data.roll) && ISFINITE(ctl_data.pitch) && ISFINITE(ctl_data.body_y_rate) &&
-	      ISFINITE(ctl_data.body_z_rate) && ISFINITE(ctl_data.pitch_rate_setpoint) &&
-	      ISFINITE(ctl_data.airspeed_min) && ISFINITE(ctl_data.airspeed_max) &&
-	      ISFINITE(ctl_data.scaler))) {
+	if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.body_y_rate) &&
+	      PX4_ISFINITE(ctl_data.body_z_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) &&
+	      PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) &&
+	      PX4_ISFINITE(ctl_data.scaler))) {
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
 
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
-	float dt = dt_micros * 1e-6f;
+	float dt = (float)dt_micros * 1e-6f;
 
 	/* lock integral for long intervals */
 	bool lock_integrator = ctl_data.lock_integrator;
@@ -122,7 +158,7 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
 	/* input conditioning */
 	float airspeed = ctl_data.airspeed;
 
-	if (!ISFINITE(airspeed)) {
+	if (!PX4_ISFINITE(airspeed)) {
 		/* airspeed is NaN, +- INF or not available, pick center of band */
 		airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
 
@@ -130,6 +166,12 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
 		airspeed = ctl_data.airspeed_min;
 	}
 
+	/* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */
+	if (_coordinated_method == COORD_METHOD_CLOSEACC) {
+		// XXX lateral acceleration needs to go into integrator with a gain
+		//_bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch)));
+	}
+
 	/* Calculate body angular rate error */
 	_rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; // body angular rate error
 
@@ -160,9 +202,16 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
 	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler *
 		       ctl_data.scaler;  //scaler is proportional to 1/airspeed
 
+
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
+float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data)
+{
+	/* dont set a rate setpoint */
+	return 0.0f;
+}
+
 float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_data)
 {
 	/* Transform setpoint to body angular rates (jacobian) */
diff --git a/attitude_fw/ecl_yaw_controller.h b/attitude_fw/ecl_yaw_controller.h
index 17a854666a..ef23274b7d 100644
--- a/attitude_fw/ecl_yaw_controller.h
+++ b/attitude_fw/ecl_yaw_controller.h
@@ -48,17 +48,48 @@
 #ifndef ECL_YAW_CONTROLLER_H
 #define ECL_YAW_CONTROLLER_H
 
+#include <stdbool.h>
+#include <stdint.h>
+
 #include "ecl_controller.h"
 
-class __EXPORT ECL_YawController final : public ECL_Controller
+class __EXPORT ECL_YawController :
+	public ECL_Controller
 {
 public:
-	ECL_YawController() = default;
+	ECL_YawController();
 	~ECL_YawController() = default;
 
-	float control_attitude(const struct ECL_ControlData &ctl_data) override;
-	float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
-	float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
+	float control_attitude(const struct ECL_ControlData &ctl_data);
+	float control_euler_rate(const struct ECL_ControlData &ctl_data);
+	float control_bodyrate(const struct ECL_ControlData &ctl_data);
+
+	/* Additional setters */
+	void set_coordinated_min_speed(float coordinated_min_speed)
+	{
+		_coordinated_min_speed = coordinated_min_speed;
+	}
+
+	void set_coordinated_method(int32_t coordinated_method)
+	{
+		_coordinated_method = coordinated_method;
+	}
+
+	enum {
+		COORD_METHOD_OPEN = 0,
+		COORD_METHOD_CLOSEACC = 1
+	};
+
+protected:
+	float _coordinated_min_speed;
+	float _max_rate;
+
+	int32_t _coordinated_method;
+
+	float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data);
+
+	float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data);
+
 };
 
 #endif // ECL_YAW_CONTROLLER_H