Hi everyone,
I'm working on an IMU project using the Madgwick AHRS algorithm with a BMI270 & BMM150 sensor on an Arduino Nano 33 BLE Sense. The setup works well when the sensor is placed horizontally or on its side; I get good Roll and Pitch angles, they are about 0°/90°/180°. However, when I place the sensor vertically(whatever the direction is), the Roll angle becomes incorrect. Instead of seeing 0° or 90° (or even 180°), I often get values around 25°, 45° or some other incorrect value. However, the Pitch angle remains really accurate at around 90° when vertical.
Here are some details of my setup:
-
I’ve tried calibrating the accelerometer, gyroscope, and magnetometer, and I’m using those values in my code. I am not sure if it's correct or not, the values of roll and pitch seems stable and reasonable when the sensor is placed horizontally or on its side.
-
I consider using another fusion algorithm 'Mahony', the roll angle is also incorrect.. there's no improvement to this value.
Below is my code:
#include <ReefwingAHRS.h>
#include <Arduino_BMI270_BMM150.h>
ReefwingAHRS ahrs;
SensorData data;
// Saved calibration offset values (from magnetometer calibration results)
float mx_offset = 80.50;
float my_offset = 29.00;
float mz_offset = 62.50;
float mx_scale = 0.87;
float my_scale = 1.37;
float mz_scale = 0.89;
// Saved IMU calibration offset values
float ax_offset = 0.03;
float ay_offset = -0.02;
float az_offset = -0.01;
float gx_offset = -0.07;
float gy_offset = -0.01;
float gz_offset = -0.11;
// Display and Loop Frequency
int loopFrequency = 0;
const long displayPeriod = 1000; // Output once per second
unsigned long previousMillis = 0;
const long totalDuration = 20000; // 20 seconds
unsigned long startMillis;
bool started = false;
void setup() {
// Initialize serial communication
Serial.begin(115200);
while (!Serial);
// Initialize IMU
if (IMU.begin()) {
Serial.println("IMU initialized.");
Serial.println("Using stored calibration values:");
Serial.print("ax_offset: "); Serial.println(ax_offset);
Serial.print("ay_offset: "); Serial.println(ay_offset);
Serial.print("az_offset: "); Serial.println(az_offset);
Serial.print("gx_offset: "); Serial.println(gx_offset);
Serial.print("gy_offset: "); Serial.println(gy_offset);
Serial.print("gz_offset: "); Serial.println(gz_offset);
} else {
Serial.println("Failed to initialize IMU!");
while (1); // Stop the program if IMU initialization fails
}
// Initialize AHRS
ahrs.begin();
ahrs.setFusionAlgorithm(SensorFusion::MADGWICK);
ahrs.setDeclination(0.89); // Southampton, UK
Serial.print("Detected Board - ");
Serial.println(ahrs.getBoardTypeString());
if (ahrs.getBoardType() == BoardType::NANO33BLE_SENSE_R2) {
Serial.println("BMI270 & BMM150 IMUs Connected.");
} else {
Serial.println("IMUs Not Detected.");
while (1);
}
}
void loop() {
if (Serial.available() > 0 && !started) {
char received = Serial.read();
if (received == 'S') {
startMillis = millis();
started = true;
}
}
if (started && millis() - startMillis < totalDuration) {
// Read sensor data and apply offsets
if (IMU.gyroscopeAvailable()) {
IMU.readGyroscope(data.gx, data.gy, data.gz);
data.gx -= gx_offset;
data.gy -= gy_offset;
data.gz -= gz_offset;
}
if (IMU.accelerationAvailable()) {
IMU.readAcceleration(data.ax, data.ay, data.az);
data.ax -= ax_offset;
data.ay -= ay_offset;
data.az -= az_offset;
}
if (IMU.magneticFieldAvailable()) {
IMU.readMagneticField(data.mx, data.my, data.mz);
data.mx = (data.mx - mx_offset) * mx_scale;
data.my = (data.my - my_offset) * my_scale;
data.mz = (data.mz - mz_offset) * mz_scale;
}
// Update AHRS data
ahrs.setData(data);
ahrs.update();
// Output once every second
if (millis() - previousMillis >= displayPeriod) {
Serial.print("--> Roll: ");
Serial.print(ahrs.angles.roll, 2);
Serial.print("\tPitch: ");
Serial.print(ahrs.angles.pitch, 2);
Serial.print("\tYaw: ");
Serial.print(ahrs.angles.yaw, 2);
Serial.print("\tHeading: ");
Serial.print(ahrs.angles.heading, 2);
Serial.print("\tLoop Frequency: ");
Serial.print(loopFrequency);
Serial.println(" Hz");
loopFrequency = 0;
previousMillis = millis();
}
loopFrequency++;
} else if (started && millis() - startMillis >= totalDuration) {
Serial.println("15 seconds elapsed. Stopping data output.");
while (1);
}
}
Questions:
- What could be causing the Roll angle to be incorrect when the sensor is vertically oriented?
- Could this be due to the Madgwick algorithm or possibly interference from the magnetometer?
- Are there any specific adjustments or calibrations I should consider for vertical placements?
- Should I consider modifying the fusion algorithm settings?
By the way, the code I'm currently working with is based on a sample provided in a GitHub repository specifically for the Arduino Nano 33 BLE Sense (GitHub - Reefwing-Software/Reefwing-AHRS: Attitude and Heading Reference System (AHRS)). If you have a accurate code example for calculating Roll, Pitch, and Yaw using the BMI270 & BMM150 sensors (even if it's using a different algorithm than Madgwick), I'd be extremely grateful if you could share it!