Judging by google this is a common question as soon as people figure out that gyro boards don't actually output gyro angles. So is there a library that handles all that and outputs usable orientation angles?
Not a tutorial or a youtube video, a library that i can use immediately.
The "library" is called AHRS or sensor fusion, and there are several. You need to calibrate the gyro to remove the stationary offsets, which are different for every sensor. For best results calibrate the accelerometer, but that is optional.
The MPU-6050, which is long obsolete, has no magnetometer for a North reference, so the yaw angle is relative to startup and will drift with time.
One possibility: GitHub - jremington/MPU-6050-Fusion: Mahony 3D filter for accel/gyro and gyro-only integration
Yeah, that's one i was trying, but even then i'm getting unusably rapid (multiple degrees per second) angle drift. I'm using the MPU6500 from a (fake) GY-91 IMU with a seperate QMC5883L for magnetometer, and am using fastIMU instead of the original MPU6050 functions.
#include "SensorFusion.h"
#include <Wire.h>
#include "FastIMU.h"
SF fusion;
float gx, gy, gz, ax, ay, az, mx, my, mz, temp;
float pitch, roll, yaw;
float deltat;
#define IMU_ADDRESS 0x68 //Change to the address of the IMU
#define PERFORM_CALIBRATION //Comment to disable startup calibration
//#define PRELOAD_CALIBRATION //Either or above
MPU6500_QMC5883L IMU; //Change to the name of any supported IMU!
calData calib = { 0 }; //Calibration data
AccelData accelData; //Sensor data
GyroData gyroData;
MagData magData;
#define EULER_DATA
//#define RAW_DATA
//#define PROCESSING
//#define SERIAL_PLOTER
void setup() {
Wire.begin();
Wire.setClock(400000); //400khz clock
Serial.begin(115200);
while (!Serial) {
;
}
int err = IMU.init(calib, IMU_ADDRESS);
if (err != 0) {
Serial.print("Error initializing IMU: ");
Serial.println(err);
while (true) {
;
}
}
#ifdef PERFORM_CALIBRATION
Serial.println("FastIMU calibration & data example");
if (IMU.hasMagnetometer()) {
delay(1000);
Serial.println("Move IMU in figure 8 pattern until done.");
delay(3000);
IMU.calibrateMag(&calib);
Serial.println("Magnetic calibration done!");
}
else {
delay(5000);
}
delay(5000);
Serial.println("Keep IMU level.");
delay(5000);
IMU.calibrateAccelGyro(&calib);
Serial.println("Calibration done!");
Serial.println("Accel biases X/Y/Z: ");
Serial.print(calib.accelBias[0]);
Serial.print(", ");
Serial.print(calib.accelBias[1]);
Serial.print(", ");
Serial.println(calib.accelBias[2]);
Serial.println("Gyro biases X/Y/Z: ");
Serial.print(calib.gyroBias[0]);
Serial.print(", ");
Serial.print(calib.gyroBias[1]);
Serial.print(", ");
Serial.println(calib.gyroBias[2]);
if (IMU.hasMagnetometer()) {
Serial.println("Mag biases X/Y/Z: ");
Serial.print(calib.magBias[0]);
Serial.print(", ");
Serial.print(calib.magBias[1]);
Serial.print(", ");
Serial.println(calib.magBias[2]);
Serial.println("Mag Scale X/Y/Z: ");
Serial.print(calib.magScale[0]);
Serial.print(", ");
Serial.print(calib.magScale[1]);
Serial.print(", ");
Serial.println(calib.magScale[2]);
}
delay(5000);
IMU.init(calib, IMU_ADDRESS);
#endif
#ifdef PRELOAD_CALIBRATION
calData preload_calib;
preload_calib.valid = true;
preload_calib.accelBias[0] = 0.05;
preload_calib.accelBias[1] = 0.00;
preload_calib.accelBias[2] = -0.03;
preload_calib.gyroBias[0] = 0.45;
preload_calib.gyroBias[1] = 0.76;
preload_calib.gyroBias[2] = 0.11;
preload_calib.magBias[0] = -0.08;
preload_calib.magBias[1] = -0.11;
preload_calib.magBias[2] = -0.32;
preload_calib.magScale[0] = 0.99;
preload_calib.magScale[1] = 0.70;
preload_calib.magScale[2] = 1.79;
IMU.init(preload_calib, IMU_ADDRESS);
#endif
//err = IMU.setGyroRange(500); //USE THESE TO SET THE RANGE, IF AN INVALID RANGE IS SET IT WILL RETURN -1
//err = IMU.setAccelRange(2); //THESE TWO SET THE GYRO RANGE TO ±500 DPS AND THE ACCELEROMETER RANGE TO ±2g
if (err != 0) {
Serial.print("Error Setting range: ");
Serial.println(err);
while (true) {
;
}
}
}
void loop() {
IMU.update();
IMU.getAccel(&accelData);
ax = accelData.accelX;
ay = accelData.accelY;
az = accelData.accelZ;
IMU.getGyro(&gyroData);
gx = gyroData.gyroX;
gy = gyroData.gyroY;
gz = gyroData.gyroZ;
IMU.getMag(&magData);
mx = magData.magX;
my = magData.magY;
mz = magData.magZ;
temp = IMU.getTemp();
#ifdef RAW_DATA
Serial << "From last Update:\t"; Serial.println(deltat, 6);
Serial << "GYRO:\tx:" << gx << "\t\ty:" << gy << "\t\tz:" << gz << newl;
Serial << "ACC:\tx:" << ax << "\t\ty:" << ay << "\t\tz:" << az << newl;
Serial << "MAG:\tx:" << mx << "\t\ty:" << my << "\t\tz:" << mz << newl;
Serial << "TEMP:\t" << temp << newl << newl;
#endif
deltat = fusion.deltatUpdate();
//fusion.MahonyUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat); //mahony is suggested if there isn't the mag
fusion.MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat); //else use the magwick
roll = fusion.getRoll();
pitch = fusion.getPitch();
yaw = fusion.getYaw();
#ifdef EULER_DATA
//Serial << "Pitch:\t" << pitch << "\t\tRoll:\t" << roll << "\t\tYaw:\t" << yaw << newl << newl;
Serial.print("Pitch:\t");
Serial.print(pitch);
Serial.print("\tYaw:\t");
Serial.print(yaw);
Serial.print("\tRoll:\t");
Serial.println(roll);
#endif
#ifdef PROCESSING
roll = fusion.getRollRadians();
pitch = fusion.getPitchRadians();
yaw = fusion.getYawRadians();
Serial.print("Pitch:\t");
Serial.print(pitch);
Serial.print("\tYaw:\t");
Serial.print(yaw);
Serial.print("\tRoll:\t");
Serial.println(roll);
#endif
delay(50); //for readability
}
Surely there has to be a plug and play solution to just get simple gyro angles without having to do an entire course. Does nobody use these thing?
There is nothing "simple" about it. The code in post #3 is a waste of your time.
Yes, i'm finding that out the hard way... This is what i've found somewhat works, the calibrated quaternion example from fastIMU.
#include "FastIMU.h"
#include "Madgwick.h"
#include <Wire.h>
#include <math.h>
// Function to convert quaternion to Euler angles
// yaw and roll switched??
void quaternionToEuler(float q0, float q1, float q2, float q3, float& pitch, float& yaw, float& roll) {
// Pitch (x-axis rotation)
float sinPitchCosYaw = 2.0 * (q0 * q1 + q2 * q3);
float cosPitchCosYaw = 1.0 - 2.0 * (q1 * q1 + q2 * q2);
pitch = atan2(sinPitchCosYaw, cosPitchCosYaw);
// Yaw (y-axis rotation)
float sinYaw = 2.0 * (q0 * q3 - q1 * q2); // Switched q2 and q3
yaw = asin(sinYaw) - PI/2.0;
// Roll (z-axis rotation)
float sinRollCosYaw = 2.0 * (q0 * q2 + q1 * q3); // Switched q2 and q3
float cosRollCosYaw = 1.0 - 2.0 * (q2 * q2 + q3 * q3);
roll = atan2(sinRollCosYaw, cosRollCosYaw);
}
#define IMU_ADDRESS 0x68 //Change to the address of the IMU
//#define PERFORM_CALIBRATION //Comment to disable startup calibration
#define PRELOAD_CALIBRATION //Either or above
MPU6500_QMC5883L IMU; //Change to the name of any supported IMU!
calData calib = { 0 };
static float magDeclination = -13.8;
AccelData IMUAccel;
GyroData IMUGyro;
MagData IMUMag;
Madgwick filter;
void setup() {
Wire.begin();
Wire.setClock(400000); //400khz clock
IMU.setIMUGeometry(0);
Serial.begin(115200);
while (!Serial) {
;
}
int err = IMU.init(calib, IMU_ADDRESS);
if (err != 0) {
Serial.print("Error initializing IMU: ");
Serial.println(err);
while (true) {
;
}
}
if (err != 0) {
Serial.print("Error Setting range: ");
Serial.println(err);
while (true) {
;
}
}
#ifdef PERFORM_CALIBRATION
Serial.println("FastIMU Calibrated Quaternion example");
if (IMU.hasMagnetometer()) {
delay(1000);
Serial.println("Move IMU in figure 8 pattern until done.");
delay(3000);
IMU.calibrateMag(&calib);
Serial.println("Magnetic calibration done!");
}
else {
delay(1000);
}
Serial.println("Keep IMU level.");
delay(5000);
IMU.calibrateAccelGyro(&calib);
Serial.println("Calibration done!");
Serial.println("Accel biases X/Y/Z: ");
Serial.print(calib.accelBias[0]);
Serial.print(", ");
Serial.print(calib.accelBias[1]);
Serial.print(", ");
Serial.println(calib.accelBias[2]);
Serial.println("Gyro biases X/Y/Z: ");
Serial.print(calib.gyroBias[0]);
Serial.print(", ");
Serial.print(calib.gyroBias[1]);
Serial.print(", ");
Serial.println(calib.gyroBias[2]);
if (IMU.hasMagnetometer()) {
Serial.println("Mag biases X/Y/Z: ");
Serial.print(calib.magBias[0]);
Serial.print(", ");
Serial.print(calib.magBias[1]);
Serial.print(", ");
Serial.println(calib.magBias[2]);
Serial.println("Mag Scale X/Y/Z: ");
Serial.print(calib.magScale[0]);
Serial.print(", ");
Serial.print(calib.magScale[1]);
Serial.print(", ");
Serial.println(calib.magScale[2]);
}
delay(5000);
IMU.init(calib, IMU_ADDRESS);
filter.begin(0.2f);
#endif
#ifdef PRELOAD_CALIBRATION
calData preload_calib;
preload_calib.valid = true;
preload_calib.accelBias[0] = 0.025;
preload_calib.accelBias[1] = 0.00;
preload_calib.accelBias[2] = -0.03;
preload_calib.gyroBias[0] = 0.45;
preload_calib.gyroBias[1] = 0.8;
preload_calib.gyroBias[2] = 0.1;
preload_calib.magBias[0] = -0.1;
preload_calib.magBias[1] = -0.15;
preload_calib.magBias[2] = -0.35;
preload_calib.magScale[0] = 1.0;
preload_calib.magScale[1] = 0.75;
preload_calib.magScale[2] = 1.8;
IMU.init(preload_calib, IMU_ADDRESS);
#endif
}
void loop() {
IMU.update();
IMU.getAccel(&IMUAccel);
IMU.getGyro(&IMUGyro);
if (IMU.hasMagnetometer()) {
IMU.getMag(&IMUMag);
filter.update(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ, IMUMag.magX, IMUMag.magY, IMUMag.magZ);
}
else {
filter.updateIMU(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ);
}
float pitch, yaw, roll;
quaternionToEuler(filter.getQuatW(), filter.getQuatX(), filter.getQuatY(), filter.getQuatZ(), pitch, yaw, roll);
pitch = degrees(pitch);
yaw = degrees(yaw) - magDeclination;
roll = degrees(roll);
Serial.print("Pitch:\t"); Serial.println(pitch);
Serial.print("Roll:\t"); Serial.println(roll);
Serial.print("Yaw:\t"); Serial.println(yaw);
Serial.println();
delay(50);
}
It seems to work relatively well, apart from the strange yaw/roll inversion error, to my untrained and completely ignorant eye.
Which brings me to this question... What do people use? Surely people don't reinvent the wheel and write their own code every time they have to use an IMU?
If you choose random code from the internet, you are very likely to be disappointed.
A very large fraction is posted by people who have no idea what they are doing, but manage to get something running that they think works, then show it off to the world. A good grounding in mathematics is required to fully understand 3D orientation calculations.
If you have a particular application in mind, it is a good idea to do some basic research and improve your understanding of the topic background before downloading and running code.
Yes, that is a problem... i'm trying to avoid random google tutorial code and sticking to the examples given in reference libraries. I've never handled quaternions before. I'd be glad to leave that work to someone with the proper mathematics background but you can't always get away with using stuff as black boxes that just merrily output the data you need.. I just noticed that you are the one who made that MPU-6050-Fusion code. I happened to order a few of those too and i tested them, with the same weird problems.
...I take it that what i should do is put those chips on the table, smash them to bits with a hammer, and buy a real 9dof IMU that does the fusion itself and doesn't take me doing a master's degree of engineering to get angles?
The only 9DOF chips with documented fusion algorithms built in, and are reasonably easy to use are the BNO055, BNO080, and BNO085, available from Adafruit and/or Sparkfun. The BNO055 is for navigation, while the other two are more for gaming headsets.
Because the built-in sensor calibration does not work well, the 3D orientation angles are not very accurate. But they are more or less "plug and play", suitable for someone who is not interested in learning about the details.
Yeah... I will look into them, but the low, low cost of those iMU's made them very interesting. If even the true Adafruit IMU's aren't very accurate, that is a problem. I am working on an autonomous plane so accurate angles are kind of a must have, the workload of all the systems i have to integrate made me hope to find a plug and play solution. I guess there's no way around having to implement it myself?
Thinking about it, since those 6050 IMUs are so cheap, has it been done to use three or more of them with a Kalman filter to average out to a more accurate reading?
This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.