Hi, I'm trying to fuse accel, giro and mag with Mahony or Madgwick algorithms, but in all algorithms the roll, pitch and yall are completely wrong and all values change every time.
My board: esp32 lilygo t-display s3.
Sensor: GY-87 with MPU6050 and HMC5883L
Includes:
MPU6050.h - https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
HMC5883L.h - https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/HMC5883L
SensorFusion.h - GitHub - aster94/SensorFusion: A simple implementation of some complex Sensor Fusion algorithms
My code:
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include "HMC5883L.h"
#include "SensorFusion.h"
SF fusion;
MPU6050 accelgyro;
HMC5883L mag;
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t mx, my, mz;
float pitch, roll, yaw;
float deltat;
int qtd = 0;
void setup() {
Wire.begin(43, 44);
accelgyro.setI2CMasterModeEnabled(false);
accelgyro.setI2CBypassEnabled(true) ;
accelgyro.setSleepEnabled(false);
Serial.begin(9600);
// initialize device
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
mag.initialize();
Serial.println(mag.testConnection() ? "HMC5883L connection successful" : "HMC5883L connection failed");
// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
accelgyro.CalibrateGyro();
accelgyro.CalibrateAccel();
accelgyro.PrintActiveOffsets();
delay(2000);
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
mag.getHeading(&mx, &my, &mz);
deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update
//choose only one of these two:
//fusion.MahonyUpdate(gx * DEG_TO_RAD, gy * DEG_TO_RAD, gz * DEG_TO_RAD, ax, ay, az, deltat); //mahony is suggested if there isn't the mag and the mcu is slow
fusion.MahonyUpdate(gx * DEG_TO_RAD, gy * DEG_TO_RAD, gz * DEG_TO_RAD, ax, ay, az, mx, my, mz, deltat);
//fusion.MadgwickUpdate(gx * DEG_TO_RAD, gy * DEG_TO_RAD, gz * DEG_TO_RAD, ax, ay, az, mx, my, mz, deltat); //else use the magwick, it is slower but more accurate
pitch = fusion.getPitch();
roll = fusion.getRoll(); //you could also use getRollRadians() ecc
yaw = fusion.getYaw();
if (qtd == 100) {
Serial.print("Pitch:\t"); Serial.println(pitch);
Serial.print("Roll:\t"); Serial.println(roll);
Serial.print("Yaw:\t"); Serial.println(yaw);
Serial.println();
qtd = 0;
}
else {
qtd ++;
}
/*
// display tab-separated accel/gyro x/y/z values
Serial.print("a/g:\t");
Serial.print(ax); Serial.print("\t");
Serial.print(ay); Serial.print("\t");
Serial.print(az); Serial.print("\t");
Serial.print(gx); Serial.print("\t");
Serial.print(gy); Serial.print("\t");
Serial.print(gz);Serial.print("\t");
Serial.print("mag:\t");
Serial.print(mx); Serial.print("\t");
Serial.print(my); Serial.print("\t");
Serial.print(mz); Serial.print("\t");
// To calculate heading in degrees. 0 degree indicates North
float heading = atan2(my, mx);
if(heading < 0)
heading += 2 * M_PI;
Serial.print("heading:\t");
Serial.println(heading * 180/M_PI);
delay (50);
*/
}
Raw data of all sensors:
a/g: -74 61 16250 -195 211 158 mag: 306 -60 134 heading: 348.91
a/g: -161 66 16341 214 447 -178 mag: 306 -61 135 heading: 348.73
a/g: -3 90 16211 -102 -151 293 mag: 304 -60 133 heading: 348.84
a/g: -154 -50 16330 52 -233 -85 mag: 304 -61 134 heading: 348.65
a/g: -40 117 16205 -114 -120 -77 mag: 306 -62 134 heading: 348.55
a/g: -78 26 16276 -19 25 -71 mag: 306 -61 133 heading: 348.73
a/g: -171 130 16367 109 -272 -56 mag: 305 -62 134 heading: 348.51
a/g: -105 62 16222 -220 324 -201 mag: 305 -62 133 heading: 348.51
a/g: -133 163 16379 49 320 -303 mag: 305 -61 133 heading: 348.69
Roll, pitch, yall
Pitch: -22.43 Roll: 134.75 Yaw: 202.64
Pitch: -38.01 Roll: 163.21 Yaw: 267.59
Pitch: -66.01 Roll: 147.37 Yaw: 357.98
Pitch: -47.74 Roll: 97.80 Yaw: 109.48
Pitch: -20.81 Roll: 109.34 Yaw: 160.89
Pitch: -8.29 Roll: 138.34 Yaw: 204.62
Pitch: -23.83 Roll: 167.53 Yaw: 242.21
Pitch: -48.21 Roll: -170.58 Yaw: 275.90
Can anyone help me with some idea about the problem?
Thank you