Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

AttitudeControl: only normalize attitude setpoint once on update

Browse Source
This commit is contained in:
Daniel Agar
2020-08-08 12:40:51 -04:00
parent 1c42d12491
commit 41a1675c53
2 changed files with 1 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp
-3
View File
@@ -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();
src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp
+1 -1
View File
@@ -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