Strange problem with MPU6050

mikiti · March 6, 2021, 4:18pm

ciao.
For an experiment, I use a module MPU6050 and a module VL53L0X. The modules are read by an Arduino Nano. Module MPU6050 is mounted on a wheel that rotates +/-90 degrees relative to the horizon (zero degree). An object can be found in front of the wheel.
MPU6050 is mounted so that the y axis is oriented along the radius of the wheel.
When module MPU6050 is horizontal (pitch between -10 and 0), it reads the distance. If the object in front is less than 500 mm, the motor turns on a led. The system works well if the wheel rotates.
But if the wheel starts to rototranslate I have some wrong signals (the LED suddenly lights up even if MPU6050 is not horizontal and even if there is nothing in front of it).
I use this sketch:

#include <Wire.h>
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter

#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;

double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

int value;

//-------------------------------------
//    dichiarazioni VL53L0X
//-------------------------------------
#include "Adafruit_VL53L0X.h"
Adafruit_VL53L0X lox = Adafruit_VL53L0X();

//-------------------------------------
//    dichiarazioni LED
//-------------------------------------
#define LED 12


void setup() {
  Serial.begin(115200);
  Wire.begin();
#if ARDUINO >= 157
  Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
  TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // 0x00 Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }

  delay(100); // Wait for sensor to stabilize

  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);

  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

  kalmanX.setAngle(roll); // Set starting angle
  kalmanY.setAngle(pitch);
  gyroXangle = roll;
  gyroYangle = pitch;
  compAngleX = roll;
  compAngleY = pitch;

  timer = micros();

//-------------------------------------
//    dichiarazioni LED
//-------------------------------------
  pinMode(LED, OUTPUT);
  digitalWrite(LED,LOW);

//-------------------------------------
//  VL53L0X
//-------------------------------------

  while (! Serial) {
    delay(1);
  }
 
  Serial.println("Adafruit VL53L0X test");
  if (!lox.begin()) {
    Serial.println(F("errore avvio VL53L0X"));
    while(1);
  }
  // power
  Serial.println(F("VL53L0X API Simple Ranging example\n\n"));
}

void loop() {
 
    gyro();
    Serial.print(kalAngleY);
    Serial.print("\t");
    digitalWrite(LED, LOW);
    if ((kalAngleY > -10.0) && (kalAngleY < 0.0)) {
    value = 0;
    VL53L0X();
    Serial.println(value);
    if (value <= 500) {
      digitalWrite(LED,HIGH);
      delay(40);
      }   
    else {
      }
    }
    Serial.println("");
}

void gyro() {
  /* Update all the values */
  while (i2cRead(0x3B, i2cData, 14));
  accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
  tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
  gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
  gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
  gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;

  double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
  timer = micros();

  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // It is then converted from radians to degrees

#ifdef RESTRICT_PITCH // Eq. 25 and 26
  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

  double gyroXrate = gyroX / 131.0; // Convert to deg/s
  double gyroYrate = gyroY / 131.0; // Convert to deg/s

#ifdef RESTRICT_PITCH
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
    kalmanX.setAngle(roll);
    compAngleX = roll;
    kalAngleX = roll;
    gyroXangle = roll;
  } else
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleX) > 90)
    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
    kalmanY.setAngle(pitch);
    compAngleY = pitch;
    kalAngleY = pitch;
    gyroYangle = pitch;
  } else
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleY) > 90)
    gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
  kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
  gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
  gyroYangle += gyroYrate * dt;
  //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate() * dt;

  compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
  compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;

  // Reset the gyro angle when it has drifted too much
  if (gyroXangle < -180 || gyroXangle > 180)
    gyroXangle = kalAngleX;
  if (gyroYangle < -180 || gyroYangle > 180)
    gyroYangle = kalAngleY;
}

void VL53L0X() {
  VL53L0X_RangingMeasurementData_t measure; 
  //Serial.print("leggo... ");
  lox.rangingTest(&measure, false); // 'true' = debug data printout
  delay(5);
  if (measure.RangeStatus != 5) {  // gli errori di fase danno valori sbagliati
  //Serial.print("distanza (mm): ");
  value = measure.RangeMilliMeter;   
  //Serial.println((int)value/10);
  }
}

and this
I2C.ino

I use a kalman filter to have a stable signal.
I don't understand why the led lights up if the "IF" condition is false.

P.S: sorry for my English ...

jremington · March 6, 2021, 5:55pm

But if the wheel starts to rototranslate I have some wrong signals (the LED suddenly lights up even if MPU6050 is not horizontal and even if there is nothing in front of it).

Explain what you expect to happen, and what happens instead. Post examples of data to show the problem.

PS: This

#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter

is not a Kalman filter. Simple averaging will work much better.

mikiti · March 7, 2021, 12:32pm

As the wheel moves, the gyroscope transmits the pitch angle. When the angle is between -15 and 0 degrees, module VL53L0X takes the distance reading. If this measurement is equal to or less than 500 mm, the LED is switched on.
The problem I have is that during the rototranslatory motion (the wheel turns and moves) even if there is no obstacle in front of it, the LED sometimes lights up.
In the graphs below you can see what happens.
The first graph I reported the trend of the measured angle (blue line), the detected distance (orange cross), trigger threshold. In the second graph the trend of the measured angle is better seen.
The group "A", "B" and "C" indicates when the led lights up by mistake. The other scattered "Xs" are negligible (no effect on the LED). Group "D" is correct (obstacle detected at about 350 mm) and the LED turns on.
I don't understand why there are groups "A", "B" and "C".

mikiti · March 9, 2021, 11:03pm

I think I understand that the problem is how quickly the angle changes. This can also be seen from the two graphs.
What can I do?

jremington · March 10, 2021, 12:54am

What do you mean by "how quickly the angle changes"? Please describe the rate(s) of rotation of the wheel.

For a rotating wheel, it really makes no sense to use an IMU. Use a shaft encoder to measure the angle.

But if you insist on the IMU, as already pointed out, the code you are using to estimate angles is wrong, and is a mess. The only correct parts are the equations from the Freescale application note. You should start over and make sure that you can get accurate and reliable angular measurements. Avoid the wrong "Kalman filter" and the "complementary filter".

By far the best approach is to use modern 3D fusion filters, for the obsolete MPU-6050 these include the Rowberg DMP library or the Mahony filter, available here.

mikiti · March 10, 2021, 10:22pm

By "how quickly the angle changes" I mean that a rapid change in the angle of the MPU causes the LED to turn on.
I can't use an encoder because the "wheel" is not a real wheel. It is an object that has a rototranslatory motion and performs incomplete rotations (with a maximum excursion of +/- 90 °), then it stops, then it starts again ...
This is why I thought of a gyroscope.

However you are right. The MPU6050 is perhaps not the best choice, or at least the sketch I use is not good.

I tried to simplify it a lot.

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

Adafruit_MPU6050 mpu;

#include "Adafruit_VL53L0X.h"
Adafruit_VL53L0X lox = Adafruit_VL53L0X();

#define LED 12
int value;

void setup(void) {
  Serial.begin(115200);
  Wire.begin();

  pinMode(LED, OUTPUT);
  digitalWrite(LED,LOW);
  
  while (!Serial) {
    delay(10); // will pause Zero, Leonardo, etc until serial console opens
  }

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

  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);  // 2 - 4 - 8 - 16
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);    // deg/s: 250 - 500 - 1000 - 2000
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);  // filter band: 5 - 10 - 21 - 44 - 94 - 184 - 260 
  Serial.println("");

//-------------------------------------
//  VL53L0X
//-------------------------------------
  while (! Serial) {
    delay(1);
  }
 
  Serial.println("Adafruit VL53L0X test");
  if (!lox.begin()) {
    Serial.println(F("errore avvio VL53L0X"));
    while(1);
  }
  // power 
  Serial.println(F("VL53L0X API Simple Ranging example\n\n"));
  
  delay(100);
}

void loop() {
digitalWrite(LED, LOW);

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

  float pitch = a.acceleration.x*10;
  
  /* Print out the values */
  Serial.print(pitch);
  Serial.print("\t");
      if ((pitch > -15.0) && (pitch < 0.0)) {
    //Serial.println("************** ok **************");
    value = 0;
    //call VL53L0X
    VL53L0X();
    Serial.print(value);
    
    if (value <= 500) {
      Serial.print("\t");
      Serial.print("------------");
      Serial.print("\t");
      digitalWrite(LED,HIGH);
      //Serial.println("----------------> OSTACOLO <----------------");
      delay(40);
    }   
    else {
    }
    //-------------------------------------
    //  fine VL53L0X
    //-------------------------------------
Serial.println("");
    }
  Serial.println("");

  delay(10);
}

void VL53L0X() {
  VL53L0X_RangingMeasurementData_t measure;  
  //Serial.print("leggo... ");
  lox.rangingTest(&measure, false);
  delay(10);
  if (measure.RangeStatus != 5) {
  //Serial.print("distanza (mm): "); 
  value = measure.RangeMilliMeter;   
  //Serial.println((int)value/10);
  }
}

But the problem is not solved.
However, if the speed of movement is reduced, the problem disappears.
I do not understand...

jremington · March 10, 2021, 11:28pm

This is not "pitch":

  float pitch = a.acceleration.x*10;

These equations are correct, if Z is vertical.

  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

However, the accelerometer can be used to measure a tilt angle only if the object is motionless. If not, you need the gyro to correct the measurement.

MPU6050 is mounted so that the y axis is oriented along the radius of the wheel.

Where do the X and Y axes point?

mikiti · March 11, 2021, 9:09pm

Yes I know, I called it "pitch" to use the same name as the previous sketch.
I will try to modify the sketch I use.

In your opinion, why does that oddity I described happen? How is it possible that the IF loop becomes true?

mikiti · March 13, 2021, 11:02am

ciao,
I changed the code again.
No changes. It's frustrating...
The LED also lights up when the pitch (apparently) is out of range.
Could it depend by that the VL53L0X module has a measuring cone that sees the plane on which the system is supported? The module is located approximately 150 mm above the ground (on which wheel moves). Perhaps, when the system moves, the instant the MPU module is in range, the VL module sees something in its measuring cone.

it's possible?

jremington · March 13, 2021, 3:15pm

Use Serial.print() statements to show all the relevant variables, and some means to decide when the program malfunctions.

You have not posted any useful information about the mechanical setup, wiring, etc. Motor power supply problems can cause false readings or malfunctions, if the Arduino and sensors are powered from the same source.

system · July 11, 2021, 3:16pm

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

Topic		Replies	Views
MPU6050 and Arduino Nano - still rotating and other problems Sensors	7	500	May 9, 2024
MPU6050 produces incorrect values during flight Sensors	28	1096	January 25, 2024
MPU6050 GyroScope Readings Are Drifting Sensors	15	5878	January 10, 2024
MPU6050 - Gyroscope problem Networking, Protocols, and Devices	4	527	May 6, 2021
MPU6050 Sensors	5	1229	May 6, 2021

Strange problem with MPU6050

Related topics