Hello people!

I am trying to use the IMU of M5 Atom Matrix with a HMC5883L (magnetometer), nevertheless, when i join the code of HMC5883L and MPU6886, the sensors don't work well (they give wrong informations about roll, pitch and yaw). The accelerometer of MPU6886 keeps the acceleration values freezed (this must be the reason roll and pitch show wrong values). Yaw values given by HMC5883L are strange too. When I test the two codes separated (HMC5883L is tested connected to M5 Atom Matrix), the sensors work perfectly fine. I use I2C ports 25 (SDA) and 21 (SCL). I've already used 21 (SDA) and 22 (SCL) too, but the same problems appear.

My code is shown below:

#include <Wire.h>
#include <HMC5883L.h>
#include "M5Atom.h"

#define CALIBRATION__MAGN_USE_EXTENDED true

const float magn_ellipsoid_center[3] = {-112.932, -417.847, -153.557};
const float magn_ellipsoid_transform[3][3] = {{0.922957, 0.0179191, -0.0157219}, {0.0179191, 0.984359, 0.0385355}, {-0.0157219, 0.0385355, 0.890763}};
float pitch, pitch1, roll, roll1, yaw, yaw1;
float accX, accY, accZ, gyroX, gyroY, gyroZ, heading;
float magnetom[3];
float magnetom_tmp[3];

HMC5883L compass;

void setup()
{
// Enable Serial, enable I2C, enable Display
M5.begin(true, true, true);
//Wire.begin(25, 21);

Serial.begin(115200);

// Initialize HMC5883L
Serial.println("Initialize HMC5883L");
while (!compass.begin())
{
Serial.println("Could not find a valid HMC5883L sensor, check wiring!");
delay(300);
}

M5.IMU.Init();

// Set measurement range
// +/- 0.88 Ga: HMC5883L_RANGE_0_88GA
// +/- 1.30 Ga: HMC5883L_RANGE_1_3GA (default)
// +/- 1.90 Ga: HMC5883L_RANGE_1_9GA
// +/- 2.50 Ga: HMC5883L_RANGE_2_5GA
// +/- 4.00 Ga: HMC5883L_RANGE_4GA
// +/- 4.70 Ga: HMC5883L_RANGE_4_7GA
// +/- 5.60 Ga: HMC5883L_RANGE_5_6GA
// +/- 8.10 Ga: HMC5883L_RANGE_8_1GA
compass.setRange(HMC5883L_RANGE_1_9GA);

// Set measurement mode
// Idle mode: HMC5883L_IDLE
// Single-Measurement: HMC5883L_SINGLE
// Continuous-Measurement: HMC5883L_CONTINOUS (default)
compass.setMeasurementMode(HMC5883L_CONTINOUS);

// Set data rate
// 0.75Hz: HMC5883L_DATARATE_0_75HZ
// 1.50Hz: HMC5883L_DATARATE_1_5HZ
// 3.00Hz: HMC5883L_DATARATE_3HZ
// 7.50Hz: HMC5883L_DATARATE_7_50HZ
// 15.00Hz: HMC5883L_DATARATE_15HZ (default)
// 30.00Hz: HMC5883L_DATARATE_30HZ
// 75.00Hz: HMC5883L_DATARATE_75HZ
compass.setDataRate(HMC5883L_DATARATE_15HZ);

// Set number of samples averaged
// 1 sample: HMC5883L_SAMPLES_1 (default)
// 2 samples: HMC5883L_SAMPLES_2
// 4 samples: HMC5883L_SAMPLES_4
// 8 samples: HMC5883L_SAMPLES_8
compass.setSamples(HMC5883L_SAMPLES_4);

}

void Matrix_Vector_Multiply(const float a[3][3], const float b[3], float out[3])
{
for(int x = 0; x < 3; x++)
{

float tiltCompensate(Vector mag, float roll, float pitch)
{

// Some of these are used twice, so rather than computing them twice in the algorithem we precompute them before hand.

float cosRoll = cos(roll);
float sinRoll = sin(roll);
float cosPitch = cos(pitch);
float sinPitch = sin(pitch);

// Tilt compensation
float Xh = mag.XAxis * cosPitch + mag.ZAxis * sinPitch;
float Yh = mag.XAxis * sinRoll * sinPitch + mag.YAxis * cosRoll - mag.ZAxis * sinRoll * cosPitch;

float heading = atan2(Yh, Xh);

return heading;
}

float correctAngle(float heading)
{
if (heading < 0) { heading += 2 * PI; }
if (heading > 2 * PI) { heading -= 2 * PI; }

return heading;
}

void loop()
{
Vector raw = compass.readRaw();
Vector norm = compass.readNormalize();

M5.IMU.getAccelData(&accX, &accY, &accZ);
M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
M5.IMU.getMadData(&pitch, &roll, &yaw);

heading = atan2(magnetom[1], magnetom[0]);
//heading=tiltCompensate(norm, roll, pitch);

float declinationAngle = -(43 + (23.0 / 60.0)) / (180 / M_PI);
heading += declinationAngle;
heading = correctAngle(heading);
heading = heading * 180/M_PI;

magnetom[0] = raw.XAxis;
magnetom[1] = raw.YAxis;
magnetom[2] = raw.ZAxis;

//soft iron calibration
#if CALIBRATION__MAGN_USE_EXTENDED == true
for (int i = 0; i < 3; i++)
magnetom_tmp[i] = magnetom[i] - magn_ellipsoid_center[i];
Matrix_Vector_Multiply(magn_ellipsoid_transform, magnetom_tmp, magnetom);
//hard iron calibration in the HMC5883L.cpp code
#else
magnetom[0] = norm.XAxis;
magnetom[1] = norm.YAxis;
magnetom[2] = norm.ZAxis;
#endif

//NWU
//MadgwickAHRSupdate(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD, accX, accY, accZ, magnetom[1], -magnetom[0], -magnetom[2], &pitch1, &roll1, &yaw1);

roll = abs(roll-90);

Serial.printf("roll: %5.2f, pitch: %5.2f, heading: %5.2f", roll, pitch, heading);
Serial.printf("\n");
Serial.printf("ax: %5.2f, ay: %5.2f, az: %5.2f", accX, accY, accZ);
Serial.printf(" g");
Serial.printf("\n");
// Serial.printf("magX: %5.2f, magY: %5.2f, magZ: %5.2f", norm.XAxis, norm.YAxis, norm.ZAxis);
// Serial.printf("\n");

delay(50);
}