diff options
Diffstat (limited to 'apps/multirotor_att_control/multirotor_rate_control.c')
| -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; |