2 files changed, 22 insertions, 32 deletions

diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index b786acf24..9601fa544 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -47,12 +47,10 @@
 
 ECL_YawController::ECL_YawController() :
 	_last_run(0),
-	_last_error(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)
+	_rate_setpoint(0.0f),
+	_max_deflection_rad(math::radians(45.0f))
+
 {
 
 }
@@ -66,7 +64,21 @@ float ECL_YawController::control(float roll, float yaw_rate, float accel_y, floa
 
 	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
 
-	return 0.0f;
+//	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));
+//		}
+
+	/* 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;
+	}
+
+	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
 }
 
 void ECL_YawController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index 66b227918..fe0abb956 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -50,39 +50,17 @@ public:
 	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));
 
-	void reset_integrator();
 
-	void set_k_side(float k_a) {
-		_k_side = k_a;
-	}
-	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_integrator_max(float max) {
-		_integrator_max = max;
+	float get_desired_rate() {
+		return _rate_setpoint;
 	}
 
 private:
 	uint64_t _last_run;
 
-	float _k_side;
-	float _k_i;
-	float _k_d;
-	float _k_roll_ff;
-	float _integrator_max;
-
-	float _last_error;
+	float _rate_setpoint;
 	float _last_output;
-	float _last_rate_hp_out;
-	float _last_rate_hp_in;
-	float _k_d_last;
-	float _integrator;
+	float _max_deflection_rad;
 
 };