Skip to content

Instantly share code, notes, and snippets.

@Sgeo
Created June 20, 2019 23:20
Show Gist options
  • Save Sgeo/4c58d69fa4375e4f2f3592e1f9c6eb0e to your computer and use it in GitHub Desktop.
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.
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
@Sgeo
Copy link
Author

Sgeo commented Jul 1, 2019

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment