diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-18 00:15:37 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-18 00:15:37 +0200 |
commit | 7c165689ce0d33a65554f62a3438c2da30404642 (patch) | |
tree | 5967e881e24f8a981c5500c3dc36320b9f90d74a | |
parent | e72a7a7dd78cde4a93d42a53328553e71560fc27 (diff) | |
download | px4-firmware-7c165689ce0d33a65554f62a3438c2da30404642.tar.gz px4-firmware-7c165689ce0d33a65554f62a3438c2da30404642.tar.bz2 px4-firmware-7c165689ce0d33a65554f62a3438c2da30404642.zip |
fw att: pitch ctrl: robustify against non finite numbers
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 13 |
1 files changed, 12 insertions, 1 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..a3f5199b1 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -67,7 +67,11 @@ ECL_PitchController::ECL_PitchController() : 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))) { + warnx("not controlling pitch"); + return _rate_setpoint; + } /* flying inverted (wings upside down) ? */ bool inverted = false; @@ -123,6 +127,13 @@ 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))) { + 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(); |