Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-01 03:04:04 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: added quaternion normalization

Browse Source
This commit is contained in:
Andrew Tridgell 2013-12-30 17:31:51 +11:00
parent 006cf58b2d
commit 90c41981ac
2 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Math/quaternion.cpp
View File

@ -87,3 +87,21 @@ void Quaternion::to_euler(float *roll, float *pitch, float *yaw) const
1 - 2.0f*(q3*q3 + q4*q4)); 1 - 2.0f*(q3*q3 + q4*q4));
} }
} }
float Quaternion::length(void) const
{
return sqrtf(sq(q1) + sq(q2) + sq(q3) + sq(q4));
}
void Quaternion::normalize(void)
{
float quatMag = length();
if (quatMag > 1e-16f)
{
float quatMagInv = 1.0f/quatMag;
q1 *= quatMagInv;
q2 *= quatMagInv;
q3 *= quatMagInv;
q4 *= quatMagInv;
}
}

3
libraries/AP_Math/quaternion.h
View File

@ -64,6 +64,9 @@ public:
// create eulers from a quaternion // create eulers from a quaternion
void to_euler(float *roll, float *pitch, float *yaw) const; void to_euler(float *roll, float *pitch, float *yaw) const;
float length(void) const;
void normalize();
// allow a quaternion to be used as an array, 0 indexed // allow a quaternion to be used as an array, 0 indexed
float & operator[](uint8_t i) { float & operator[](uint8_t i) {
float *_v = &q1; float *_v = &q1;