Troubleshooting IMU Orientation Issues on Arduino Nano BLE Sense Rev 2

Using Arduino Programming Questions
1

I'm using the Arduino Nano BLE Sense Rev 2 board and its IMU to get real-time absolute orientation angles (pitch, roll, yaw). However, I am encountering an issue. When I perform a pitch rotation, the yaw value also changes significantly. Additionally, when I actually rotate the board by 90 degrees, the plotted yaw readings show only around 40 degrees. Could this be due to magnetometer calibration or some other issue? Below is my code:

#include <Arduino_BMI270_BMM150.h>
#include <MadgwickAHRS.h>

Madgwick filter;
unsigned long microsPerReading, microsPrevious;

// Calibration parameters for magnetometer
float magOffsetX, magOffsetY, magOffsetZ;
float magScaleX = 1.0, magScaleY = 1.0, magScaleZ = 1.0;
// Calibration parameters for gyroscope
float gyroOffsetX = 0, gyroOffsetY = 0, gyroOffsetZ = 0;
// Calibration parameters for accelerometer
float accelOffsetX = 0, accelOffsetY = 0, accelOffsetZ = 0;
float accelScaleX = 1.0, accelScaleY = 1.0, accelScaleZ = 1.0;

void setup() {
  Serial.begin(115200); // Increase baud rate to handle higher frequency data output
  while (!Serial);

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

  Serial.println("IMU initialized successfully!");

  // Calibrate sensors
  calibrateGyroscope();
  calibrateAccelerometer();
  calibrateMagnetometer();

  // Set sampling rate (e.g., 100 Hz) and beta parameter
  filter.begin(100);
  filter.setBeta(0.8); // Adjust beta parameter to improve stability
  microsPerReading = 1000000 / 100;
  microsPrevious = micros();
}

void loop() {
  float ax, ay, az;
  float gx, gy, gz;
  float mx, my, mz;

  // Check if it's time for the next sample
  if (micros() - microsPrevious >= microsPerReading) {
    microsPrevious += microsPerReading;

    // Read accelerometer data
    if (IMU.readAcceleration(ax, ay, az)) {
      // Apply accelerometer calibration
      ax = (ax - accelOffsetX) / accelScaleX;
      ay = (ay - accelOffsetY) / accelScaleY;
      az = (az - accelOffsetZ) / accelScaleZ;
    }

    // Read gyroscope data
    if (IMU.readGyroscope(gx, gy, gz)) {
      // Apply gyroscope calibration
      gx -= gyroOffsetX;
      gy -= gyroOffsetY;
      gz -= gyroOffsetZ;
    }

    // Read magnetometer data
    if (IMU.readMagneticField(mx, my, mz)) {
      // Calibrate magnetometer data
      mx = (mx - magOffsetX) / magScaleX;
      my = (my - magOffsetY) / magScaleY;
      mz = (mz - magOffsetZ) / magScaleZ;
    }

    // Update Madgwick filter
    filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);

    // Get Euler angles
    float roll = filter.getRoll();
    float pitch = filter.getPitch();
    float yaw = filter.getYaw();

    // Output Euler angles
    Serial.print(roll);
    Serial.print('\t');
    Serial.print(pitch);
    Serial.print('\t');
    Serial.println(yaw);
  }
}

void calibrateGyroscope() {
  Serial.println("Starting gyroscope calibration...");
  delay(5000); // Give user 5 seconds to keep the sensor still

  float sumX = 0, sumY = 0, sumZ = 0;
  const int sampleCount = 1000;

  for (int i = 0; i < sampleCount; i++) {
    if (IMU.readGyroscope(gyroOffsetX, gyroOffsetY, gyroOffsetZ)) {
      sumX += gyroOffsetX;
      sumY += gyroOffsetY;
      sumZ += gyroOffsetZ;
      delay(5);
    }
  }

  // Calculate average offset
  gyroOffsetX = sumX / sampleCount;
  gyroOffsetY = sumY / sampleCount;
  gyroOffsetZ = sumZ / sampleCount;

  Serial.println("Gyroscope calibration complete.");
  Serial.print("OffsetX: ");
  Serial.print(gyroOffsetX);
  Serial.print(" OffsetY: ");
  Serial.print(gyroOffsetY);
  Serial.print(" OffsetZ: ");
  Serial.println(gyroOffsetZ);
}

void calibrateAccelerometer() {
  Serial.println("Starting accelerometer calibration...");
  Serial.println("Place the sensor still on a flat surface for 60 seconds.");
  delay(5000); // Give user 5 seconds to keep the sensor still

  float minX = 32767, minY = 32767, minZ = 32767;
  float maxX = -32768, maxY = -32768, maxZ = -32768;

  unsigned long startTime = millis();
  while (millis() - startTime < 60000) {
    float x, y, z;
    if (IMU.readAcceleration(x, y, z)) {
      if (x < minX) minX = x;
      if (y < minY) minY = y;
      if (z < minZ) minZ = z;
      if (x > maxX) maxX = x;
      if (y > maxY) maxY = y;
      if (z > maxZ) maxZ = z;
    }
  }

  // Calculate calibration offset and scale
  accelOffsetX = (maxX + minX) / 2.0;
  accelOffsetY = (maxY + minY) / 2.0;
  accelOffsetZ = (maxZ + minZ) / 2.0;

  accelScaleX = (maxX - minX) / 2.0;
  accelScaleY = (maxY - minY) / 2.0;
  accelScaleZ = (maxZ - minZ) / 2.0;

  Serial.println("Accelerometer calibration complete.");
  Serial.print("OffsetX: ");
  Serial.print(accelOffsetX);
  Serial.print(" OffsetY: ");
  Serial.print(accelOffsetY);
  Serial.print(" OffsetZ: ");
  Serial.println(accelOffsetZ);

  Serial.print("ScaleX: ");
  Serial.print(accelScaleX);
  Serial.print(" ScaleY: ");
  Serial.print(accelScaleY);
  Serial.print(" ScaleZ: ");
  Serial.println(accelScaleZ);
}

void calibrateMagnetometer() {
  Serial.println("Starting magnetometer calibration...");
  Serial.println("Move the sensor in a figure 8 pattern until calibration is complete.");
  delay(5000); // Give user 5 seconds to start moving the sensor

  float minX = 32767, minY = 32767, minZ = 32767;
  float maxX = -32768, maxY = -32768, maxZ = -32768;

  unsigned long startTime = millis();
  while (millis() - startTime < 60000) { // Increase calibration time to 60 seconds
    float x, y, z;
    if (IMU.magneticFieldAvailable()) {
      IMU.readMagneticField(x, y, z);
      if (x < minX) minX = x;
      if (y < minY) minY = y;
      if (z < minZ) minZ = z;
      if (x > maxX) maxX = x;
      if (y > maxY) maxY = y;
      if (z > maxZ) maxZ = z;
    }
  }

  // Calculate calibration offset and scale
  magOffsetX = (maxX + minX) / 2.0;
  magOffsetY = (maxY + minY) / 2.0;
  magOffsetZ = (maxZ + minZ) / 2.0;

  magScaleX = (maxX - minX) / 2.0;
  magScaleY = (maxY - minY) / 2.0;
  magScaleZ = (maxZ - minZ) / 2.0;

  Serial.println("Magnetometer calibration complete.");
  Serial.print("OffsetX: ");
  Serial.print(magOffsetX);
  Serial.print(" OffsetY: ");
  Serial.print(magOffsetY);
  Serial.print(" OffsetZ: ");
  Serial.println(magOffsetZ);

  Serial.print("ScaleX: ");
  Serial.print(magScaleX);
  Serial.print(" ScaleY: ");
  Serial.print(magScaleY);
  Serial.print(" ScaleZ: ");
  Serial.println(magScaleZ);
  delay(5000);
}
2

Please edit your post to add code tags. Instructions can be found in the "How to get the best out of this forum" post.

The fusion code won't work as expected unless the gyro and the magnetometer are both correctly calibrated. You may also need to calibrate the accelerometer.

An excellent tutorial covering the latter two sensors is Tutorial: How to calibrate a compass (and accelerometer) with Arduino | Underwater Arduino Data Loggers

3

Did you purposefully turn off the required magnetometer calibration? The primitive approach in the code won't work well, but it is much better than nothing.

4

I edited the new code with accelerometer, gyro and magnetometer calibration in the code, but the Euler angle from the print looks still very weird. When the IMU is stationary, the data is not stable enough, and the Euler angles after yaw, pitch, and roll rotations are clearly incorrect. Could this be because the REV2 is not suitable for the Madgwick filter, or is there an issue with my calibration process? I've tried calculating yaw using yawMag = atan2(mY, mX) * 180 / PI, but the yaw value changes significantly with pitch rotation. Why is this happening? Could it still be due to an issue with the magnetometer calibration? Thank you very much for your help!

5

Sorry, in the running code, I did add calibrateMagnetometer() in the setup and did about 1 min rotating in random direction.

6

Post the revised code, using code tags. You can check the calibration of the sensors by making 3D plots. The corrected mag/accel data should be spheres centered on the origin, as shown in this forum post.

Two comments on potential fatal errors in the original code:

  1. the gyro data must be scaled to radians per second. They are not.

  2. Verify from the sensor data sheet that the coordinate systems of the gyro, accelerometer and the accelerometer have the same axial orientations and are right handed. This line of code is suspect:

    filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);

I've tried calculating yaw using yawMag = atan2(mY, mX) * 180 / PI, but the yaw value changes significantly with pitch rotation.

That is expected. That equation works only if the magnetometer is calibrated and used for measurement when it is perfectly level, with Z vertical.

7

Beats me why so many try to re-invent the wheel under the pretext of "need to learn".
A $40 flight controller and free iNav or whatever software has already done the hard yards and even more plus plus plus...........

9

See this post for a suggestion BLE Sense Rev2 IMU & Magnetometer Sensor Fusion - values drift, what is the correct axis orientation for BMI270/BMM150? - #3 by jremington