Skip to content

Instantly share code, notes, and snippets.

@timtrueman
Created March 5, 2010 08:13
Show Gist options
  • Star 5 You must be signed in to star a gist
  • Fork 4 You must be signed in to fork a gist
  • Save timtrueman/322555 to your computer and use it in GitHub Desktop.
Save timtrueman/322555 to your computer and use it in GitHub Desktop.
Tilt-compensated heading from magnetometer readings, roll and pitch
def wrap(angle):
if angle > pi:
angle -= (2*pi)
if angle < -pi:
angle += (2*pi)
if angle < 0:
angle += 2*pi
return angle
def magnetometer_readings_to_tilt_compensated_heading(bx, by, bz, phi, theta):
""" Takes in raw magnetometer values, pitch and roll and turns it into a tilt-compensated heading value ranging from -pi to pi (everything in this function should be in radians). """
variation = 4.528986*(pi/180) # magnetic variation for Corpus Christi, should match your bx/by/bz and where they were measured from (a lookup table is beyond the scope of this gist)
Xh = bx * cos(theta) + by * sin(phi) * sin(theta) + bz * cos(phi) * sin(theta)
Yh = by * cos(phi) - bz * sin(phi)
return wrap((atan2(-Yh, Xh) + variation))
@FatLurch
Copy link

It looks like in this case, phi = roll and theta = pitch. I thought in Euler angles that psi was usually roll instead of phi.

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