diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-18 00:16:10 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-18 00:16:10 +0200 |
commit | 9a35bac2adc1e803dea7cdb6a2db277f111724e0 (patch) | |
tree | 789b2c8be345426eeabe28963d42c6c7790ff977 /src | |
parent | 52596be98c77497d67615435fdbd8e403cae618f (diff) | |
download | px4-firmware-9a35bac2adc1e803dea7cdb6a2db277f111724e0.tar.gz px4-firmware-9a35bac2adc1e803dea7cdb6a2db277f111724e0.tar.bz2 px4-firmware-9a35bac2adc1e803dea7cdb6a2db277f111724e0.zip |
fw att: yaw ctrl: robustify against non finite numbers
Diffstat (limited to 'src')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 12 |
1 files changed, 12 insertions, 0 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 255776765..d43e0314e 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -66,6 +66,12 @@ 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))) { + return _rate_setpoint; + } // static int counter = 0; /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ _rate_setpoint = 0.0f; @@ -103,6 +109,12 @@ 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))) { + 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(); |