6-Axis IMU improvement

Using Arduino Sensors
1

Hi!

Recently I've been working with MPU6050 and Madgwick filter. Right now my results are quite alright. I'm reading accelerometer and gyroscope data and then this library GitHub - arduino-libraries/MadgwickAHRS: Arduino implementation of the MadgwickAHRS algorithm gives me quaternion which I made public so I can use it later. Then I visualize orientation using pyOpenGL. I want to improve this project by adding magnetometer but when I used Adafruit HMC5883L library there was a big delay and yaw slowly snaps into specific direction. Can you help me improve my code in any way?

#include "Wire.h"
#include <MPU6050_light.h>
#include <MadgwickAHRS.h>

MPU6050 mpu(Wire);
Madgwick filter;
unsigned long microsPerReading, microsPrevious;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  byte status = mpu.begin();
  filter.begin(25);

  Serial.print(F("MPU6050 status: "));
  Serial.println(status);
  while(status!=0){ }
  delay(1000);
  mpu.calcOffsets(true,true); 

  microsPerReading = 1000000 / 25;
  microsPrevious = micros();
}

void loop() {
  float ax, ay, az;
  float gx, gy, gz;
  unsigned long microsNow;
 
  microsNow = micros();
  if (microsNow - microsPrevious >= microsPerReading) {

    mpu.update();
    ax = mpu.getAccX();
    ay = mpu.getAccY();
    az = mpu.getAccZ();
    gx = mpu.getGyroX();
    gy = mpu.getGyroY();
    gz = mpu.getGyroZ();

    filter.updateIMU(gx, gy, gz, ax, ay, az);

    Serial.print(filter.q0);
    Serial.print(" ");
    Serial.print(filter.q1);
    Serial.print(" ");
    Serial.print(filter.q2);
    Serial.print(" ");
    Serial.println(filter.q3);

    microsPrevious = microsPrevious + microsPerReading;
  }
  
}


import pygame
from pygame.locals import *
import serial
from OpenGL.GL import *
from OpenGL.GLU import *


class Cube():
    def __init__(self):
        self.verticies = (
            (3, -1, -4),
            (3, 1, -4),
            (-3, 1, -4),
            (-3, -1, -4),
            (3, -1, 4),
            (3, 1, 4),
            (-3, -1, 4),
            (-3, 1, 4)
        )

        self.edges = (
            (0, 1),
            (0, 3),
            (0, 4),
            (2, 1),
            (2, 3),
            (2, 7),
            (6, 3),
            (6, 4),
            (6, 7),
            (5, 1),
            (5, 4),
            (5, 7)
        )

    def update(self, w, x, y, z):

        glPushMatrix()
        glRotatef(w*360, y, z, x)
        glBegin(GL_LINES)
        for edge in self.edges:
            for vertex in edge:
                glVertex3fv(self.verticies[vertex])
        glEnd()
        glPopMatrix()

def main():
    ser = serial.Serial('COM5', 9600)
    pygame.init()
    display = (800, 600)
    pygame.display.set_mode(display, DOUBLEBUF|OPENGL)

    gluPerspective(45, (display[0]/display[1]), 0.1, 50.0)

    glTranslatef(0.0, 0.0, -20)
    cube = Cube()
    while True:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                pygame.quit()
                quit()
        glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT)
        data = ser.readline().decode('UTF-8')
        data = data.split(' ')
        print(data)
        try:
            cube.update(float(data[0]), float(data[1]), float(data[2]), float(data[3]))
        except:
            pass
        pygame.display.flip()


main()
2

The magnetometer axes and the accelerometer axes have to be both right handed and coincide in spatial directions, or the Madgwick filter won't work.

How did you handle that requirement?

3

I've aligned x and y axis printed on modules. I don't really know what "right handed axes" mean but I assume that MPU is installed correctly since it gives pretty good results. When I get home I'll check again axes while rotating to see how values change and post here my code with magnetometer.

4

Here is my code using magnetometer with this library GitHub - jarzebski/Arduino-HMC5883L: HMC5883L Triple Axis Digital Compass Arduino Library

#include "Wire.h"
#include <MPU6050_light.h>
#include <HMC5883L.h>
#include <MadgwickAHRS.h>

MPU6050 mpu(Wire);
HMC5883L compass;
Madgwick filter;
unsigned long microsPerReading, microsPrevious;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  byte status = mpu.begin();
  filter.begin(25);

  Serial.print(F("MPU6050 status: "));
  Serial.println(status);
  while(status!=0 || !compass.begin()){ }

  compass.setRange(HMC5883L_RANGE_4GA);
  compass.setMeasurementMode(HMC5883L_CONTINOUS);
  compass.setDataRate(HMC5883L_DATARATE_30HZ);
  compass.setSamples(HMC5883L_SAMPLES_1);

  delay(1000);
  mpu.calcOffsets(true,true); 

  microsPerReading = 1000000 / 25;
  microsPrevious = micros();
}

void loop() {
  float ax, ay, az;
  float gx, gy, gz;
  float mx, my, mz;
  unsigned long microsNow;
 
  microsNow = micros();
  if (microsNow - microsPrevious >= microsPerReading) {

    mpu.update();
    Vector norm = compass.readNormalize();

    ax = mpu.getAccX();
    ay = mpu.getAccY();
    az = mpu.getAccZ();
    gx = mpu.getGyroX();
    gy = mpu.getGyroY();
    gz = mpu.getGyroZ();
    mx = norm.XAxis;
    my = norm.YAxis;
    mz = norm.ZAxis;

    filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);

    Serial.print(filter.q0);
    Serial.print(" ");
    Serial.print(filter.q1);
    Serial.print(" ");
    Serial.print(filter.q2);
    Serial.print(" ");
    Serial.println(filter.q3);

    microsPrevious = microsPrevious + microsPerReading;
  }
  
}

This is my output while not moving. As you can see it slowly snaps into specific direction.

0.92 0.02 -0.05 0.38
0.92 0.02 -0.05 0.39
0.92 0.02 -0.05 0.39
0.92 0.02 -0.05 0.39
0.92 0.02 -0.05 0.39
0.92 0.02 -0.05 0.40
0.91 0.02 -0.05 0.40
0.91 0.02 -0.05 0.40
0.91 0.02 -0.05 0.41
0.91 0.02 -0.05 0.41
0.91 0.02 -0.05 0.41
0.91 0.02 -0.05 0.41
0.91 0.02 -0.05 0.42
0.91 0.02 -0.05 0.42
0.91 0.02 -0.05 0.42
...
0.22 0.00 0.00 0.97
5

It looks like you have not calibrated the magnetometer. They don't work out of the box for directional purposes, and for best results, should be calibrated in the final installation.

See Tutorial: How to calibrate a compass (and accelerometer) with Arduino | Underwater Arduino Data Loggers

Practical example: https://forum.pololu.com/t/correcting-the-balboa-magnetometer/14315

1 Like
6

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.