fixed elevator wobbling at low airspeed.

Removed not applicable if conditions.

4 files changed, 20 insertions, 25 deletions

diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp
index 46140fbfd..9cd08a50d 100644
--- a/src/lib/ecl/attitude_fw/ecl_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_controller.cpp
@@ -49,6 +49,7 @@
 #include "ecl_controller.h"
 
 #include <stdio.h>
+#include <mathlib/mathlib.h>
 
 ECL_Controller::ECL_Controller(const char *name) :
 	_last_run(0),
@@ -126,3 +127,14 @@ float ECL_Controller::get_desired_bodyrate()
 {
 	return _bodyrate_setpoint;
 }
+
+float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) {
+	float airspeed_result = airspeed;
+	if (!isfinite(airspeed)) {
+		/* airspeed is NaN, +- INF or not available, pick center of band */
+		airspeed = 0.5f * (minspeed + maxspeed);
+	} else if (airspeed < minspeed) {
+		airspeed = minspeed;
+	}
+	return airspeed_result;
+}
diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h
index 254052964..360fec28f 100644
--- a/src/lib/ecl/attitude_fw/ecl_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_controller.h
@@ -51,6 +51,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <systemlib/perf_counter.h>
+#include <mathlib/mathlib.h>
 
 struct ECL_ControlData {
 	float roll;
@@ -119,4 +120,5 @@ protected:
 	perf_counter_t _nonfinite_input_perf;
 	static const uint8_t _perf_name_max = 40;
 	char _perf_name[_perf_name_max];
+	float constrain_airspeed(float airspeed, float minspeed, float maxspeed);
 };
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index ca454fa62..c80eb357c 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -94,9 +94,11 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
 		}
 	}
 
+	/* input conditioning */
+	float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);
 	/* calculate the offset in the rate resulting from rolling  */
 	//xxx needs explanation and conversion to body angular rates or should be removed
-	float turn_offset = fabsf((CONSTANTS_ONE_G / ctl_data.airspeed) *
+	float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
 				  tanf(roll) * sinf(roll)) * _roll_ff;
 
 	if (inverted) {
@@ -154,17 +156,6 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
 		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 * (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 (jacobian) */
 	_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
 			     cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
@@ -175,7 +166,7 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
 
 	_rate_error = _bodyrate_setpoint - pitch_bodyrate;
 
-	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
+	if (!lock_integrator && _k_i > 0.0f) {
 
 		float id = _rate_error * dt * ctl_data.scaler;
 
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 5d0846ac3..160dc5cad 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -106,17 +106,6 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 		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 * (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 (jacobian) */
 	_bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
 
@@ -126,7 +115,7 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 	/* Calculate body angular rate error */
 	_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
 
-	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
+	if (!lock_integrator && _k_i > 0.0f) {
 
 		float id = _rate_error * dt * ctl_data.scaler;
 
@@ -157,3 +146,4 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
+