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

6 files changed, 231 insertions, 67 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;
 
 };