6 files changed, 77 insertions, 18 deletions

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 9584924cc..46db788a6 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -61,13 +61,24 @@ ECL_PitchController::ECL_PitchController() :
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_bodyrate_setpoint(0.0f)
+	_bodyrate_setpoint(0.0f),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
 {
 }
 
-float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
+ECL_PitchController::~ECL_PitchController()
 {
+	perf_free(_nonfinite_input_perf);
+}
 
+float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
+{
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) {
+		perf_count(_nonfinite_input_perf);
+		warnx("not controlling pitch");
+		return _rate_setpoint;
+	}
 
 	/* flying inverted (wings upside down) ? */
 	bool inverted = false;
@@ -123,6 +134,14 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 		float yaw_rate_setpoint,
 		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
 {
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
+				isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) &&
+				isfinite(airspeed_max) && isfinite(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();
@@ -132,9 +151,6 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_ff = 0;
-
 	/* input conditioning */
 	if (!isfinite(airspeed)) {
 		/* airspeed is NaN, +- INF or not available, pick center of band */
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
index 30a82a86a..39b9f9d03 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -51,12 +51,15 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <systemlib/perf_counter.h>
 
 class __EXPORT ECL_PitchController //XXX: create controller superclass
 {
 public:
 	ECL_PitchController();
 
+	~ECL_PitchController();
+
 	float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed);
 
 
@@ -126,6 +129,7 @@ private:
 	float _rate_error;
 	float _rate_setpoint;
 	float _bodyrate_setpoint;
+	perf_counter_t _nonfinite_input_perf;
 };
 
 #endif // ECL_PITCH_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 2e86c72dc..9894a34d7 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -59,12 +59,23 @@ ECL_RollController::ECL_RollController() :
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_bodyrate_setpoint(0.0f)
+	_bodyrate_setpoint(0.0f),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input"))
 {
 }
 
+ECL_RollController::~ECL_RollController()
+{
+	perf_free(_nonfinite_input_perf);
+}
+
 float ECL_RollController::control_attitude(float roll_setpoint, float roll)
 {
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(roll_setpoint) && isfinite(roll))) {
+		perf_count(_nonfinite_input_perf);
+		return _rate_setpoint;
+	}
 
 	/* Calculate error */
 	float roll_error = roll_setpoint - roll;
@@ -86,6 +97,14 @@ float ECL_RollController::control_bodyrate(float pitch,
 		float yaw_rate_setpoint,
 		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
 {
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) &&
+			isfinite(airspeed_min) && isfinite(airspeed_max) &&
+			isfinite(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();
@@ -95,9 +114,6 @@ float ECL_RollController::control_bodyrate(float pitch,
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_ff = 0; //xxx: param
-
 	/* input conditioning */
 //	warnx("airspeed pre %.4f", (double)airspeed);
 	if (!isfinite(airspeed)) {
@@ -122,8 +138,8 @@ float ECL_RollController::control_bodyrate(float pitch,
 		float id = _rate_error * dt;
 
 		/*
-		 * anti-windup: do not allow integrator to increase if actuator is at limit
-		 */
+		* anti-windup: do not allow integrator to increase if actuator is at limit
+		*/
 		if (_last_output < -1.0f) {
 			/* only allow motion to center: increase value */
 			id = math::max(id, 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 92c64b95f..0799dbe03 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -51,12 +51,15 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <systemlib/perf_counter.h>
 
 class __EXPORT ECL_RollController //XXX: create controller superclass
 {
 public:
 	ECL_RollController();
 
+	~ECL_RollController();
+
 	float control_attitude(float roll_setpoint, float roll);
 
 	float control_bodyrate(float pitch,
@@ -117,6 +120,7 @@ private:
 	float _rate_error;
 	float _rate_setpoint;
 	float _bodyrate_setpoint;
+	perf_counter_t _nonfinite_input_perf;
 };
 
 #endif // ECL_ROLL_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index 255776765..fe03b8065 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -58,20 +58,33 @@ ECL_YawController::ECL_YawController() :
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
 	_bodyrate_setpoint(0.0f),
-	_coordinated_min_speed(1.0f)
+	_coordinated_min_speed(1.0f),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"))
 {
 }
 
+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)
 {
+	/* 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))) {
+		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(denumerator != 0.0f) { //XXX: floating point comparison
+		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;
 //			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);
 		}
@@ -103,6 +116,13 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
 		float pitch_rate_setpoint,
 		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
 {
+	/* 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))) {
+		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();
@@ -112,11 +132,6 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-
-//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_ff = 0;
-
-
 	/* input conditioning */
 	if (!isfinite(airspeed)) {
 	/* airspeed is NaN, +- INF or not available, pick center of band */
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index 03f3202d0..a360c14b8 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -50,12 +50,15 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <systemlib/perf_counter.h>
 
 class __EXPORT ECL_YawController //XXX: create controller superclass
 {
 public:
 	ECL_YawController();
 
+	~ECL_YawController();
+
 	float 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);
@@ -118,6 +121,7 @@ private:
 	float _rate_setpoint;
 	float _bodyrate_setpoint;
 	float _coordinated_min_speed;
+	perf_counter_t _nonfinite_input_perf;
 
 };