ESP32 and MPU6050 without library

crownous · April 18, 2023, 6:49pm

Hi, I used MPU6050 and this code on ATmega328P. Now I upgraded mcu to esp32 and the code stopped working well. I found out that the problem is in gyro, the values are too high. Any ideas how to fix it?


#include<Wire.h>
const int MPU_addr = 0x68;
double pitchInput, rollInput, yawInput, altitudeInput;
double xAcc, yAcc, zAcc, xGyro, yGyro, zGyro;
double currentGyroMillis, previousGyroMillis, deltaGyroTime;
double pitchInputAcc, rollInputAcc, yawInputAcc;
double pitchInputGyro, rollInputGyro, yawInputGyro;
double rollGyroOffset, pitchGyroOffset, yawGyroOffset, rollAccOffset, pitchAccOffset, yawAccOffset;

void setup() {
  Wire.begin();
  Serial.begin(115200);
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  //gyro config
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1B);
  Wire.write(0x10);
  Wire.endTransmission(true);
  //accelometer config
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1C);
  Wire.write(0x10);
  Wire.endTransmission(true);
  currentGyroMillis = millis();
  if (rollAccOffset == 0) {
    for (int i = 0; i < 200; i++) {

      Wire.beginTransmission(MPU_addr);
      Wire.write(0x3B);
      Wire.endTransmission(false);
      Wire.requestFrom(MPU_addr, 6, true);

      xAcc = (Wire.read() << 8 | Wire.read()) / 4096.0 ;
      yAcc = (Wire.read() << 8 | Wire.read()) / 4096.0 ;
      zAcc = (Wire.read() << 8 | Wire.read()) / 4096.0 ;

      pitchAccOffset += (atan((yAcc) / sqrt(pow((xAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG);
      rollAccOffset += (atan(-1 * (xAcc) / sqrt(pow((yAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG);

      if (i == 199) {
        rollAccOffset = rollAccOffset /  200;
        pitchAccOffset = pitchAccOffset / 200;
      }
    }
  }
  if (rollGyroOffset == 0) {
    for (int i = 0; i < 200; i++) {

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

      xGyro = Wire.read() << 8 | Wire.read();
      yGyro = Wire.read() << 8 | Wire.read();
      zGyro = Wire.read() << 8 | Wire.read();

      rollGyroOffset += yGyro / 32.8 ;
      pitchGyroOffset += xGyro / 32.8;
      yawGyroOffset += zGyro / 32.8;
      if (i == 199) {
        rollGyroOffset = rollGyroOffset / 200;
        pitchGyroOffset = pitchGyroOffset / 200;
        yawGyroOffset = yawGyroOffset / 200;
      }
    }
  }
}

void loop() {
  previousGyroMillis = currentGyroMillis;
  currentGyroMillis = millis();
  deltaGyroTime = (currentGyroMillis - previousGyroMillis) / 1000;

  //gyro

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 6, true);
  xGyro = Wire.read() << 8 | Wire.read();
  yGyro = Wire.read() << 8 | Wire.read();
  zGyro = Wire.read() << 8 | Wire.read();


  xGyro = (xGyro / 32.8) - pitchGyroOffset;
  yGyro = (yGyro / 32.8) - rollGyroOffset;
  zGyro = (zGyro / 32.8) - yawGyroOffset;

  pitchInputGyro = xGyro * deltaGyroTime ;
  rollInputGyro = yGyro * deltaGyroTime;
  yawInputGyro = zGyro * deltaGyroTime;

  //accelometer

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 6, true);
  xAcc = (Wire.read() << 8 | Wire.read()) / 4096.0 ;
  yAcc = (Wire.read() << 8 | Wire.read()) / 4096.0 ;
  zAcc = (Wire.read() << 8 | Wire.read()) / 4096.0 ;

  pitchInputAcc = (atan((yAcc) / sqrt(pow((xAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG) - pitchAccOffset;
  rollInputAcc = (atan(-1 * (xAcc) / sqrt(pow((yAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG) - rollAccOffset;

  //complementary filter
  rollInput = 0.98 * (rollInput + rollInputGyro) + 0.02 * (rollInputAcc);
  pitchInput = 0.98 * (pitchInput + pitchInputGyro) + 0.02 * (pitchInputAcc);
  yawInputAcc = atan2((sin(rollInput) * cos(pitchInput) * xAcc + sin(pitchInput) * yAcc + cos(rollInput) * cos(pitchInput) * zAcc), sqrt(pow(sin(rollInput) * sin(pitchInput) * xAcc - cos(rollInput) * sin(pitchInput) * zAcc, 2) + pow(cos(pitchInput) * xAcc, 2))) - 1;
  yawInput = 0.98 * (yawInput + yawInputGyro) + 0.02 * (yawInputAcc);
}

crownous · April 19, 2023, 7:03am

I forgot to mention that the module works well with the adafruit library.

kolaha · April 19, 2023, 8:34am

replace gyro

crownous · April 19, 2023, 8:35am

Well, as I said, module works fine and there is also no way to replace just gyro, because it's all in one chip.

kolaha · April 19, 2023, 8:38am

replace chip. me for example, i will not using broken part

crownous · April 19, 2023, 8:42am

I'm not sure if you understand me, but I tested the module with ATmega328p and it is fine. I also tried running it with esp32 and esp8266 but it doesn't work there. Could the incompatibility in code be caused by, for example, a different processor architecture?

horace · April 19, 2023, 8:54am

cannot see print statements in your code in post 1
can you give examples of the output ftom the ATmega328p and ESP devices indicating the problem?
have you tried the Adafruit_MPU6050 library?

crownous · April 19, 2023, 9:01am

Yes, I tried. ESP32 adafruit lib output:

Accel X: -0.12 	Y: 0.26 	Z: 11.66 m/s^2 
Gyro X: 0.01 	Y: -0.01 	Z: 0.04 radians/s

ESP32 and my code output:

Pitch2.33
Roll-44.25
Yaw9.07

ATmega328P and my code:

Pitch0.02
Roll-0.04
Yaw0.01

horace · April 19, 2023, 9:42am

did you alter the ATmega328P code to get it to compile on the ESP32?
problem could be something to do with int being 16bit on the ATmega328P and 32 bit on the ESP32
I suggest adding print statements to both versions to display intermediate values so you can determine where it goes wrong, e.g. rollGyroOffset etc at the end of startup()

Koepel · April 19, 2023, 10:06am

In the virtual world, there is no problem with voltages or with a slightly tilted sensor. I think it works:

Start the simulation, then click on the MPU-6050 module to change the settings.

Your MPU-6050 is probably counterfeit.
Does your module have a voltage regulator. If you power that with 3.3V, then the sensor might only get 2.5V or so. If you are very sure that your module has a voltage regulator, then power the module with 5V to VCC.

How do you know that the ESP32 results in wrong values ? Maybe those values are correct and the values with the Uno are wrong, and the roll of -44 is just an offset of a damaged sensor.

If you connect another board to the sensor, how can you be sure that you did not tilt the sensor ?
Can you attach the sensor with a clamp to something ? and then interchange the Uno with the ESP32 ?

By the way, you should show the sketch that shows the problem, not another sketch. Since you don't show the calculated values in the sketch, it is not the sketch that you used. It is very important for us that we look at the code that causes the problem.

[UPDATE]
When using millis() or micros() and calculating the difference with a previous time, then you must use "unsigned long".

Your loop() is running at a high speed, and the accuracy of millis() on a Uno is only 4ms. That means you will get a big difference between the Uno and the ESP32.
Can you try "micros()" ?

unsigned long previousMicros;

void loop()
{
  unsigned long currentMicros = micros();
  unsigned long elapsedMicros = currentMicros - previousMicros;
  previousMicros = currentMicros;

  deltaGyroTime = (double) elapsedMicros / 1000000.0;
}

crownous · April 19, 2023, 10:22am

I used the code I posted, just added some Serial.print() functions to display the calculated values on the serial monitor. After uploading the code, a "new" 0 is created for all axes, so changing the board doesn't play a role. Finally it really looks like the error is on the sensor side, I already ordered a new one. When it arrives, I will test it and inform you here.

That's a good idea for better accuracy. Thanks!

Koepel · April 19, 2023, 10:31am

Yes, and that is very important to know
If you have many Serial.print() in the loop() and you are sending more than the baudrate can keep up to transmit, then the TX buffer (inside the Serial library) will get full. Then the Serial.print() will wait with each character until there is a free spot in the buffer.

A full TX buffer can make the Arduino Uno run 100 times slower. A slowed down loop() makes millis() more accurate, because there is more time.

You see how important it is to see the exact sketch that causes the problem

horace · April 19, 2023, 11:58am

as an experiment added some print statements to your code to display values in setup()

ATmega328P results

rollAccOffset 15.26
rollAccOffset 15.26
rollGyroOffset 1.76
pitchGyroOffset  8.59
yawGyroOffset 0.29

ESP32 results

rollAccOffset -45.06
rollAccOffset -45.06
rollGyroOffset 1.84
pitchGyroOffset  8.59
yawGyroOffset 0.27

modified following statements in setup() to use short (16bit) type

      xAcc = (short(Wire.read() << 8 | Wire.read())) / 4096.0 ;
      yAcc = (short(Wire.read() << 8 | Wire.read())) / 4096.0 ;
      zAcc = (short(Wire.read() << 8 | Wire.read())) / 4096.0 ;

and the ESP32 prints

rollAccOffset 17.01
rollAccOffset 17.01
rollGyroOffset 1.79
pitchGyroOffset  8.60
yawGyroOffset 0.29

looks like conversion problems
think you need to redesign your algorithm

horace · April 19, 2023, 7:15pm

try this version of your program of post 1
added a couple of prints and cast the result of wire.read() operations to int16_t (signed 16 bit integer)

#include<Wire.h>
const int MPU_addr = 0x68;
double pitchInput, rollInput, yawInput, altitudeInput;
double xAcc, yAcc, zAcc, xGyro, yGyro, zGyro;
double currentGyroMillis, previousGyroMillis, deltaGyroTime;
double pitchInputAcc, rollInputAcc, yawInputAcc;
double pitchInputGyro, rollInputGyro, yawInputGyro;
double rollGyroOffset, pitchGyroOffset, yawGyroOffset, rollAccOffset, pitchAccOffset, yawAccOffset;

void setup() {
  Wire.begin();
  Serial.begin(115200);
  Serial.println(sizeof(int16_t));
  Serial.println(sizeof(int));
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  //gyro config
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1B);
  Wire.write(0x10);
  Wire.endTransmission(true);
  //accelometer config
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1C);
  Wire.write(0x10);
  Wire.endTransmission(true);
  currentGyroMillis = millis();
  if (rollAccOffset == 0) {
    for (int i = 0; i < 200; i++) {

      Wire.beginTransmission(MPU_addr);
      Wire.write(0x3B);
      Wire.endTransmission(false);
      Wire.requestFrom(MPU_addr, 6, true);

      xAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0 ;
      yAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0 ;
      zAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0 ;

      pitchAccOffset += (atan((yAcc) / sqrt(pow((xAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG);
      rollAccOffset += (atan(-1 * (xAcc) / sqrt(pow((yAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG);

      if (i == 199) {
        rollAccOffset = rollAccOffset /  200;
        pitchAccOffset = pitchAccOffset / 200;
      }
    }
  }
    Serial.print("rollAccOffset "); Serial.println(rollAccOffset);
      Serial.print("rollAccOffset "); Serial.println(rollAccOffset);

  if (rollGyroOffset == 0) {
    for (int i = 0; i < 200; i++) {

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

      xGyro = (int16_t(Wire.read() << 8 | Wire.read()));
      yGyro = (int16_t(Wire.read() << 8 | Wire.read()));
      zGyro = (int16_t(Wire.read() << 8 | Wire.read()));

      rollGyroOffset += yGyro / 32.8 ;
      pitchGyroOffset += xGyro / 32.8;
      yawGyroOffset += zGyro / 32.8;
      if (i == 199) {
        rollGyroOffset = rollGyroOffset / 200;
        pitchGyroOffset = pitchGyroOffset / 200;
        yawGyroOffset = yawGyroOffset / 200;
      }
    }
  }
 Serial.print("rollGyroOffset "); Serial.println(rollGyroOffset); 
 Serial.print("pitchGyroOffset  "); Serial.println(pitchGyroOffset ); 
 Serial.print("yawGyroOffset "); Serial.println(yawGyroOffset);
  Serial.print(" "); Serial.println();
 }

void loop() {
  previousGyroMillis = currentGyroMillis;
  currentGyroMillis = millis();
  deltaGyroTime = (currentGyroMillis - previousGyroMillis) / 1000;

  //gyro

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 6, true);
  xGyro = (int16_t(Wire.read() << 8 | Wire.read()));
  yGyro = (int16_t(Wire.read() << 8 | Wire.read()));
  zGyro = (int16_t(Wire.read() << 8 | Wire.read()));


  xGyro = (xGyro / 32.8) - pitchGyroOffset;
  yGyro = (yGyro / 32.8) - rollGyroOffset;
  zGyro = (zGyro / 32.8) - yawGyroOffset;

  pitchInputGyro = xGyro * deltaGyroTime ;
  rollInputGyro = yGyro * deltaGyroTime;
  yawInputGyro = zGyro * deltaGyroTime;

  //accelometer

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 6, true);
  xAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0 ;
  yAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0 ;
  zAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0 ;

  pitchInputAcc = (atan((yAcc) / sqrt(pow((xAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG) - pitchAccOffset;
  rollInputAcc = (atan(-1 * (xAcc) / sqrt(pow((yAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG) - rollAccOffset;

  //complementary filter
  rollInput = 0.98 * (rollInput + rollInputGyro) + 0.02 * (rollInputAcc);
  pitchInput = 0.98 * (pitchInput + pitchInputGyro) + 0.02 * (pitchInputAcc);
  yawInputAcc = atan2((sin(rollInput) * cos(pitchInput) * xAcc + sin(pitchInput) * yAcc + cos(rollInput) * cos(pitchInput) * zAcc), sqrt(pow(sin(rollInput) * sin(pitchInput) * xAcc - cos(rollInput) * sin(pitchInput) * zAcc, 2) + pow(cos(pitchInput) * xAcc, 2))) - 1;
  yawInput = 0.98 * (yawInput + yawInputGyro) + 0.02 * (yawInputAcc);
  Serial.print("pitch "); Serial.println(pitchInput);
 Serial.print("roll "); Serial.println(rollInput);
 Serial.print("yawInput "); Serial.println(yawInput);
 delay(3000);
}

esp32 results seem similar to those from ATmega328P
ESP32 serial monitor output

rollAccOffset 15.30
rollAccOffset 15.30
rollGyroOffset 2.88
pitchGyroOffset  9.17
yawGyroOffset 0.37
 
pitch 0.33
roll 0.77
yawInput 0.07
pitch -0.99
roll -2.23
yawInput 0.21
pitch -2.65
roll -4.99
yawInput -0.55
pitch -4.53
roll -7.63
yawInput -0.50
pitch -5.12
roll -10.65

crownous · April 20, 2023, 6:18pm

Thanks! That makes sense.

system · October 17, 2023, 6:19pm

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

Topic		Replies	Views
MPU6050 high Gyro value Sensors	8	600	December 14, 2023
Weird behavior with MPU6050? Programming Questions	8	414	May 6, 2023
Variable runtime using ESP32 board and MPU6050 Hardware	2	245	July 17, 2024
[Solved] Problem with data transfer from MPU6050 (GY521) to ESP32 Sensors	7	7014	May 6, 2021
MPU6050 increasing tilt, pich yaw values problem Sensors	4	825	November 5, 2022

ESP32 and MPU6050 without library

Related topics