I am using an ESP-32 S3 to calculate Yaw/Pitch/Roll from a Haitronic MPU-9250.
Managed to get the Pitch and Roll angles accurately but I am unable to get a proper Yaw angle. I am afraid it is due to the Magnetometer not being calibrated correctly.
The library I am using -> GitHub - hideakitai/MPU9250: Arduino library for MPU9250 Nine-Axis (Gyro + Accelerometer + Compass) MEMS MotionTracking™ Device
I have used the calibration code in the library to calibrate my IMU hence the values for the bias. The declination was calculated from the global magnetic field using an online calculator.
Kindly, I need help solving the yaw, depending on where I look it, when starting the device the initial yaw angle is never 0. for example if I look east its at 40 degrees. If I look west its at 100 degrees. Never does it go from 0 to 90. Nor have the same value.
If possible, a simple yet effective guide on calibrating the magnetometer or a better library as I've tried everything I could think of so far. Thank you in advance.
The Wiring:
MPU-9250 -> ESP-32 S3 Dev module
VCC -> 3.3V
GND & AD0* -> GND
SDA** -> GPIO 4
SCL**-> GPIO 5
**The GPIOs on the ESP-32 S3 are fully programmable and can select any for SDA/SCL.
*AD0 is shorted to GND to enable the use of the Magnetometer on the MPU9250.
The Code:
#include "MPU9250.h"
MPU9250Setting setting;
MPU9250 mpu;
float cRoll = 0, cPitch = 0, cYaw = 0;
unsigned long previousTime = 0; // Stores the previous time
unsigned long currentTime = 0; // Stores the current time
unsigned long elapsedTime = 0;
//
const float Pi = 3.14159;
//
void setup() {
Serial.begin(115200);
Wire.begin(4, 5);
mpu.verbose(true);
delay(2000);
setMPU();
if (!mpu.setup(0x68, setting, Wire)) { // change to your own address
while (1) {
Serial.println("MPU connection failed. Please check your connection with `connection_check` example.");
delay(5000);
}
}
mpu.setMagneticDeclination(4.35);
mpu.setAccBias(10.84, -1.02, 28.18);
mpu.setGyroBias(-3.30, 3.78, 1.61);
mpu.setMagBias(176.21, 830.72, -701.03);
mpu.setMagScale(1.54, 0.78, 0.93);
mpu.verbose(false);
}
void setMPU() {
setting.accel_fs_sel = ACCEL_FS_SEL::A16G;
setting.gyro_fs_sel = GYRO_FS_SEL::G2000DPS;
setting.mag_output_bits = MAG_OUTPUT_BITS::M16BITS;
setting.fifo_sample_rate = FIFO_SAMPLE_RATE::SMPL_200HZ;
setting.gyro_fchoice = 0x03;
setting.gyro_dlpf_cfg = GYRO_DLPF_CFG::DLPF_41HZ;
setting.accel_fchoice = 0x01;
setting.accel_dlpf_cfg = ACCEL_DLPF_CFG::DLPF_45HZ;
return;
}
void loop() {
if (mpu.update()) {
//
float aX = mpu.getAccX();
float aY = mpu.getAccY();
float aZ = mpu.getAccZ();
float gX = mpu.getGyroX();
float gY = mpu.getGyroY();
float gZ = mpu.getGyroZ();
float mX = mpu.getMagX();
float mY = mpu.getMagY();
float mZ = mpu.getMagZ();
//roll calc
float roll = atan2(aY, sqrt(aX * aX + aZ * aZ)) * (180 / Pi);
//pitch calc
float pitch = atan2(-aX, sqrt(aY * aY + aZ * aZ)) * (180 / Pi);
// Magnetic-X and Magnetic-Y calc
float Mx = (mX * cos(pitch)) + (mY * sin(roll) * sin(pitch)) - (mZ * cos(roll) * sin(pitch));
float My = (mY * cos(roll)) - (mZ * sin(roll));
// yaw calc
float yaw = atan2(My, Mx) * (180 / Pi);
//time
currentTime = millis();
elapsedTime = (currentTime-previousTime)/1000;
previousTime = currentTime;
//complimentary
cRoll = ((0.98 * (cRoll + (gX * elapsedTime))) + (0.02 * roll));
cPitch = ((0.98 * (cPitch + (gY * elapsedTime))) + (0.02 * pitch));
cYaw = ((0.98 * (cYaw + (gZ * elapsedTime))) + (0.02 * yaw));
//output
Serial.print(" ");
Serial.print(roll);
Serial.print(", ");
Serial.print(cRoll);
Serial.print(", ");
Serial.print(pitch);
Serial.print(", ");
Serial.print(cPitch);
Serial.print(", ");
Serial.print(yaw);
Serial.print(", ");
Serial.print(cYaw);
Serial.print(", ");
Serial.print(mpu.getYaw());
Serial.println(", ");
delay(5);
}
}
Output sample: (Roll/cRoll/Pitch/cPitch/Yaw/cYaw)
