1 files changed, 40 insertions, 44 deletions

diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index fe03b8065..82ba2c6c7 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -47,45 +47,43 @@
 #include <systemlib/err.h>
 
 ECL_YawController::ECL_YawController() :
-	_last_run(0),
-	_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),
-	_coordinated_min_speed(1.0f),
-	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"))
+	ECL_Controller("yaw"),
+	_coordinated_min_speed(1.0f)
 {
 }
 
 ECL_YawController::~ECL_YawController()
 {
-	perf_free(_nonfinite_input_perf);
 }
 
-float ECL_YawController::control_attitude(float roll, float pitch,
-		float speed_body_u, float speed_body_v, float speed_body_w,
-		float roll_rate_setpoint, float pitch_rate_setpoint)
+float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) &&
-				isfinite(speed_body_w) && isfinite(roll_rate_setpoint) &&
-				isfinite(pitch_rate_setpoint))) {
+	if (!(isfinite(ctl_data.roll) &&
+				isfinite(ctl_data.pitch) &&
+				isfinite(ctl_data.speed_body_u) &&
+				isfinite(ctl_data.speed_body_v) &&
+				isfinite(ctl_data.speed_body_w) &&
+				isfinite(ctl_data.roll_rate_setpoint) &&
+				isfinite(ctl_data.pitch_rate_setpoint))) {
 		perf_count(_nonfinite_input_perf);
 		return _rate_setpoint;
 	}
+
 //	static int counter = 0;
 	/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
 	_rate_setpoint = 0.0f;
-	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 (sqrtf(ctl_data.speed_body_u * ctl_data.speed_body_u + ctl_data.speed_body_v * ctl_data.speed_body_v +
+				ctl_data.speed_body_w * ctl_data.speed_body_w) > _coordinated_min_speed) {
+		float denumerator = (ctl_data.speed_body_u * cosf(ctl_data.roll) * cosf(ctl_data.pitch) +
+				ctl_data.speed_body_w * sinf(ctl_data.pitch));
+
 		if(fabsf(denumerator) > FLT_EPSILON) {
-			_rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
+			_rate_setpoint = (ctl_data.speed_body_w * ctl_data.roll_rate_setpoint +
+					9.81f * sinf(ctl_data.roll) * cosf(ctl_data.pitch) +
+					ctl_data.speed_body_u * ctl_data.pitch_rate_setpoint * sinf(ctl_data.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);
 		}
 
@@ -111,46 +109,49 @@ float ECL_YawController::control_attitude(float roll, float pitch,
 	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)
+float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
-				isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) &&
-				isfinite(airspeed_max) && isfinite(scaler))) {
+	if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) &&
+				isfinite(ctl_data.yaw_rate) && isfinite(ctl_data.pitch_rate_setpoint) &&
+				isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) &&
+				isfinite(ctl_data.scaler))) {
 		perf_count(_nonfinite_input_perf);
 		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 = (float)dt_micros * 1e-6f;
 
 	/* lock integral for long intervals */
+	bool lock_integrator = ctl_data.lock_integrator;
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
 	/* input conditioning */
+	float airspeed = ctl_data.airspeed;
 	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;
+	airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
+	} else if (airspeed < ctl_data.airspeed_min) {
+	airspeed = ctl_data.airspeed_min;
 	}
 
 
-	/* Transform setpoint to body angular rates */
-	_bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian
+	/* Transform setpoint to body angular rates (jacobian) */
+	_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
+		cosf(ctl_data.roll)*cosf(ctl_data.pitch) * _rate_setpoint;
 
-	/* Transform estimation to body angular rates */
-	float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian
+	/* Transform estimation to body angular rates (jacobian) */
+	float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate +
+		cosf(ctl_data.roll)*cosf(ctl_data.pitch) * ctl_data.yaw_rate;
 
 	/* Calculate body angular rate error */
 	_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
 
-	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
+	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
 
 	float id = _rate_error * dt;
 
@@ -173,14 +174,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
 	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * ctl_data.scaler;  //scaler is proportional to 1/airspeed
 	//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
 
 
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
-
-void ECL_YawController::reset_integrator()
-{
-	_integrator = 0.0f;
-}