MPU-6050, calculating Yaw, Pitch, and Roll

Projects General Guidance
1

Hi all -

I know this is a complex subject and there is A LOT of discussion about the best practices. At the moment, I have an MPU-6050 hooked up to my device (I'm using an ESP-32, but I think those details might be irrelevant here), it is sending messages over OSC - but I would just like to convert the gyroscope readings (which I believe are measuring angular velocity?) to pitch, yaw, and roll - with a modicum of accuracy.
I know that there are very complex filters and paradigms for how this information can be processed - I know there are way better sensors available now - but I'd just like to use what I have to do something very basic, as a way of starting this learning process.

The code I cobbled together is below - if you can help, I would be greatly appreciative.


#include <WiFi.h>
#include <OSCMessage.h>
#include <WiFiUdp.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

Adafruit_MPU6050 mpu;
//network credentials
const char* ssid = "mynetwork";
const char* password = "mypassword";
WiFiUDP Udp;
const char *ip = "192.168.254.200"; // IP of the computer where you want to send the OSC message
const int port = 12345;         // Port where the computer is listening for OSC messages


void setup(void) {
  Serial.begin(115200);
  WiFi.begin(ssid, password);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit MPU6050 test!");

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
  case MPU6050_RANGE_2_G:
    Serial.println("+-2G");
    break;
  case MPU6050_RANGE_4_G:
    Serial.println("+-4G");
    break;
  case MPU6050_RANGE_8_G:
    Serial.println("+-8G");
    break;
  case MPU6050_RANGE_16_G:
    Serial.println("+-16G");
    break;
  }
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
  case MPU6050_RANGE_250_DEG:
    Serial.println("+- 250 deg/s");
    break;
  case MPU6050_RANGE_500_DEG:
    Serial.println("+- 500 deg/s");
    break;
  case MPU6050_RANGE_1000_DEG:
    Serial.println("+- 1000 deg/s");
    break;
  case MPU6050_RANGE_2000_DEG:
    Serial.println("+- 2000 deg/s");
    break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
  case MPU6050_BAND_260_HZ:
    Serial.println("260 Hz");
    break;
  case MPU6050_BAND_184_HZ:
    Serial.println("184 Hz");
    break;
  case MPU6050_BAND_94_HZ:
    Serial.println("94 Hz");
    break;
  case MPU6050_BAND_44_HZ:
    Serial.println("44 Hz");
    break;
  case MPU6050_BAND_21_HZ:
    Serial.println("21 Hz");
    break;
  case MPU6050_BAND_10_HZ:
    Serial.println("10 Hz");
    break;
  case MPU6050_BAND_5_HZ:
    Serial.println("5 Hz");
    break;
  }

  Serial.println("");
  delay(100);
  Udp.begin(12346);

}

void loop() {
  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  OSCMessage msg("/valz"); // OSC address
    msg.add(a.acceleration.x);         // Adding argument
    msg.add(a.acceleration.y);         // Adding argument
    msg.add(a.acceleration.z);
    msg.add(g.gyro.x); 
    msg.add(g.gyro.y); 
    msg.add(g.gyro.z);
    msg.add(temp.temperature);      // Adding argument

  Udp.beginPacket(ip, port); // Computer's IP and port
  msg.send(Udp);             // Send the message over UDP
  Udp.endPacket();           // End the UDP packet

  delay(20);
}
2

You cannot measure yaw with the MPU-6050 as a horizon reference is required (such as a magnetometer).

By far the most stable and reliable method to measure YPR, with yaw relative to the starting orientation in this case) is to use an AHRS filter.

The Mahony filter is the simplest and fastest, and this one works well.

NOTE: It is very important not to slow down the orientation update loop by printing angles too often, or the filter will lose accuracy.

3

Thanks, @jremington - this is another library that would be rolled into my code above?

4

The Github code is standalone (except for the Arduino Wire library).

I avoid Adafruit sensor libraries, as they are very slow and unwieldy.

6

Wow, great bit of code you wrote there - I think it's 1/3rd of the size of what I was trying to run before.
I had two small questions, though : what variable can I access to get the accelerometer readings?
And re: update loops - what is the fastest I can run it? Since I'm sending messages over OSC, I'm already dealing with a bit of a lag there - so I'm hoping to accelerate as much as possible.

7

These variables:

  Wire.requestFrom(MPU_addr, 14); // request a total of 14 registers
  int t = Wire.read() << 8;
  ax = t | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  t = Wire.read() << 8;
  ay = t | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  t = Wire.read() << 8;
  az = t | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)

Since I'm sending messages over OSC,

Send them infrequently. The filter update frequency needs to be fast, 50-100 Hz if possible with no other activity, or the numerical integration will fail.

In normal situations orientation doesn't change rapidly. There should be no need to calculate and send Euler angles somewhere else at a frequency faster than a few Hz.

This code sets the "print angle" frequency to 5 Hz:

// print interval
unsigned long print_ms = 200; //print angles every "print_ms" milliseconds