Hello, everyone.
First of all, thank you for visiting my post.
I'm currently working on a project that involves using the MPU9250 sensor to measure object orientation.
However, measured yaw angle gradually decreases over time, even when the sensor is stationary.
I would be thankful if I someone can help me troubleshoot this problem.
Here, are some details about my setup:
- I'm using Arduino Uno board
- I'm using Arduino official MadgwickAHRS library (GitHub - arduino-libraries/MadgwickAHRS: Arduino implementation of the MadgwickAHRS algorithm)
My code is as following.
#include <MadgwickAHRS.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU9250.h"
MPU9250 accelgyro;
I2Cdev I2C_M;
Madgwick filter;
uint8_t buffer_m[6];
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t mx, my, mz;
float heading;
float tilt_heading;
float A_xyz[3];
float G_xyz[3];
float M_xyz[3];
#define sample_num_mdate 5000
volatile float mx_sample[3];
volatile float my_sample[3];
volatile float mz_sample[3];
static float mx_centre = 0;
static float my_centre = 0;
static float mz_centre = 0;
volatile int mx_max = 0;
volatile int my_max = 0;
volatile int mz_max = 0;
volatile int mx_min = 0;
volatile int my_min = 0;
volatile int mz_min = 0;
float temperature;
float pressure;
float atm;
float altitude;
int sample_rate;
unsigned long micros_per_reading, micros_previous;
float roll, pitch, yaw;
void setup() {
// put your setup code here, to run once:
Wire.begin();
Serial.begin(38400);
accelgyro.initialize();
sample_rate = 50;
filter.begin(sample_rate);
micros_per_reading = 1000000 / sample_rate;
micros_previous = micros();
delay(1000);
}
void loop() {
// put your main code here, to run repeatedly:
get_accel_data();
get_gyro_data();
get_compass_date_calibrated();
get_heading();
get_tilt_heading();
unsigned long micros_now;
// check if it's time to read data and update the filter
micros_now = micros();
if (micros_now - micros_previous >= micros_per_reading) {
// update the filter, which computes orientation
filter.updateIMU(G_xyz[0], G_xyz[1], G_xyz[2], A_xyz[0], A_xyz[1], A_xyz[2]);
// print the heading, pitch and roll
roll = filter.getRoll();
pitch = filter.getPitch();
heading = filter.getYaw();
Serial.print("Orientation: ");
Serial.print(heading);
Serial.print(" ");
Serial.print(pitch);
Serial.print(" ");
Serial.println(roll);
// increment previous time, so we keep proper pace
micros_previous = micros_previous + micros_per_reading;
}
Serial.flush();
Serial.print(A_xyz[0]);
Serial.print(",");
Serial.print(A_xyz[1]);
Serial.print(",");
Serial.print(A_xyz[2]);
Serial.print(",");
Serial.print(G_xyz[0]);
Serial.print(",");
Serial.print(G_xyz[1]);
Serial.print(",");
Serial.print(G_xyz[2]);
Serial.print(",");
Serial.print(M_xyz[0]);
Serial.print(",");
Serial.print(M_xyz[1]);
Serial.print(",");
Serial.print(M_xyz[2]);
Serial.print(",");
Serial.print(heading);
Serial.print(",");
Serial.print(tilt_heading);
Serial.println();
}
void get_heading(void)
{
heading = 180 * atan2(M_xyz[1], M_xyz[0]) / PI;
if (heading < 0) heading += 360;
}
void get_tilt_heading(void)
{
float pitch = asin(-A_xyz[0]);
float roll = asin(A_xyz[1] / cos(pitch));
float xh = M_xyz[0] * cos(pitch) + M_xyz[2] * sin(pitch);
float yh = M_xyz[0] * sin(roll) * sin(pitch) + M_xyz[1] * cos(roll) - M_xyz[2] * sin(roll) * cos(pitch);
float zh = -M_xyz[0] * cos(roll) * sin(pitch) + M_xyz[1] * sin(roll) + M_xyz[2] * cos(roll) * cos(pitch);
tilt_heading = 180 * atan2(yh, xh) / PI;
if (yh < 0) tilt_heading += 360;
}
void Mxyz_init_calibrated ()
{
Serial.println(F("Before using 9DOF,we need to calibrate the compass frist,It will takes about 2 minutes."));
Serial.print(" ");
Serial.println(F("During calibratting ,you should rotate and turn the 9DOF all the time within 2 minutes."));
Serial.print(" ");
Serial.println(F("If you are ready ,please sent a command data 'ready' to start sample and calibrate."));
while (!Serial.find("ready"));
Serial.println(" ");
Serial.println("ready");
Serial.println("Sample starting......");
Serial.println("waiting ......");
get_calibration_Data ();
Serial.println(" ");
Serial.println("compass calibration parameter ");
Serial.print(mx_centre);
Serial.print(" ");
Serial.print(my_centre);
Serial.print(" ");
Serial.println(mz_centre);
Serial.println(" ");
}
void get_calibration_Data ()
{
for (int i = 0; i < sample_num_mdate; i++)
{
get_one_sample_date_mxyz();
if (mx_sample[2] >= mx_sample[1])mx_sample[1] = mx_sample[2];
if (my_sample[2] >= my_sample[1])my_sample[1] = my_sample[2]; //find max value
if (mz_sample[2] >= mz_sample[1])mz_sample[1] = mz_sample[2];
if (mx_sample[2] <= mx_sample[0])mx_sample[0] = mx_sample[2];
if (my_sample[2] <= my_sample[0])my_sample[0] = my_sample[2]; //find min value
if (mz_sample[2] <= mz_sample[0])mz_sample[0] = mz_sample[2];
}
mx_max = mx_sample[1];
my_max = my_sample[1];
mz_max = mz_sample[1];
mx_min = mx_sample[0];
my_min = my_sample[0];
mz_min = mz_sample[0];
mx_centre = (mx_max + mx_min) / 2;
my_centre = (my_max + my_min) / 2;
mz_centre = (mz_max + mz_min) / 2;
}
void get_one_sample_date_mxyz()
{
get_compass_data();
mx_sample[2] = M_xyz[0];
my_sample[2] = M_xyz[1];
mz_sample[2] = M_xyz[2];
}
void get_accel_data(void)
{
accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
A_xyz[0] = (double) ax / 16384;
A_xyz[1] = (double) ay / 16384;
A_xyz[2] = (double) az / 16384;
}
void get_gyro_data(void)
{
accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
G_xyz[0] = (double) gx * 250 / 32768;
G_xyz[1] = (double) gy * 250 / 32768;
G_xyz[2] = (double) gz * 250 / 32768;
}
void get_compass_data(void)
{
I2C_M.writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x01); //enable the magnetometer
delay(10);
I2C_M.readBytes(MPU9150_RA_MAG_ADDRESS, MPU9150_RA_MAG_XOUT_L, 6, buffer_m);
mx = ((int16_t)(buffer_m[1]) << 8) | buffer_m[0] ;
my = ((int16_t)(buffer_m[3]) << 8) | buffer_m[2] ;
mz = ((int16_t)(buffer_m[5]) << 8) | buffer_m[4] ;
M_xyz[0] = (double) mx * 1200 / 4096;
M_xyz[1] = (double) my * 1200 / 4096;
M_xyz[2] = (double) mz * 1200 / 4096;
}
void get_compass_date_calibrated ()
{
get_compass_data();
M_xyz[0] = M_xyz[0] - mx_centre;
M_xyz[1] = M_xyz[1] - my_centre;
M_xyz[2] = M_xyz[2] - mz_centre;
}
Serial ouput's format is as following.:
Orientation: [Yaw] [Pitch] [Roll]
acc_x, acc_y, acc_z, g_x, g_y, g_z, mag_x, mag_y, mag_z, heading, tilt_heading
And serial output is as following:
Orientation: 179.95 0.49 0.87
-0.04,0.04,1.00,6.71,-0.11,-1.01,14.06,-13.77,-22.56,179.95,315.68
Orientation: 179.93 0.66 1.17
-0.03,0.04,0.99,6.96,-0.14,-0.81,13.48,-14.65,-23.44,179.93,312.81
Orientation: 179.92 0.85 1.43
-0.03,0.03,0.99,7.01,-0.30,-0.80,13.77,-13.48,-23.73,179.92,315.69
Orientation: 179.91 1.03 1.71
-0.03,0.04,0.99,7.00,-0.24,-0.72,13.77,-14.06,-23.14,179.91,314.70
Orientation: 179.89 1.25 1.88
-0.03,0.03,0.99,6.84,-0.02,-1.00,14.06,-14.36,-22.56,179.89,314.34
Orientation: 179.87 1.47 2.09
-0.03,0.04,0.99,6.97,-0.11,-0.81,13.77,-13.18,-23.44,179.87,316.55
Orientation: 179.86 1.69 2.26
-0.03,0.04,0.99,6.87,-0.14,-0.86,14.06,-13.77,-23.73,179.86,315.91
Orientation: 179.84 1.86 2.25
-0.03,0.03,0.98,6.65,-0.17,-0.83,13.77,-14.36,-22.85,179.84,313.83
Orientation: 179.81 1.85 2.15
-0.03,0.03,0.99,6.87,-0.42,-0.83,13.77,-14.36,-23.73,179.81,313.88
Orientation: 179.79 1.62 2.27
-0.03,0.04,0.99,7.16,-0.31,-1.06,14.65,-14.36,-22.85,179.79,316.00
Orientation: 179.77 1.79 2.27
-0.03,0.03,0.99,7.12,-0.33,-0.94,14.06,-13.48,-23.14,179.77,316.34
Orientation: 179.74 1.97 2.27
-0.04,0.04,0.99,7.06,-0.12,-0.98,13.77,-14.06,-23.14,179.74,314.37
Orientation: 179.72 1.83 2.23
-0.03,0.03,0.99,6.97,-0.08,-0.78,14.06,-14.36,-23.14,179.72,314.60
Orientation: 179.70 1.84 2.13
-0.03,0.04,0.99,6.78,-0.11,-0.85,14.06,-14.36,-22.85,179.70,314.64
Orientation: 179.68 2.06 2.33
-0.03,0.04,0.99,6.68,0.02,-1.00,13.48,-14.65,-23.14,179.68,312.66
Orientation: 179.66 2.14 2.26
-0.04,0.04,0.99,6.95,-0.01,-0.74,13.77,-14.94,-22.85,179.66,312.56
Orientation: 179.63 1.99 2.22
-0.03,0.04,0.99,6.68,-0.19,-0.82,14.06,-13.77,-23.44,179.63,315.76
Orientation: 179.61 1.87 2.16
-0.03,0.04,0.99,6.93,-0.13,-0.91,13.77,-14.06,-23.14,179.61,314.55
Orientation: 179.59 2.09 2.25
-0.04,0.04,0.99,6.97,-0.14,-0.88,13.48,-14.06,-23.44,179.59,313.69
Orientation: 179.57 1.97 2.18
-0.03,0.04,0.99,6.72,-0.18,-0.88,13.48,-14.36,-22.85,179.57,313.18
Orientation: 179.56 2.07 2.53
-0.03,0.04,0.99,6.81,0.08,-0.86,14.06,-14.06,-22.85,179.56,315.26
Orientation: 179.53 2.07 2.44
-0.04,0.04,0.99,6.94,-0.37,-0.90,14.06,-14.36,-22.56,179.53,314.41
Orientation: 179.51 1.98 2.36
-0.03,0.03,0.99,6.83,0.11,-0.70,13.77,-14.65,-22.85,179.51,313.23
Orientation: 179.48 1.84 2.32
-0.03,0.04,0.99,6.83,-0.23,-0.84,13.18,-14.36,-23.14,179.48,312.74
Orientation: 179.46 1.92 2.24
-0.03,0.03,0.99,6.80,-0.40,-0.74,13.77,-14.06,-22.85,179.46,314.30
Orientation: 179.44 2.12 2.26
-0.04,0.04,0.99,6.86,-0.10,-1.09,12.89,-13.77,-23.73,179.44,312.84
Orientation: 179.41 2.17 2.18
-0.04,0.04,0.99,6.87,-0.27,-0.85,14.06,-14.06,-23.14,179.41,315.02
Orientation: 179.39 2.02 2.15
-0.03,0.03,0.98,7.11,-0.12,-0.85,13.77,-13.77,-22.56,179.39,315.01
Orientation: 179.37 1.85 2.13
-0.03,0.03,0.99,6.89,-0.19,-0.69,13.77,-14.06,-23.14,179.37,314.52
Orientation: 179.34 1.95 2.06
-0.03,0.04,0.99,6.77,-0.21,-1.17,14.65,-14.65,-23.14,179.34,315.17
Orientation: 179.31 2.02 1.98
-0.03,0.03,0.99,6.86,0.02,-0.82,13.77,-13.77,-23.14,179.31,314.93
Orientation: 179.29 1.84 1.97
-0.03,0.03,0.99,6.68,-0.14,-0.85,13.77,-14.06,-22.85,179.29,314.37
Orientation: 179.28 2.03 2.23
-0.03,0.04,0.99,6.63,-0.24,-0.85,14.06,-14.06,-23.14,179.28,315.10
Orientation: 179.25 2.05 2.13
-0.04,0.04,0.99,6.62,-0.06,-0.85,13.48,-13.77,-23.44,179.25,314.42
Orientation: 179.09 1.76 2.23
-0.03,0.04,0.99,6.73,-0.12,-0.92,13.18,-14.65,-22.85,179.09,312.19
Orientation: 179.06 1.86 2.15
-0.03,0.04,0.99,6.58,0.11,-0.91,14.06,-14.36,-22.85,179.06,314.63
Orientation: 179.04 1.86 2.07
-0.03,0.03,0.99,6.99,-0.10,-0.84,13.48,-13.77,-23.44,179.04,314.34
Orientation: 179.02 1.65 2.10
-0.03,0.03,0.99,7.08,-0.31,-0.81,13.48,-14.06,-23.14,179.02,314.02
Orientation: 179.00 1.82 2.09
-0.03,0.03,0.98,6.95,-0.25,-0.92,13.77,-14.06,-23.73,179.00,314.28
Orientation: 178.98 2.02 2.33
-0.03,0.04,0.99,6.85,-0.21,-1.05,13.77,-13.77,-23.73,178.98,315.24
Orientation: 178.95 2.11 2.27
-0.04,0.04,0.99,7.05,-0.11,-1.13,13.48,-14.06,-23.14,178.95,313.70
Orientation: 178.93 1.88 2.38
-0.03,0.04,0.99,7.03,-0.20,-0.92,14.06,-14.06,-23.14,178.93,315.39
Orientation: 178.90 1.88 2.29
-0.03,0.03,0.99,7.14,-0.12,-0.91,13.77,-14.65,-23.14,178.90,313.06
Orientation: 178.88 1.80 2.21
-0.03,0.04,0.99,6.78,-0.27,-0.95,13.77,-14.36,-22.85,178.88,314.02
Orientation: 178.86 1.69 2.15
-0.03,0.04,0.99,6.98,-0.29,-0.78,13.18,-14.06,-23.44,178.86,313.38
Orientation: 178.84 1.87 2.42
-0.03,0.04,0.99,7.07,-0.34,-0.80,13.48,-14.36,-22.85,178.84,313.44
Orientation: 178.82 1.81 2.34
-0.03,0.04,0.99,7.00,-0.25,-0.92,13.77,-14.06,-22.56,178.82,314.72
Orientation: 178.79 1.95 2.29
-0.03,0.04,0.99,6.83,-0.02,-0.92,14.06,-14.36,-23.14,178.79,314.47
Orientation: 178.77 1.89 2.21
-0.03,0.03,0.99,6.89,0.06,-1.03,13.77,-14.36,-23.44,178.77,313.84
Orientation: 178.74 1.92 2.12
-0.03,0.03,0.99,6.99,-0.37,-0.85,14.36,-14.06,-22.85,178.74,315.61
Orientation: 178.71 1.91 2.03
-0.03,0.03,0.99,7.01,0.07,-0.97,13.48,-14.06,-23.73,178.71,313.78
Orientation: 178.69 1.90 1.94
-0.03,0.03,0.99,6.90,-0.14,-1.02,13.48,-14.65,-22.85,178.69,312.55
Thank you for reading.