Tilt Compensated Compass for Arduino Nano 33 Rev 2

acidrain21001 · July 21, 2025, 9:46pm

Here's a tilt compensated compass for the Arduino Nano 33 Rev2.
The code will be at the end of the page, but it’s important to read how you should run it.
The reason I’m sharing this is because I didn’t find one specific for this board and its IMU/Magnetometer package.
It's mostly based on the following tutorial.

I also used the following video as reference.

You should look beyond the tilt compensated compass aspect of it. I’m new to working with sensors and this project allowed me to learn a few things. If you’re new to working with accelerometers, gyroscopes and magnetometers these are the most important things you should get out of this.

1 - How to do a basic calibration of accelerometers, gyroscopes and magnetometers.
2 - How to fuse sensors together.
3 - How to filter input values.

First time usage - Calibration

Before you start, this project can run in two modes, calibration and normal mode.
The first time you run it you should run with this variable set to true:

bool calibrationModeEnabled = true;

Follow the instructions on the Serial console.
The first thing it will tell you is to put the board on a flat leveled surface and don't move it to calculate the accelerometer and gyroscope errors.
After a few seconds it will instruct you to move the board around for a while to calculate the hard iron error and soft iron scaling of the magnetometer.
Set the values provided in the Serial console on the corresponding variables.

Example

float accel_zerog[3] = { 0.0057, -0.0191, 0.0135 };
float gyro_zerorate[3] = { -0.0232, 0.1022, -0.6760 };
float mag_hardiron[3] = { -12.0, 10.0, -20.5 };
float mag_softiron[3] = { 1.0191, 0.9714, 1.0108 };

After this you can set the following variable to false and upload the program to the board again.

bool calibrationModeEnabled = false;

Now you should see the heading on the serial console. You should point the X axis of the magnetometer (inside the red rectangle) to where you want to read.
If you wish, instead of using the X axis of the magnetometer you can use the side of the board where the ublox chip is located to point to the heading by setting the following variable to false:

bool useBoardMagnetometerReferentialToPointAtHeading = false;

I hope you find this useful. If you have any doubts I encourage you the read the original tutorial and the video at the start because they do a way better job at explaining this stuff than I do.

Code

#include "Arduino_BMI270_BMM150.h"


/* 
 * This is used to override the default magnetometer sampling rate.
 * To change it to 30Hz go to the file Arduino\libraries\Arduino_BMI270_BMM150\src\utilities\BMM150-Sensor-API\bmm150.c
 * Search for the line 825 which contains the following:
 *
 *      case BMM150_PRESETMODE_HIGHACCURACY:
 * 
 * Inside this case statement change the line:
 *
 *      settings->data_rate = BMM150_DATA_RATE_20HZ;
 *
 * To
 *
 *    settings->data_rate = BMM150_DATA_RATE_30HZ;
*/
class MyBoschSensor : public BoschSensorClass {

public:
  MyBoschSensor(TwoWire& wire = Wire)
    : BoschSensorClass(wire){};

protected:
  virtual int8_t configure_sensor(struct bmm150_dev* dev) override {
    int8_t rslt;
    struct bmm150_settings settings;

    settings.pwr_mode = BMM150_POWERMODE_NORMAL;
    rslt = bmm150_set_op_mode(&settings, dev);

    if (rslt == BMM150_OK) {
      /* Setting the preset mode as High Accuracy power mode
      * i.e. data rate = 20Hz
      */
      settings.preset_mode = BMM150_PRESETMODE_HIGHACCURACY;
      rslt = bmm150_set_presetmode(&settings, dev);

      if (rslt == BMM150_OK) {
        /* Map the data interrupt pin */
        settings.int_settings.drdy_pin_en = 0x01;
        //rslt = bmm150_set_sensor_settings(BMM150_SEL_DRDY_PIN_EN, &settings, dev);
      }
    }
    return rslt;
  }
};

MyBoschSensor myIMU(Wire1);

const float geoDeclinationAngle = 11.41666666666667;
//const float localMagDeclinationAngle = -0.7266666666666667;
float ax, ay, az, gx, gy, gz, mx, my, mz;
float accRoll, accPitch, accYaw, accTotalVector, gyroRoll, gyroPitch, gyroYaw, gyroPitchOutput, gyroRollOutput, magRoll, magPitch, magYaw, magXHor, magYHor, magXDampened, magYDampened;
float elapsedTime, currentTime, previousTime;
int numberOfAccelerometerAndGyroCalibrationCycles = 200;
int numberOfMagnetometerCalibrationCycles = 2000;
bool calibrationModeEnabled = false;
bool calibrateAccelerometerAndGyroOnStart = false;
bool debugEnabled = false;

// This can be used to select between the magnetometer referencial or the side of the board opposite to the USB port to point at the heading.
bool useBoardMagnetometerReferentialToPointAtHeading = false;

/* Sensor Calibration Section */

// Accelerometer calibration
float accel_zerog[3] = { 0, 0, 0 };

// Gryroscope calibration
float gyro_zerorate[3] = { 0, 0, 0 };

// Magnetometer calibration
float mag_hardiron[3] = { 0, 0, 0 };

float mag_softiron[3] = { 0, 0, 0 };



void setup() {
  int i = 0;
  Serial.begin(115200);
  while (!Serial)
    ;
  Serial.println("Tilt Compensated Compass Starting...");

  if (!myIMU.begin()) {
    Serial.println();
    Serial.println("Failed to initialize IMU!");
    while (1)
      ;
  }

  delay(500);

  Serial.println();
  Serial.print("Magnetic field sample rate = ");
  Serial.print(myIMU.magneticFieldSampleRate());
  Serial.println(" Hz");
  Serial.println();
  Serial.print("Accelerometer sample rate = ");
  Serial.print(myIMU.accelerationSampleRate());
  Serial.println(" Hz");
  Serial.println();
  Serial.print("Gyrsoscope sample rate = ");
  Serial.print(myIMU.gyroscopeSampleRate());
  Serial.println(" Hz");
  Serial.println();


  // The accelerometer and gyroscope can be calibrated every time the board starts
  // or the values can be set by hand once, after entering calibration mode.
  if (calibrateAccelerometerAndGyroOnStart && !calibrationModeEnabled) {
    calibrateAccelerometerAndGyro();
  }


  if (calibrationModeEnabled) {
    calibrateSensors();
    while (1)
      ;
  }

  // Flash three times to indicate setup is over
  for (i = 0; i < 3; i++) {
    digitalWrite(LED_BUILTIN, HIGH);
    delay(100);
    digitalWrite(LED_BUILTIN, LOW);
    delay(100);
  }

  currentTime = millis();
}

void loop() {
  String cardinal;

  if (myIMU.accelerationAvailable() && myIMU.gyroscopeAvailable()) {
    myIMU.readAcceleration(ax, ay, az);
    myIMU.readGyroscope(gx, gy, gz);

    // Remove accelerometer bias
    ax -= accel_zerog[0];
    ay -= accel_zerog[1];
    az -= accel_zerog[2];

    // Remove gyro bias
    gx -= gyro_zerorate[0];
    gy -= gyro_zerorate[1];
    gz -= gyro_zerorate[2];

    previousTime = currentTime;                         // Previous time is stored before the actual time read
    currentTime = millis();                             // Current time actual time read
    elapsedTime = (currentTime - previousTime) / 1000;  // Divide by 1000 to get seconds

    // Currently the raw values are in degrees per second, deg/s, so we need to multiply by seconds (s) to get the angle in degrees
    gyroPitch += gx * elapsedTime;  // deg/s * s = deg
    gyroRoll += gy * elapsedTime;
    gyroYaw += gz * elapsedTime;

    // ----- Compensate pitch and roll for gyro yaw
    gyroPitch += gyroRoll * sin(gz * elapsedTime * DEG_TO_RAD);  // Transfer the roll angle to the pitch angle if the Z-axis has yawed
    gyroRoll += gyroPitch * sin(gz * elapsedTime * DEG_TO_RAD);  // Transfer the pitch angle to the roll angle if the Z-axis has yawed

    // ----- Accelerometer angle calculations
    accTotalVector = sqrt((ax * ax) + (ay * ay) + (az * az));   // Calculate the total (3D) vector
    accPitch = -asin((float)ay / accTotalVector) * RAD_TO_DEG;  //Calculate the pitch angle. The sign is negative because, as the pitch increases, the gravity vector increases on the accelerometer -Y axis.
    accRoll = asin((float)ax / accTotalVector) * RAD_TO_DEG;    //Calculate the roll angle

    gyroPitch = gyroPitch * 0.9 + accPitch * 0.1;  //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
    gyroRoll = gyroRoll * 0.9 + accRoll * 0.1;     //Correct the drift of the gyro roll angle with the accelerometer roll angle

    gyroPitchOutput = gyroPitchOutput * 0.70 + gyroPitch * 0.3;  //Take 70% of the output pitch value and 30% of the raw pitch value
    gyroRollOutput = gyroRollOutput * 0.70 + gyroRoll * 0.3;     //Take 70% of the output roll value and 30% of the raw roll value
  }


  if (myIMU.magneticFieldAvailable()) {
    myIMU.readMagneticField(mx, my, mz);
    float mag_data[] = { mx, my, mz };

    // Apply hard iron offsets
    for (int i = 0; i < 3; i++) {
      mag_data[i] -= mag_hardiron[i];
    }

    // Apply soft iron scaling
    for (int i = 0; i < 3; i++) {
      mag_data[i] *= mag_softiron[i];
    }

    mx = mag_data[0];
    my = mag_data[1];
    mz = mag_data[2];
  }

  magRoll = gyroRollOutput * DEG_TO_RAD;
  magPitch = gyroPitchOutput * DEG_TO_RAD;

  // ----- Apply the standard tilt formulas
  //Original formula
  //magXHor = mx * cos(magPitch) + my * sin(magRoll) * sin(magPitch) + mz * cos(magRoll) * sin(magPitch);
  //magYHor = my * cos(magRoll) + mz * sin(magRoll);
  magXHor = mx * cos(magPitch) - my * sin(magRoll) * sin(magPitch) + mz * cos(magRoll) * sin(magPitch);
  magYHor = my * cos(magRoll) + mz * sin(magRoll);


  // ----- Dampen any data fluctuations
  magXDampened = magXDampened * 0.7 + magXHor * 0.3;
  magYDampened = magYDampened * 0.7 + magYHor * 0.3;

  float headingRadians;

  if (useBoardMagnetometerReferentialToPointAtHeading) {
    // Use this if you want to use the board referential's X axis to point to the measured heading.
    headingRadians = atan2(-magYDampened, magXDampened);
  } else {
    // Use this if you want to use the side of the board opposite to the USB port to point to the measured heading.
    headingRadians = atan2(magXDampened, magYDampened);
  }

  float headingDegrees = headingRadians * RAD_TO_DEG;

  headingDegrees += geoDeclinationAngle;  // Geographic North

  if (headingDegrees >= 360.0) {
    headingDegrees -= 360.0;
  }

  if (headingDegrees < 0.0) {
    headingDegrees += 360.0;
  }

  if (headingDegrees > 348.75 || headingDegrees < 11.25) {
    cardinal = " N";
  } else if (headingDegrees > 11.25 && headingDegrees < 33.75) {
    cardinal = " NNE";
  } else if (headingDegrees > 33.75 && headingDegrees < 56.25) {
    cardinal = " NE";
  } else if (headingDegrees > 56.25 && headingDegrees < 78.75) {
    cardinal = " ENE";
  } else if (headingDegrees > 78.75 && headingDegrees < 101.25) {
    cardinal = " E";
  } else if (headingDegrees > 101.25 && headingDegrees < 123.75) {
    cardinal = " ESE";
  } else if (headingDegrees > 123.75 && headingDegrees < 146.25) {
    cardinal = " SE";
  } else if (headingDegrees > 146.25 && headingDegrees < 168.75) {
    cardinal = " SSE";
  } else if (headingDegrees > 168.75 && headingDegrees < 191.25) {
    cardinal = " S";
  } else if (headingDegrees > 191.25 && headingDegrees < 213.75) {
    cardinal = " SSW";
  } else if (headingDegrees > 213.75 && headingDegrees < 236.25) {
    cardinal = " SW";
  } else if (headingDegrees > 236.25 && headingDegrees < 258.75) {
    cardinal = " WSW";
  } else if (headingDegrees > 258.75 && headingDegrees < 281.25) {
    cardinal = " W";
  } else if (headingDegrees > 281.25 && headingDegrees < 303.75) {
    cardinal = " WNW";
  } else if (headingDegrees > 303.75 && headingDegrees < 326.25) {
    cardinal = " NW";
  } else if (headingDegrees > 326.25 && headingDegrees < 348.75) {
    cardinal = " NNW";
  }


  //The magnetic pitch and roll are flipped
  Serial.print("P R: ");
  Serial.print(magPitch * RAD_TO_DEG);
  Serial.print(" ");
  Serial.print(magRoll * RAD_TO_DEG);
  Serial.print(" Heading: ");
  Serial.print(headingDegrees, 0);
  Serial.println(cardinal);
}

void calibrateSensors() {
  Serial.println("");
  Serial.println("");
  Serial.println("Tilt Compensated Compass sensors calibration starting");

  calibrateAccelerometerAndGyro();
  calibrateMagnetometer();

  Serial.println("");
  Serial.println("");
  Serial.println("Calibration finished");
}

void calibrateAccelerometerAndGyro() {
  int i = 0;

  // Remove any existing calibrations
  for (i = 0; i < 3; i++) {
    accel_zerog[i] = 0;
    gyro_zerorate[i] = 0;
  }

  if (calibrationModeEnabled) {
    Serial.println("");
    Serial.println("");
    Serial.println("Accelerometer and Gyroscope Calibration Starting");
    Serial.println(F("Place board on a flat, stable surface!"));
    Serial.print(F("Calibration starting in 3..."));
    delay(1000);
    Serial.print("2...");
    delay(1000);
    Serial.print("1...");
    delay(1000);
    Serial.println("Now.");
  }

  i = 0;

  while (i < numberOfAccelerometerAndGyroCalibrationCycles) {
    if (myIMU.accelerationAvailable() && myIMU.gyroscopeAvailable()) {
      myIMU.readAcceleration(ax, ay, az);
      myIMU.readGyroscope(gx, gy, gz);

      accel_zerog[0] += ax;
      accel_zerog[1] += ay;
      accel_zerog[2] += az;

      gyro_zerorate[0] += gx;
      gyro_zerorate[1] += gy;
      gyro_zerorate[2] += gz;

      if (debugEnabled && i % 10 == 0) {
        Serial.print("Progress: ");
        Serial.print((100.0 * i) / numberOfAccelerometerAndGyroCalibrationCycles, 0);
        Serial.println("%");
      }

      analogWrite(LED_BUILTIN, map(i, 0, numberOfAccelerometerAndGyroCalibrationCycles, 255, 0));
      i++;
    }
  }

  analogWrite(LED_BUILTIN, -1);  // Disable PWM

  accel_zerog[0] /= numberOfAccelerometerAndGyroCalibrationCycles;
  accel_zerog[1] /= numberOfAccelerometerAndGyroCalibrationCycles;
  accel_zerog[2] /= numberOfAccelerometerAndGyroCalibrationCycles;
  accel_zerog[2] -= 1.0;  // Removes gravity

  gyro_zerorate[0] /= numberOfAccelerometerAndGyroCalibrationCycles;
  gyro_zerorate[1] /= numberOfAccelerometerAndGyroCalibrationCycles;
  gyro_zerorate[2] /= numberOfAccelerometerAndGyroCalibrationCycles;

  if (debugEnabled || calibrationModeEnabled) {
    Serial.println("");
    Serial.println("");
    Serial.println("Accelerometer and Gyroscope Calibration Results");
    Serial.println("");
    Serial.print("accel_zerog = [");
    Serial.print(accel_zerog[0], 4);
    Serial.print(", ");
    Serial.print(accel_zerog[1], 4);
    Serial.print(", ");
    Serial.print(accel_zerog[2], 4);
    Serial.println(" ]");
    Serial.println("");
    Serial.print("gyro_zerorate = [");
    Serial.print(gyro_zerorate[0], 4);
    Serial.print(", ");
    Serial.print(gyro_zerorate[1], 4);
    Serial.print(", ");
    Serial.print(gyro_zerorate[2], 4);
    Serial.println(" ]");

    if (calibrationModeEnabled) {
      delay(4000);
    }
  }
}

void calibrateMagnetometer() {
  int i = 0;
  float minmx = 32767;
  float maxmx = -32767;
  float minmy = 32767;
  float maxmy = -32767;
  float minmz = 32767;
  float maxmz = -32767;
  float mx, my, mz;
  float chord_x, chord_y, chord_z;  // Used for calculating scale factors
  float chord_average;


  // Remove any existing calibrations
  for (i = 0; i < 3; i++) {
    mag_hardiron[i] = 0;
    mag_softiron[i] = 0;
  }

  Serial.println("");
  Serial.println("");
  Serial.println("Magnetometer Calibration Started");
  Serial.println();
  Serial.println(F("Move the board around to calibrate."));
  Serial.print(F("Calibration starting in "));

  for (int i = 5; i > 0; i--) {
    Serial.print(i);
    Serial.print("...");
    delay(1000);
  }

  Serial.println("Now.");

  i = 0;

  while (i < numberOfMagnetometerCalibrationCycles) {
    if (myIMU.magneticFieldAvailable()) {
      myIMU.readMagneticField(mx, my, mz);

      minmx = min(mx, minmx);
      minmy = min(my, minmy);
      minmz = min(mz, minmz);
      maxmx = max(mx, maxmx);
      maxmy = max(my, maxmy);
      maxmz = max(mz, maxmz);

      if (i % 100 == 0) {
        Serial.print("Progress: ");
        Serial.print((100.0 * i) / numberOfMagnetometerCalibrationCycles, 0);
        Serial.println("%");
      }

      i++;
    }
  }

  // ----- Calculate hard-iron offsets
  mag_hardiron[0] = (maxmx + minmx) / 2;
  mag_hardiron[1] = (maxmy + minmy) / 2;
  mag_hardiron[2] = (maxmz + minmz) / 2;

  // ----- Calculate soft-iron scale factors
  chord_x = ((float)(maxmx - minmx)) / 2;  // Get average max chord length in counts
  chord_y = ((float)(maxmy - minmy)) / 2;
  chord_z = ((float)(maxmz - minmz)) / 2;

  chord_average = (chord_x + chord_y + chord_z) / 3;  // Calculate average chord length

  mag_softiron[0] = chord_average / chord_x;  // Calculate X scale factor
  mag_softiron[1] = chord_average / chord_y;  // Calculate Y scale factor
  mag_softiron[2] = chord_average / chord_z;

  Serial.println("");
  Serial.println("");
  Serial.println("Magnetometer Calibration Results");
  Serial.println("");
  Serial.print("mag_hardiron = [");
  Serial.print(mag_hardiron[0], 4);
  Serial.print(", ");
  Serial.print(mag_hardiron[1], 4);
  Serial.print(", ");
  Serial.print(mag_hardiron[2], 4);
  Serial.println(" ]");
  Serial.println("");
  Serial.print("mag_softiron = [");
  Serial.print(mag_softiron[0], 4);
  Serial.print(", ");
  Serial.print(mag_softiron[1], 4);
  Serial.print(", ");
  Serial.print(mag_softiron[2], 4);
  Serial.println(" ]");
}

J-M-L · July 22, 2025, 9:48am

that's because you did not use the code tags as recommended in How to get the best out of this forum

~~Given you shared an interesting piece of code, I will fix it for you~~

I gave it a tried but you messed up everything after adding the quote when the forum had already tried to add code tags. It's a mess now ...

Please correct your post.

There is a small pencil below your existing post.

click on this pencil ➜ that will let you edit your post.
Select the part of the text that corresponds to the code and delete that
go to your source code in the IDE, make sure to properly indent the code in the IDE (this can be done by pressing ctrlT on a PC or cmdT on a Mac).
copy the indented code in the IDE
On an empty line, Click on the <code/> icon in the toolbar to indicate that you want a code section (you'll see two lines with three backticks with text to be replaced in between)
paste the code in between the three backticks lines (replace the template text)
click Save Edit

thanks

jim-p · July 22, 2025, 10:10am

Does it need to be level?
You do need to fix the code.

acidrain21001 · July 22, 2025, 10:29am

First time usage - Calibration

Before you start, this project can run in two modes, calibration and normal mode.
The first time you run it you should run with this variable set to true:

Like I explain in the original post this runs in two modes, Calibration and Normal Mode, that's specific to the calibration mode. For you to be able to calculate the accelerometer and gyroscope offsets the board needs to be leveled and still.
During normal mode you can pitch and rotate it.
There's no need to go back to the code.

jim-p · July 22, 2025, 10:36am

You did not say that it need to be level in the original post. Maybe you should correct that.

There's no need to go back to the code.

I think there is. As @J-M-L pointed ot it's totally unreadable, you need to fix it.

acidrain21001 · July 22, 2025, 11:03am

Ok, the issue I was having was with the new rich text editor.
As soon as I pressed the code button it tried to format regular paragraphs as code, even if they weren't selected.

acidrain21001 · July 22, 2025, 11:07am

I misunderstood you, I thought you were saying that if the board needed to be leveled to indicate the correct heading then I needed to go back to the code.

jim-p · July 22, 2025, 11:29am

No just during calibration.
Thamks for posting the code in the correct format, now people can easily copy it.

J-M-L · July 22, 2025, 12:27pm

thanks for fixing it

Topic		Replies	Views
Magnetometer Easy Calibration Programming	21	627	May 25, 2025
Reliable tilt compensated compass system Sensors	54	9976	June 20, 2023
Two wheel balncing robot with adxl345 General Guidance	68	618	March 22, 2026
Connecting an IMU GY N601N1, reading angles General Guidance	89	665	July 6, 2026
Calibrated AHRS compass not working properly Programming	13	337	September 26, 2024

Tilt Compensated Compass for Arduino Nano 33 Rev 2

Related topics