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);
}
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;
}