Merged master into uavcan

1 files changed, 21 insertions, 5 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 */