fw: att fix initialization and add parameter to disable coordinated turns at low speed

2 files changed, 8 insertions, 7 deletions

diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index 04293efd6..a4ecc48a2 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -59,21 +59,22 @@ ECL_YawController::ECL_YawController() :
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
 	_bodyrate_setpoint(0.0f),
-	_coordinated(0.0f)
+	_coordinated_min_speed(1.0f)
 {
 }
 
 float ECL_YawController::control_attitude(float roll, float pitch,
-		float speed_body_u, float speed_body_w,
+		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 (_coordinated > 0.1) {
+	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) {
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index 154471caf..b5ae1e6af 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -57,7 +57,7 @@ public:
 	ECL_YawController();
 
 	float control_attitude(float roll, float pitch,
-			float speed_body_u, float speed_body_w,
+			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,
@@ -85,8 +85,8 @@ public:
 	void set_k_roll_ff(float roll_ff) {
 		_roll_ff = roll_ff;
 	}
-	void set_coordinated(float coordinated) {
-		_coordinated = coordinated;
+	void set_coordinated_min_speed(float coordinated_min_speed) {
+		_coordinated_min_speed = coordinated_min_speed;
 	}
 
 
@@ -115,7 +115,7 @@ private:
 	float _rate_error;
 	float _rate_setpoint;
 	float _bodyrate_setpoint;
-	float _coordinated;
+	float _coordinated_min_speed;
 
 };