Mpu 6050 keeps getting stuck at a specific value

Projects Programming
1

I wrote a relatively simple IMU script for the mpu 6050. It takes the accel data, calculates pitch and roll, and then applies a simple low pass filter to it. I also use the gyro data to determine the gryo pitch and yaw by multiplying the angular velocity every (0.5 sec) to save calculation costs. Finally, I use a complimentary filter to calculate the final 'system' pitch and yaw. For some reason unknown to me, the imu 'freezes' (the value doesn't change) when the pitch reaches 116 degrees and/or roll reaches 175 degrees, the only thing that changes is the gyro pitch. I can't figure out why this is happening for the life of me so any help is greatly appreciated.

Connections are fairly simple. Imu VCC is 3.3v and Gnd is Gnd, SCL to SCL and SDA to SDA. Using a Arduino Mega.

#include <Wire.h>
#include <math.h>

#define M_PI 3.14
#define RAD_TO_DEG 57.2958
#define GYRO_TIME_MS 500
#define GYRO_TIME_S GYRO_TIME_MS/1000

#define mpu_addr 0x68

#define accx_offset -3182 
#define accy_offset -123 
#define accz_offset 1562
#define gyrox_offset -3
#define gyroy_offset 0
#define gyroz_offset -22

int curr_gyro_time = GYRO_TIME_MS;
//euler angles
float pitch = 0;
float pitch_old = 0;
float pitch_new = 0;
float pitch_filter = 0.9;
float roll = 0;
float roll_old = 0;
float roll_new = 0;
float roll_filter = 0.9;
// gyro angles
float gyro_x = 0;
float gyro_y = 0;
float gyro_z = 0;
//complimentary filter
float system_pitch = 0;
float system_roll = 0;
float comp_pitch = 0.95;
float comp_roll = 0.95;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Wire.begin();
  Wire.beginTransmission(mpu_addr);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);
}

void loop() {
  // put your main code here, to run repeatedly:
  Wire.beginTransmission(mpu_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(mpu_addr, 6, true);
  
  float AccX = ((Wire.read() << 8 | Wire.read()) - accx_offset) / 16384.0; // X-axis value
  float AccY = ((Wire.read() << 8 | Wire.read()) - accy_offset) / 16384.0; // Y-axis value
  float AccZ = ((Wire.read() << 8 | Wire.read()) - accz_offset) / 16384.0; // Z-axis value

  pitch_new = atan2(AccX, AccZ) * RAD_TO_DEG;
  pitch = pitch_filter * pitch_old + (1 - pitch_filter) * pitch_new;
  pitch_old = pitch;

  roll_new  = atan2(AccY, AccZ) * RAD_TO_DEG;
  roll = roll_filter * roll_old + (1 - roll_filter) * roll_new;
  roll_old = roll;

  Wire.beginTransmission(mpu_addr);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(mpu_addr, 6, true);

  float GyroX = ((Wire.read() << 8 | Wire.read()) - gyrox_offset) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
  float GyroY = ((Wire.read() << 8 | Wire.read()) - gyroy_offset) / 131.0;
  float GyroZ = ((Wire.read() << 8 | Wire.read()) - gyroz_offset) / 131.0;

  if(millis() > curr_gyro_time)
  {
    gyro_x += GyroX * GYRO_TIME_S;
    gyro_y += GyroY * GYRO_TIME_S;
    gyro_z += GyroZ * GYRO_TIME_S;

    curr_gyro_time += GYRO_TIME_MS;

    //complimentary filter
    system_pitch = comp_pitch * gyro_x + (1 - comp_pitch) * pitch;
    system_roll  = comp_roll  * gyro_x + (1 - comp_roll)  * roll;
  }

  Serial.print(gyro_x);
  Serial.print(" ");
  Serial.print(pitch);
  Serial.print(" ");
  Serial.println(roll);

  //euler angles
  // pitch
  //Serial.print(system_pitch);
  // roll
  //Serial.print(system_roll);
  // yaw
  //Serial.print(gyro_x)
}
2

Euler angles are useless at certain orientations, due to gimbal lock.

Pitch and roll, and the complementary filter, are useful only for approximately level flight, which is why the field has moved on the using quaternion-based fusion filters.

3

So you're saying that the lock is happening because of gimbal lock? I am converting it to quaternions later, but imo I don't think its because of euler angles since the angles vary too much for the lock up to happen.

4

Is there no bug in the code? I feel like thats more likely than the gimbal lock since this has happened like 6 to 8 times.

5

No, that is not what I am saying.

Don't waste your time with the complementary filter. Also, one of the two equations you are using to calculate pitch and roll from the accelerometer data is incorrect.

See this tutorial for one correct approach: How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing-DFRobot

6

Okay if you have any resources for directly converting to quaternion it would be helpful since I'm going from euler to quaternion because I don't know how to directly calculate quaternions from accel and gyro data. That being said, I'd really appreciate it if you could help me figure out why the imu data is freezing up like that.

7

You could use any of the libraries based on the Mahony or Madgwick fusion filter. There are lots of web pages describing the theory.

This one works with the MPU-6050, although since that sensor was discontinued years ago, you have some sort of clone or counterfeit, and performance probably won't meet the original specifications.

8

Are these filters fast? I have devices reading and writing in real time

9

Try them and see if any meet your requirements.

10

OK thank you.

For the quaternion sensor fusion, is this what you were referring to? https://stanford.edu/class/ee267/lectures/lecture10.pdf the last couple of slides

Secondly for the link that you have provided above for the correct angle equations, do you have any resources for how those equations were derived.

And lastly, if gimbal lock does happen, that would mean that quaternions calculated from the gimbal locked euler angles would be incorrect right?

11

No, but there is a lot of useful information in that lecture, including one of several correct sets of equations for obtaining tilt angles from an accelerometer.

It is pointless to calculate quaternions from Euler angles without specifying which of the twelve definitions is used for the angles, regardless of gimbal lock.

12

Thank you. And can the madgwick or mahony filter be used without magnetometer data?

13

Yes, but the origin of the yaw angle will not be defined, and will drift.

14

I've modified my code to include the proper formulas and changed the gyro calculations, but for some reason, the accelerometer says that the imu is pitched at -14 degrees even though it has been kept on a level surface.

#include <Wire.h>
#include <math.h>

#define M_PI 3.14
#define RAD_TO_DEG 57.2958
#define GYRO_TIME_MS 500
#define GYRO_TIME_S GYRO_TIME_MS/1000

#define mpu_addr 0x68

#define accx_offset -3390 
#define accy_offset -150 
#define accz_offset 1552
#define gyrox_offset 0
#define gyroy_offset 2
#define gyroz_offset -25

int curr_gyro_time = GYRO_TIME_MS;
//euler angles
float pitch = 0;
float pitch_old = 0;
float pitch_new = 0;
float pitch_filter = 0.9;
float roll = 0;
float roll_old = 0;
float roll_new = 0;
float roll_filter = 0.9;
// gyro angles
float gyro_x = 0;
float gyro_y = 0;
float gyro_z = 0;
//complimentary filter
float system_pitch = 0;
float system_roll = 0;
float comp_pitch = 0.95;
float comp_roll = 0.95;

float prevT = 0;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Wire.begin();
  Wire.beginTransmission(mpu_addr);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);
}

void loop() {
  // put your main code here, to run repeatedly:
  Wire.beginTransmission(mpu_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(mpu_addr, 6, true);
  
  float AccX = ((Wire.read() << 8 | Wire.read()) - accx_offset) / 16384.0; // X-axis value
  float AccY = ((Wire.read() << 8 | Wire.read()) - accy_offset) / 16384.0; // Y-axis value
  float AccZ = ((Wire.read() << 8 | Wire.read()) - accz_offset) / 16384.0; // Z-axis value

  // Serial.println(AccX);

  pitch_new = atan2((-AccX), sqrt((AccY*AccY) + (AccZ*AccZ))) * RAD_TO_DEG;
  pitch = pitch_filter * pitch_old + (1 - pitch_filter) * pitch_new;
  pitch_old = pitch;

  roll_new  = atan2(AccY, AccZ) * RAD_TO_DEG;
  roll = roll_filter * roll_old + (1 - roll_filter) * roll_new;
  roll_old = roll;

  Wire.beginTransmission(mpu_addr);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(mpu_addr, 6, true);

  float GyroX = ((Wire.read() << 8 | Wire.read()) - gyrox_offset) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
  float GyroY = ((Wire.read() << 8 | Wire.read()) - gyroy_offset) / 131.0;
  float GyroZ = ((Wire.read() << 8 | Wire.read()) - gyroz_offset) / 131.0;

  float currT = millis();
  float dt = (currT - prevT) / 1000.0f;
  prevT = currT;

  gyro_x += GyroX * dt;
  gyro_y += GyroY * dt;
  gyro_z += GyroZ * dt;

  // if(millis() > curr_gyro_time)
  // {
  //   gyro_x += GyroX * GYRO_TIME_S;
  //   gyro_y += GyroY * GYRO_TIME_S;
  //   gyro_z += GyroZ * GYRO_TIME_S;

  //   curr_gyro_time += GYRO_TIME_MS;
  // }

  //complimentary filter
  system_pitch = comp_pitch * gyro_y + (1 - comp_pitch) * pitch;
  system_roll  = comp_roll  * gyro_x + (1 - comp_roll)  * roll;

  Serial.print(gyro_y);
  Serial.print(" ");
  Serial.print(pitch);
  Serial.print(" ");
  Serial.println(system_pitch);

  // digitalWrite(LED_BUILTIN, HIGH);

  //euler angles
  // pitch
  //Serial.print(system_pitch);
  // roll
  //Serial.print(system_roll);
  // yaw
  //Serial.print(gyro_x)
}
15

@jremington the gyro drift is also pretty crazy, after moving it around for like 10 15 seconds, the gyro ends up at like -20 degrees permanently.

16

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