C++ 3D Compass for Emlid Navio2 with an MPU-9250 sensor

True Story Follows

So I’m on the grind like Rumpelstiltskin turning keyboard strokes into gold and developing an autopilot module for Blimp Drones. I bought the super convenient and excellent hardware package, the Navio2 in conjunction with a Raspberry Pi, but I had some problems getting the compass working properly. Pitch and roll are quite simple because the direction is relative only to gravity, but getting an azimuth can be trickier because this direction relies on a compass.

From the example code packaged with the Navio, I could not get yaw working. It demonstrated how raw values could be obtained, but the hurdle to ascertain a compass heading from the raw values was not as trivial to me as it was made out to be. I ended up taking the arc tangent of raw magnetometer values, but it turns out this only works if the device is perfectly flat (no pitch or roll). That, of course, is not a valid assumption for an aircraft. The other example code I could find on the internets was coupled to a particular hardware set (mostly Arduino’s). Compilation required dependencies that I didn’t have, and it wasn’t so straightforward to extract the exact components.

After a little bit of effort, I’ve distilled the publicly available code on the internets into a more straightforward package meant to be compiled on a Raspberry Pi in conjunction with a Navio2.

The Code

All of the code for this project can be found on GitHub.


  • Does this code work? Yes.
  • Is there copied code from Emlid? Yes.
  • Do I owe Kris Winer a beer based on his licensing agreements? Yes
  • Does it have corresponding test coverage? No.
  • Are there some hacks in place to make it work? Yes (Rather than figure out orienting the x, y, z planes properly I just added degree offsets to final values)


With that said, here’s some example usage of the 3d compass files:

#include "compass3d.h"
#include <iostream>

// These values are obtained by calibrating the magnetometer.  The easiest way to do this
// is to log x, y, and z values over time and try to orient the magnetometer in all possible
// directions.  The offset in each axis should be such that the min and max values for each axis
// with the offset applied should average to 0.
float xOffset = -43.6189453125;
float yOffset = 13.7592773437;
float zOffset = -9.2390625;

// magnetic declination varies by region. Can be found at http://www.magnetic-declination.com/
// current declination is based off of San Francisco
float magneticDeclinationOffset = 13.8;

int main() {
	Compass3D *compass = new Compass3D();
	while(1) {
		compass->updateFromSensor(xOffset, yOffset, zOffset);
		float pitch = compass->getPitch();
		float yaw = compass->getYaw(magneticDeclinationOffset);
		float roll = compass->getRoll();
		printf("%.2f, %.2f, %.2f\n", pitch, yaw, roll);
	return 0;