3

I'm attempting to acquire angle values from an MPU6050 IMU sensor using an ESP32-DevKitM-1 microcontroller.

My setup contains the ESP32 board, the MPU6050 board, and an SD card reader to save the readings from the sensor:

enter image description here

enter image description here

I'm using the Arduino IDE v2.3.0.

I adapted the following testing code excerpt from another post about sensor, using the newer Adafruit_MPU6050 library and the SD functions:

#include <Wire.h>
#include "SPI.h"

#include <Adafruit_Sensor.h>
#include <Adafruit_MPU6050.h>

#include "SD.h"

const int PIN_IMU_SCL = 22;
const int PIN_IMU_SDA = 21;
const int PIN_SD_CS = 23;
const int PIN_SD_MISO = 10;
const int PIN_SD_MOSI = 18;
const int PIN_SD_CLK = 5;

const int IMU_GyroScale = 131;

Adafruit_MPU6050 IMU;
double IMU_Time;
double IMU_TimePrev = -1;
double IMU_GyroRotX;
double IMU_GyroRotY;
double IMU_GyroRotZ;

File DataFile;

void IMU_GetData(double& angleX, double& angleY, double& angleZ, double& temp)
{
  bool firstTime = false;

  if (IMU_TimePrev == -1)
    firstTime = true;

  // Calculating the elapsed time...
  IMU_TimePrev = IMU_Time;
  IMU_Time = millis();
  double timeDelta = (IMU_Time - IMU_TimePrev) / 1000;

  // Acquiring the readings from the accelerometer & gyroscope...
  sensors_event_t accelData;
  sensors_event_t gyroData;
  sensors_event_t tempData;
  IMU.getEvent(&accelData, &gyroData, &tempData);

  // Calculating the accelerometer angle...
  double accelRotX = (180 / PI) * atan(accelData.acceleration.x / sqrt(pow(accelData.acceleration.y, 2) + pow(accelData.acceleration.z, 2))); 
  double accelRotY = (180 / PI) * atan(accelData.acceleration.y / sqrt(pow(accelData.acceleration.x, 2) + pow(accelData.acceleration.z, 2)));
  double accelRotZ = (180 / PI) * atan(sqrt(pow(accelData.acceleration.x, 2) + pow(accelData.acceleration.y, 2)) / accelData.acceleration.z);

  // Calculating the gyroscope angle...
  double gyroRotX = (gyroData.gyro.x / IMU_GyroScale);
  double gyroRotY = (gyroData.gyro.y / IMU_GyroScale);
  double gyroRotZ = (gyroData.gyro.z / IMU_GyroScale);

  // Combining the two (complementary filter)...
  if (firstTime)
  {
    IMU_GyroRotX = accelRotX;
    IMU_GyroRotY = accelRotY;
    IMU_GyroRotZ = accelRotZ;
  }
  else
  {
    IMU_GyroRotX += timeDelta * gyroRotX;
    IMU_GyroRotY += timeDelta * gyroRotY;
    IMU_GyroRotZ += timeDelta * gyroRotZ;
  }

  angleX = (0.96 * accelRotX) + (0.04 * IMU_GyroRotX);
  angleY = (0.96 * accelRotY) + (0.04 * IMU_GyroRotY);
  angleZ = (0.96 * accelRotZ) + (0.04 * IMU_GyroRotZ);

  temp = tempData.temperature;
}

bool IMU_Init()
{
  bool status = IMU.begin();

  if (status)
  {
    IMU.setAccelerometerRange(MPU6050_RANGE_8_G);
    IMU.setGyroRange(MPU6050_RANGE_500_DEG);
    IMU.setFilterBandwidth(MPU6050_BAND_21_HZ);
  }

  IMU_Time = millis();

  delay(100);

  return status;
}

void setup() 
{
  Serial.begin(9600);
  Serial.println("Initializing...");

  Wire.begin(PIN_IMU_SDA, PIN_IMU_SCL);
  IMU_Init();

  SPI.begin(PIN_SD_CLK, PIN_SD_MISO, PIN_SD_MOSI, PIN_SD_CS);

  while (!SD.begin(PIN_SD_CS))
  {
    Serial.println("Unable to open SD!");
    delay(500);
  }

  DataFile = SD.open("/angles.txt", FILE_WRITE);
  DataFile.close();

  delay(100);
  Serial.println("Initialized!");
}

void loop() 
{
  double angleX;
  double angleY;
  double angleZ;
  double temp;
  IMU_GetData(angleX, angleY, angleZ, temp);
  String anglesStr = String(angleX) + ":" + String(angleY) + ":" + String(angleZ);

  DataFile = SD.open("/angles.txt", FILE_APPEND);
  DataFile.println(anglesStr);
  DataFile.close();

  Serial.println(anglesStr);

  delay(200);
}

Where IMU_GetData is the function responsible for acquiring the angle data from the gyroscope and the accelerator sensors on the MPU6050 board, and implementing a filter to attain a set of filtered angle values.

I then simply save the filtered angle values to the SD card. I communicate with the sensor using the Wire I2C library, and with the SD card using the SPI library.

The main issue I'm running into is the fact that the resultant angle values do not appear to accurately represent the rotation of the sensor.

As a test, I rotated the sensor 360° slowly around each one of the 3 axes, and saved the continuous angle readings during each rotation:

enter image description here

enter image description here

enter image description here

The time interval between each reading in the plots above is ~0.2 s.

The first issue is that a rotation about the Z-axis visible in the last graph above appears to not cause any appreciable change in any of the angle readings.

I initally assumed that could be caused by the value of the IMU_GyroScale constant that the gyroscope angle values get multiplied by during the calculations carried out in the IMU_GetData function. I assumed that all MPU6050 chips will use that same scaling factor, but then I tried several of the other listed scaling factors listed in the datasheet, with no appreciable difference.

Is there something visibly wrong in my code with the angle calculations for the Z-axis? Could this be a sign of a defective sensor board?

The second thing I was curious about was the format of the filtered angle values.

I initially assumed that the filtered angle values I would be getting from the sensor would be the Euler angles; Roll, Pitch & Yaw. However, these appear to not represent those.

In fact, one of the main sources listed in the original posting that describes in detail the process of calculating the filtered angle values, also provides a visualization of the angle values acquired from the sensor:

enter image description here

Are these "tilt" values related to Euler angles? Is it possible to convert between the two?

2 Answers 2

2

The formulas you are using look plain wrong. There are a few issues that stick out:

  • Rotating along z has no effect on the accelerometers. Any formula that pretends to give you the three Euler angles out of accelerometer data can only be wrong. You need a magnetic compass to get the yaw angle, otherwise you would have to rely solely on the gyros, which are prone to long-term drift.

  • The formulas for updating angles from gyro data are awfully complex. You don't want to do that. You want to instead keep track of you attitude using quaternions, which give much nicer maths.

  • If you look at the formulas that update IMU_GyroRot*, you should notice that they are prone to unbounded drift, which is then inherited by angle*. Obviously, the author of these formulas completely missed the point of a complementary filter.

I suggest you look at this old answer to a very similar question.

2
  • Thanks for your comment; Since gyroscopes drift their Yaw measurement, does that mean I'd strichtly need to use a magnetometer to measure the Yaw? As for the angle formulas, it does indeed appear that the author of the original article from 2013 may have missed a couple of things. I didn't actually know this, but the MPU6050 contains a Digital Motion Processor (DMP), that apparently handles a lot of the calculations for you. Using the i2cdev library from jrowberg, I was able to get some readings. Unfortunately, the Yaw still appears to drift, both in Euler and Quaternions...
    – Runsva
    Feb 16 at 8:06
  • 1
    @Runsva: Good that you found a way to get the DMP-computed rotation! Yes, you need a magnetometer to measure yaw, I do not know of any other easy way to get it. If you only have accelerometers and gyros, your only option is to calibrate the gyros as accurately as you can, and hope it is good enough to get an acceptable drift rate. Feb 16 at 8:21
0

Make sure that the sensor is correctly orientated and calibrated, verify gyro scaling and range settings, and consider the application requirements when choosing between tilt angles and Euler angles. If Euler angles are necessary, you'll need to implement a different algorithm to calculate them accurately.

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.