From 13000a71212076a515043fb7ca89ce77cf6c36b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Nov 2012 16:51:13 +0100 Subject: [PATCH] Yaw rate sanity checking for controller --- apps/multirotor_att_control/multirotor_rate_control.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index bbaec30330..93f7085aec 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;