ESP32 with MPU6050 via Arduino IDE

[FrankenApps]

I am trying to get a MPU6050 working with ESP32. The following code works, with a minor adjustment (see comment in the code) with an Arduino UNO. I ran the I2CScanner and detected the MPU6050 with the Adress 0x68, which is the default one. The connections should be right:
VCC -> 3V3
GND -> GND
SDA -> 21
SCL -> 22

I get the error code "I2C write failed: 7" or "I2C write failed: 6", I am not sure, but I think error code 6 happend, because I2C was not at 400 kHz there..

This is the code:

Main.ino

Code: Select all

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

#define RESTRICT_PITCH 

Kalman kalmanX; 
Kalman kalmanY;

//Kalman
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;

double gyroXangle, gyroYangle; 
double compAngleX, compAngleY; 
double kalAngleX, kalAngleY; 

uint32_t timer;
uint32_t timer2;
uint8_t i2cData[14]; 

void setup() {
  Serial.begin(115200);
  //  TWBR = ((F_CPU / 400000L) - 16) / 2; //I2C bei 400kHz     !!! This did not work because Wire library of esp32 is different !!!

  //Sort of hacky solution:
   Wire.begin(21,22,400000); // Pins 21 und 22 bei 400kHz       !!! So I changed it to that, the effect should be the same !!!                    

  i2cData[0] = 7; //Sample Rate ist 100Hz
  i2cData[1] = 0x00; // Kein FSYNC und setzte 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; 
  i2cData[3] = 0x00; 
  while (i2cWrite(0x19, i2cData, 4, false)); 
  while (i2cWrite(0x6B, 0x01, true)); 

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { 
    Serial.print(F("Sensor nicht erreichbar!"));
    while (1);
  }

  delay(100); //sensor hochfahren


  while (i2cRead(0x3B, i2cData, 6));
  accX = (i2cData[0] << 8) | i2cData[1];
  accY = (i2cData[2] << 8) | i2cData[3];
  accZ = (i2cData[4] << 8) | i2cData[5];

  
#ifdef RESTRICT_PITCH 
  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else 
  double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

  kalmanX.setAngle(roll); //Startwinkel
  kalmanY.setAngle(pitch);
  gyroXangle = roll;
  gyroYangle = pitch;
  compAngleX = roll;
  compAngleY = pitch;

  timer = micros();
}

void loop(){
  double dt = (double)(micros() - timer2) / 1000000; 
  timer2 = micros();
  
  Serial.println(MPU6050());
}

double MPU6050() {
 //Update die Arrays
  while (i2cRead(0x3B, i2cData, 14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = (i2cData[6] << 8) | i2cData[7];
  gyroX = (i2cData[8] << 8) | i2cData[9];
  gyroY = (i2cData[10] << 8) | i2cData[11];
  gyroZ = (i2cData[12] << 8) | i2cData[13];

  double dt = (double)(micros() - timer) / 1000000; 
  timer = micros();

 
#ifdef RESTRICT_PITCH 
  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; 
  double gyroYrate = gyroY / 131.0; 

#ifdef RESTRICT_PITCH
  
  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); 

  if (abs(kalAngleX) > 90)
    gyroYrate = -gyroYrate; 
  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
 
  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); 

  if (abs(kalAngleY) > 90)
    gyroXrate = -gyroXrate; 
  kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt);
#endif

  gyroXangle += gyroXrate * dt; 
  gyroYangle += gyroYrate * 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;

 //resete Gyroskop nach zu großer Differenz
  if (gyroXangle < -180 || gyroXangle > 180)
    gyroXangle = kalAngleX;
  if (gyroYangle < -180 || gyroYangle > 180)
    gyroYangle = kalAngleY;


  return(kalAngleY);
}

I2C.ino

Code: Select all

const uint8_t IMUAddress = 0x68; 
const uint16_t I2C_TIMEOUT = 1000; 

uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
  return i2cWrite(registerAddress, &data, 1, sendStop); 
}

uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data, length);
  uint8_t rcode = Wire.endTransmission(sendStop); 
  if (rcode) {
    Serial.print(F("i2cWrite failed: "));
    Serial.println(rcode);
  }
  return rcode; 
}

uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
  uint32_t timeOutTimer;
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  uint8_t rcode = Wire.endTransmission(false); 
  if (rcode) {
    Serial.print(F("i2cRead failed: "));
    Serial.println(rcode);
    return rcode; 
  }
  Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); 
  for (uint8_t i = 0; i < nbytes; i++) {
    if (Wire.available())
      data[i] = Wire.read();
    else {
      timeOutTimer = micros();
      while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
      if (Wire.available())
        data[i] = Wire.read();
      else {
        Serial.println(F("i2cRead timeout"));
        return 5; 
      }
    }
  }
  return 0; 
}

Any hint on what could have gone wrong would be really helpful. Thanks in advance

[Deouss]

Maybe talk to author of your library. This one https://github.com/mohamed-elsabagh/esp32-mpu6050 seem to work fine

[FrankenApps]

Well I do not use a library apart from Kalman.h which definitly can not be the problem...

I got my MPU6050 working with I2CDevLib and the MPU6050 library by Jeff Rowberg with the following changes:

https://github.com/jrowberg/i2cdevlib/pull/367

Then because it still did not work I replaced

Code: Select all

#ifndef __arm__
    #include <avr/pgmspace.h>
#else

with

Code: Select all

#ifndef __arm__
    #include <pgmspace.h>
#else

in "MPU6050_6Axis_MotionApps20.h".

Then I made two little changes in the "MPU6050_DMP6" Example to get it working:

Code: Select all

        Wire.begin();
        TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)

changed to

Code: Select all

Wire.begin(21 , 22, 400000);

and

Code: Select all

Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady, RISING);

needs to be changed to

Code: Select all

Serial.println(F("Enabling interrupt detection (ESP32 pin 32)..."));
pinMode(32, INPUT_PULLUP);
attachInterrupt(32, dmpDataReady, RISING);

so the INT Pin must be connected to the GPIO 32.

So I got it working, but if anybody could nevertheless tell me why my initial code is not working, I would be glad to hear about that...

[xvinny]

FrankenApps,

I ran into the same problem with MPU6050 and got a bug in the last version of i2c library in arduino-esp32 SDK.

In order to apply a workaround until the next official release, you should download (from github.com/arduino-esp32) and replace these four files:
- libraries\Wire\Wire.cpp
- libraries\Wire\Wire.h
- cores\esp32\esp32-hal-i2c.cpp
- cores\esp32\esp32-hal-i2c.h

You will ask: why is this happening? Is Espressif releasing a bugged SDK?
The answer is yes, unfortunately.

So TAKE CARE when updating ESP SDK (both Arduino or ESP-IDF).

[FrankenApps]

Ok, thanks for that suggestion.
It will unfortunately only work, if ESP32 core isn't installed via Boardsmanager

[mcca69]

FrankenApps,

I ran into the same problem with MPU6050 and got a bug in the last version of i2c library in arduino-esp32 SDK.

In order to apply a workaround until the next official release, you should download (from github.com/arduino-esp32) and replace these four files:
- libraries\Wire\Wire.cpp
- libraries\Wire\Wire.h
- cores\esp32\esp32-hal-i2c.cpp
- cores\esp32\esp32-hal-i2c.h

You will ask: why is this happening? Is Espressif releasing a bugged SDK?
The answer is yes, unfortunately.

So TAKE CARE when updating ESP SDK (both Arduino or ESP-IDF).

That worked, I just replaced the files in the Arduino folder for the ESP32 libraries.
Thank you

[xvinny]

FrankenApps,

I ran into the same problem with MPU6050 and got a bug in the last version of i2c library in arduino-esp32 SDK.

In order to apply a workaround until the next official release, you should download (from github.com/arduino-esp32) and replace these four files:
- libraries\Wire\Wire.cpp
- libraries\Wire\Wire.h
- cores\esp32\esp32-hal-i2c.cpp
- cores\esp32\esp32-hal-i2c.h

You will ask: why is this happening? Is Espressif releasing a bugged SDK?
The answer is yes, unfortunately.

So TAKE CARE when updating ESP SDK (both Arduino or ESP-IDF).

That worked, I just replaced the files in the Arduino folder for the ESP32 libraries.
Thank you

The new version of the SDK has the fix for this bug

[ddnn88]

Here is an example using ESP32 + MPU9250 + Arduino IDE:
https://www.hackster.io/donowak/esp32-m ... ion-467dc1 .

MPU9250 is something different than MPU6050, but maybe you will find something useful for your project