Created
June 20, 2019 23:20
-
-
Save Sgeo/4c58d69fa4375e4f2f3592e1f9c6eb0e to your computer and use it in GitHub Desktop.
Part of convert.c in the i-Glasses SDK. Note that "raw" should be called "cooked". An emulator needs to reverse this math.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
void rawToEulerInt(long rawMagneticX,long rawMagneticY,long rawMagneticZ, | |
long rawGravimetricX,long rawGravimetricY, | |
long * eulerYaw,long * eulerPitch,long * eulerRoll) { | |
vec3 rotMagnetic; | |
vec2 linGravimetric; | |
vec2 normRotMag; | |
sinCos pitch,roll; | |
#ifdef USE_FILTERS | |
if (!madeFilters) { | |
if (!initFIR(&fx,3)) return; | |
if (!initFIR(&fy,3)) return; | |
if (!initFIR(&fz,3)) return; | |
if (!initFIR(&f1,6)) return; | |
if (!initFIR(&f2,6)) return; | |
madeFilters = 1; | |
} // if | |
rawMagneticX = filterFIR(&fx,rawMagneticX); | |
rawMagneticY = filterFIR(&fy,rawMagneticY); | |
rawMagneticZ = filterFIR(&fz,rawMagneticZ); | |
linGravimetric.x = filterFIR(&f1,MUL(rawGravimetricX,RAD180DEG)); | |
linGravimetric.y = filterFIR(&f2,MUL(rawGravimetricY,RAD180DEG)); | |
#else | |
linGravimetric.x = MUL(rawGravimetricX,RAD180DEG); | |
linGravimetric.y = MUL(rawGravimetricY,RAD180DEG); | |
#endif | |
sincos(&pitch,linGravimetric.x); | |
sincos(&roll,linGravimetric.y); | |
// Inversely transform the magnetic vector back into definition space. | |
// This allows an easy computation of yaw by looking at the projection | |
// of the magnetic vector in the x-z plane, which is the plane of the | |
// earth. | |
// Perform a rotation about x: Pitch | |
rotMagnetic.y = MUL(pitch.cos,rawMagneticY) - MUL(pitch.sin,rawMagneticZ); | |
rotMagnetic.z = MUL(pitch.sin,rawMagneticY) + MUL(pitch.cos,rawMagneticZ); | |
// Perform a rotation about z: Roll. We only need x, so y isn't transformed. | |
rotMagnetic.x = MUL(roll.cos,rawMagneticX) - MUL(roll.sin,rotMagnetic.y); | |
*eulerYaw = TODEGREES(getatan2(rotMagnetic.z,rotMagnetic.x)); | |
*eulerPitch = TODEGREES(linGravimetric.x); | |
*eulerRoll = TODEGREES(linGravimetric.y); | |
} // rawToEulerInt |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Magnetic vector = (cos yaw, 0, sin yaw) into the eulerYaw equation should give back eulerYaw. Antirotate around roll then antirotate around pitch, so CONVERT.C rotates correctly. CONVERT.C assumes magnetic vector is relative to pitch/roll, so antirotating will give that.