From 41a1675c530d02bff78bc3c545fbe4ee23ec764c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 8 Aug 2020 12:40:51 -0400 Subject: [PATCH] AttitudeControl: only normalize attitude setpoint once on update --- src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp | 3 --- src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp index a5e5be3e7c..de0ba91b05 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -56,9 +56,6 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const { Quatf qd = _attitude_setpoint_q; - // ensure input quaternions are exactly normalized because acosf(1.00001) == NaN - qd.normalize(); - // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch const Vector3f e_z = q.dcm_z(); const Vector3f e_z_d = qd.dcm_z(); diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp index 9b55e9e8b9..70980e2b35 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp @@ -75,7 +75,7 @@ public: * @param qd desired vehicle attitude setpoint * @param yawspeed_setpoint [rad/s] yaw feed forward angular rate in world frame */ - void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint) { _attitude_setpoint_q = qd; _yawspeed_setpoint = yawspeed_setpoint; } + void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint) { _attitude_setpoint_q = qd; _attitude_setpoint_q.normalize(); _yawspeed_setpoint = yawspeed_setpoint; } /** * Adjust last known attitude setpoint by a delta rotation