I want to get roll pitch and yaw data from BMI270 and BMM150 sensors. I have a Nano 33 BLE Rev2 and these sensors are onboard it. I used adafruit ahrs library's mahony filter to remove noise. The data is accrate and noise free when the board is stationary. But when I tilt it a bit and make it stationary in some other position, there are huge spikes in values and only after about 2-3 seconds I get proper data again. I have already done calibration too.
#include "Adafruit_AHRS_Mahony.h"
#include "Adafruit_AHRS_Madgwick.h"
#include <Adafruit_AHRS_NXPFusion.h>
#include <Arduino_BMI270_BMM150.h>
#include <Wire.h>
// Mag calibration values are calculated via ahrs_calibration.
// These values must be determined for each baord/environment.
// See the image in this sketch folder for the values used
// below.
// Offsets applied to raw x/y/z mag values
float mag_offsets[3] = { -20.80F, 10.19F, -18.41 };
// Soft iron error compensation matrix
float mag_softiron_matrix[3][3] = { { 1.004, -0.006, 0.003 },
{ -0.006, 0.984, -0.004 },
{ 0.003, -0.004, 1.013 } };
float mag_field_strength = 44.75F;
// Offsets applied to compensate for gyro zero-drift error for x/y/z
// Raw values converted to rad/s based on 250dps sensitiviy (1 lsb = 0.00875 rad/s)
float rawToDPS = 0.00875F;
float dpsToRad = 0.017453293F;
/*float gyro_zero_offsets[3] = { 175.0F * rawToDPS * dpsToRad,
-729.0F * rawToDPS * dpsToRad,
101.0F * rawToDPS * dpsToRad };
*/
// Mahony is lighter weight as a filter and should be used
// on slower systems
Adafruit_Mahony filter;
//Adafruit_Madgwick filter;
//Adafruit_NXPSensorFusion filter;
float accelX, accelY, accelZ, // units m/s/s i.e. accelZ if often 9.8 (gravity)
gyroX, gyroY, gyroZ, // units dps (degrees per second)
gyroDriftX, gyroDriftY, gyroDriftZ, // units dps
magX, magY, magZ, mag_x, mag_y;
long lastInterval, lastTime, a;
void setup() {
Serial.begin(9600);
// Wait for the Serial Monitor to open (comment out to run without Serial Monitor)
while (!Serial)
;
Serial.println(F("Adafruit AHRS BLE Fusion Example"));
IMU.begin();
calibrateIMU(250, 250);
lastTime = micros();
// Filter expects 25 samples per second
// The filter only seems to be responsive with a much smaller value, though!
// ToDo: Figure out the disparity between actual refresh rate and the value
// provided here!
filter.begin(25);
}
void loop() {
readIMU();
unsigned long currentTime = micros();
lastInterval = currentTime - lastTime; // expecting this to be ~104Hz +- 4%
lastTime = currentTime;
// Update the filter
filter.update(gyroX, gyroY, gyroZ,
accelX, accelY, accelZ,
magX, magY, magZ);
// Print the orientation filter output
if (millis() - a >= 250) {
a += 250;
float roll = filter.getRoll();
float pitch = filter.getPitch();
float heading = filter.getYaw();
Serial.print(millis());
Serial.print(" - Orientation: ");
Serial.print(roll);
Serial.print(" ");
Serial.print(pitch);
Serial.print(" ");
Serial.println(heading);
/*
// Print the orientation filter output in quaternions.
// This avoids the gimbal lock problem with Euler angles when you get
// close to 180 degrees (causing the model to rotate or flip, etc.)
float qw, qx, qy, qz;
filter.getQuaternion(&qw, &qx, &qy, &qz);
Serial.print(millis());
Serial.print(" - Quat: ");
Serial.print(qw);
Serial.print(" ");
Serial.print(qx);
Serial.print(" ");
Serial.print(qy);
Serial.print(" ");
Serial.println(qz);
*/
}
}
void readIMU() {
IMU.readAcceleration(accelX, accelY, accelZ);
IMU.readGyroscope(gyroX, gyroY, gyroZ);
IMU.readMagneticField(magX, magY, magZ);
}
void calibrateIMU(int delayMillis, int calibrationMillis) {
int calibrationCount = 0;
delay(delayMillis); // to avoid shakes after pressing reset button
float sumX, sumY, sumZ;
int startTime = millis();
while (millis() < startTime + calibrationMillis) {
readIMU();
// in an ideal world gyroX/Y/Z == 0, anything higher or lower represents drift
sumX += gyroX;
sumY += gyroY;
sumZ += gyroZ;
calibrationCount++;
}
if (calibrationCount == 0) {
Serial.println("Failed to calibrate");
}
gyroDriftX = sumX / calibrationCount;
gyroDriftY = sumY / calibrationCount;
gyroDriftZ = sumZ / calibrationCount;
}
