From 6b4ccaa53e4e1c7ff45fe057bc2d1745dc7597f7 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Sat, 13 Mar 2021 23:00:29 +0100 Subject: [PATCH] Handle invalid yaw rate setpoints --- .../mc_att_control/AttitudeControl/AttitudeControl.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp index de0ba91b05..859b86316f 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -97,7 +97,9 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const // and multiply it by the yaw setpoint rate (yawspeed_setpoint). // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame // such that it can be added to the rates setpoint. - rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint; + if (is_finite(_yawspeed_setpoint)) { + rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint; + } // limit rates for (int i = 0; i < 3; i++) {