Calculation of Magnetometer Heading (relative to magnetic north / 0 - 360°)

[wilhelmzeuschner]

I require some help calculating the heading relative to magnetic north with this library and an MPU9250.

float heading_m = (atan2(IMU.getMagY_uT(), IMU.getMagX_uT()) * 180) / PI; does not work as with other magnetometer examples ( Adafruit Example for Heading) 😞

As far as I understand, the readings gathered by the library do not represent vectors as it is the case with the Adafruit code. Adding a function to this library to calculate the heading would be extremely useful!

Thanks in advance!

[SamiXSous]

@wilhelmzeuschner I am currently facing the same problem. Did you find a way to resolve this issue? If so please do share.

Thanks in advance!

[flybrianfly]

Sorry I missed this. The code below will give you heading if you keep the IMU level, it's similar to the code posted by @wilhelmzeuschner. Please note that you absolutely need to calibrate the magnetometer on the MPU-9250 for this to work. The correction factors are typically very large with these sensors. The magnetometer calibration is all of the EEPROM stuff that is happening (i.e. loading calibration factors).

#include "MPU9250.h"
#include "EEPROM.h"

/* magnetometer data */
float h, hx, hy, hz;
/* magnetometer calibration data */
float hxb, hxs, hyb, hys, hzb, hzs;
/* euler angles */
float yaw_rad, heading_rad;
/* filtered heading */
float filtered_heading_rad;
float window_size = 20;
/* conversion radians to degrees */
const float R2D = 180.0f / PI;
/* MPU 9250 object */
MPU9250 imu(Wire, 0x68);
/* MPU 9250 data ready pin */
const uint8_t kMpu9250DrdyPin = 2;
/* Flag set to indicate MPU 9250 data is ready */
volatile bool imu_data_ready = false;

/* ISR to set data ready flag */
void data_ready()
{
  imu_data_ready = true;
}

void setup()
{
  /* Serial for displaying results */
  Serial.begin(115200);
  while(!Serial) {}
  /* 
  * Start the sensor, set the bandwidth the 10 Hz, the output data rate to
  * 50 Hz, and enable the data ready interrupt. 
  */
  imu.begin();
  imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_10HZ);
  imu.setSrd(19);
  imu.enableDataReadyInterrupt();
  /*
  * Load the magnetometer calibration
  */
  uint8_t eeprom_buffer[24];
  for (unsigned int i = 0; i < sizeof(eeprom_buffer); i++ ) {
    eeprom_buffer[i] = EEPROM.read(i);
  }
  memcpy(&hxb, eeprom_buffer, sizeof(hxb));
  memcpy(&hyb, eeprom_buffer + 4, sizeof(hyb));
  memcpy(&hzb, eeprom_buffer + 8, sizeof(hzb));
  memcpy(&hxs, eeprom_buffer + 12, sizeof(hxs));
  memcpy(&hys, eeprom_buffer + 16, sizeof(hys));
  memcpy(&hzs, eeprom_buffer + 20, sizeof(hzs));
  imu.setMagCalX(hxb, hxs);
  imu.setMagCalY(hyb, hys);
  imu.setMagCalZ(hzb, hzs);
  /* Attach the data ready interrupt to the data ready ISR */
  pinMode(kMpu9250DrdyPin, INPUT);
  attachInterrupt(kMpu9250DrdyPin, data_ready, RISING);
}

void loop()
{
  if (imu_data_ready) {
    imu_data_ready = false;
    /* Read the MPU 9250 data */
    imu.readSensor();
    hx = imu.getMagX_uT();
    hy = imu.getMagY_uT();
    hz = imu.getMagZ_uT();
    /* Normalize magnetometer data */
    h = sqrtf(hx * hx + hy * hy + hz * hz);
    hx /= h;
    hy /= h;
    hz /= h;
    /* Compute euler angles */
    yaw_rad = atan2f(-hy, hx);
    heading_rad = constrainAngle360(yaw_rad);
    /* Filtering heading */
    filtered_heading_rad = (filtered_heading_rad * (window_size - 1.0f) + heading_rad) / window_size;
    /* Display the results */
    Serial.print(yaw_rad * R2D);
    Serial.print("\t");
    Serial.print(heading_rad * R2D);
    Serial.print("\t");
    Serial.println(filtered_heading_rad * R2D);
  }
}

/* Bound angle between 0 and 360 */
float constrainAngle360(float dta) {
  dta = fmod(dta, 2.0 * PI);
  if (dta < 0.0)
    dta += 2.0 * PI;
  return dta;
}

Here is a tilt compass, it uses the accelerometer to correct the magnetometer reading, so the compass should still work even if it's not level:

#include "MPU9250.h"
#include "EEPROM.h"

/* accelerometer and magnetometer data */
float a, ax, ay, az, h, hx, hy, hz;
/* magnetometer calibration data */
float hxb, hxs, hyb, hys, hzb, hzs;
/* euler angles */
float pitch_rad, roll_rad, yaw_rad, heading_rad;
/* filtered heading */
float filtered_heading_rad;
float window_size = 20;
/* conversion radians to degrees */
const float R2D = 180.0f / PI;
/* MPU 9250 object */
MPU9250 imu(Wire, 0x68);
/* MPU 9250 data ready pin */
const uint8_t kMpu9250DrdyPin = 2;
/* Flag set to indicate MPU 9250 data is ready */
volatile bool imu_data_ready = false;

/* ISR to set data ready flag */
void data_ready()
{
  imu_data_ready = true;
}

void setup()
{
  /* Serial for displaying results */
  Serial.begin(115200);
  while(!Serial) {}
  /* 
  * Start the sensor, set the bandwidth the 10 Hz, the output data rate to
  * 50 Hz, and enable the data ready interrupt. 
  */
  imu.begin();
  imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_10HZ);
  imu.setSrd(19);
  imu.enableDataReadyInterrupt();
  /*
  * Load the magnetometer calibration
  */
  uint8_t eeprom_buffer[24];
  for (unsigned int i = 0; i < sizeof(eeprom_buffer); i++ ) {
    eeprom_buffer[i] = EEPROM.read(i);
  }
  memcpy(&hxb, eeprom_buffer, sizeof(hxb));
  memcpy(&hyb, eeprom_buffer + 4, sizeof(hyb));
  memcpy(&hzb, eeprom_buffer + 8, sizeof(hzb));
  memcpy(&hxs, eeprom_buffer + 12, sizeof(hxs));
  memcpy(&hys, eeprom_buffer + 16, sizeof(hys));
  memcpy(&hzs, eeprom_buffer + 20, sizeof(hzs));
  imu.setMagCalX(hxb, hxs);
  imu.setMagCalY(hyb, hys);
  imu.setMagCalZ(hzb, hzs);
  /* Attach the data ready interrupt to the data ready ISR */
  pinMode(kMpu9250DrdyPin, INPUT);
  attachInterrupt(kMpu9250DrdyPin, data_ready, RISING);
}

void loop()
{
  if (imu_data_ready) {
    imu_data_ready = false;
    /* Read the MPU 9250 data */
    imu.readSensor();
    ax = imu.getAccelX_mss();
    ay = imu.getAccelY_mss();
    az = imu.getAccelZ_mss();
    hx = imu.getMagX_uT();
    hy = imu.getMagY_uT();
    hz = imu.getMagZ_uT();
    /* Normalize accelerometer and magnetometer data */
    a = sqrtf(ax * ax + ay * ay + az * az);
    ax /= a;
    ay /= a;
    az /= a;
    h = sqrtf(hx * hx + hy * hy + hz * hz);
    hx /= h;
    hy /= h;
    hz /= h;
    /* Compute euler angles */
    pitch_rad = asinf(ax);
    roll_rad = asinf(-ay / cosf(pitch_rad));
    yaw_rad = atan2f(hz * sinf(roll_rad) - hy * cosf(roll_rad), hx * cosf(pitch_rad) + hy * sinf(pitch_rad) * sinf(roll_rad) + hz * sinf(pitch_rad) * cosf(roll_rad));
    heading_rad = constrainAngle360(yaw_rad);
    /* Filtering heading */
    filtered_heading_rad = (filtered_heading_rad * (window_size - 1.0f) + heading_rad) / window_size;
    /* Display the results */
    Serial.print(pitch_rad * R2D);
    Serial.print("\t");
    Serial.print(roll_rad * R2D);
    Serial.print("\t");
    Serial.print(yaw_rad * R2D);
    Serial.print("\t");
    Serial.print(heading_rad * R2D);
    Serial.print("\t");
    Serial.println(filtered_heading_rad * R2D);
  }
}

/* Bound angle between 0 and 360 */
float constrainAngle360(float dta) {
  dta = fmod(dta, 2.0 * PI);
  if (dta < 0.0)
    dta += 2.0 * PI;
  return dta;
}

Finally, here is the magnetometer calibration sketch:

#include "MPU9250.h"
#include "EEPROM.h"

/* MPU 9250 object */
MPU9250 imu(Wire, 0x68);

/* EEPROM buffer to mag bias and scale factors */
uint8_t eeprom_buffer[24];
float value;

void setup() {
  /* Serial for displaying instructions */
  Serial.begin(115200);
  while(!Serial) {}
  /* Start communication with IMU */
  imu.begin();
  /* Calibrate magnetometer */
  Serial.print("Calibrating magnetometer, please slowly move in a figure 8 until complete...");
  imu.calibrateMag();
  Serial.println("Done!");
  Serial.print("Saving results to EEPROM...");
  /* Save to EEPROM */
  value = imu.getMagBiasX_uT();
  memcpy(eeprom_buffer, &value, sizeof(value));
  value = imu.getMagBiasY_uT();
  memcpy(eeprom_buffer + 4, &value, sizeof(value));
  value = imu.getMagBiasZ_uT();
  memcpy(eeprom_buffer + 8, &value, sizeof(value));
  value = imu.getMagScaleFactorX();
  memcpy(eeprom_buffer + 12, &value, sizeof(value));
  value = imu.getMagScaleFactorY();
  memcpy(eeprom_buffer + 16, &value, sizeof(value));
  value = imu.getMagScaleFactorZ();
  memcpy(eeprom_buffer + 20, &value, sizeof(value));
  for (unsigned int i = 0; i < sizeof(eeprom_buffer); i++) {
    EEPROM.write(i, eeprom_buffer[i]);
  }
  Serial.println("Done! You may power off your board.");
}

void loop() {}

[AndrewNoviello]

Hi, I am attempting to use this library for robotics using the MPU9250. I am using your code above, but am getting strange and inconsistent values. First of all, after the first few seconds of running, the sensor values stop changing and all the values stay the same from then after. Do you have any ideas as to why? Secondly, even when rotating it around, the sensor gives the same output for multiple different rotation spots, all in total opposite orientations. Could this be a calibration issue? Thanks for any help!