diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-20 16:51:13 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-20 16:51:13 +0100 |
commit | 13000a71212076a515043fb7ca89ce77cf6c36b4 (patch) | |
tree | 7eaf07f01e1bb7b0f9a623199a2c3cb85a7710be | |
parent | 7820024d905396fd8d2bf7b29093cddfd0e9a0d5 (diff) | |
download | px4-firmware-13000a71212076a515043fb7ca89ce77cf6c36b4.tar.gz px4-firmware-13000a71212076a515043fb7ca89ce77cf6c36b4.tar.bz2 px4-firmware-13000a71212076a515043fb7ca89ce77cf6c36b4.zip |
Yaw rate sanity checking for controller
-rw-r--r-- | apps/multirotor_att_control/multirotor_rate_control.c | 5 |
1 files changed, 5 insertions, 0 deletions
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index bbaec3033..93f7085ae 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -209,6 +209,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control yaw rate */ float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); + /* increase resilience to faulty control inputs */ + if (!isfinite(yaw_rate_control)) { + yaw_rate_control = 0.0f; + warnx("rej. NaN ctrl yaw"); + } actuators->control[0] = roll_control; actuators->control[1] = pitch_control; |