diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-18 00:15:50 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-18 00:15:50 +0200 |
commit | 52596be98c77497d67615435fdbd8e403cae618f (patch) | |
tree | 4970a29c9db931317d0d2f469d8b82ef19325c17 /src | |
parent | 7c165689ce0d33a65554f62a3438c2da30404642 (diff) | |
download | px4-firmware-52596be98c77497d67615435fdbd8e403cae618f.tar.gz px4-firmware-52596be98c77497d67615435fdbd8e403cae618f.tar.bz2 px4-firmware-52596be98c77497d67615435fdbd8e403cae618f.zip |
fw att: roll ctrl: robustify against non finite numbers
Diffstat (limited to 'src')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 15 |
1 files changed, 13 insertions, 2 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 2e86c72dc..6ad00049d 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -65,6 +65,10 @@ ECL_RollController::ECL_RollController() : float ECL_RollController::control_attitude(float roll_setpoint, float roll) { + /* Do not calculate control signal with bad inputs */ + if (!(isfinite(roll_setpoint) && isfinite(roll))) { + return _rate_setpoint; + } /* Calculate error */ float roll_error = roll_setpoint - roll; @@ -86,6 +90,13 @@ 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))) { + 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(); @@ -122,8 +133,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); |