@ezikiel12
That exactly what I would have recommended or you could try to look at the difference between the current angle and the new angle from the accelerometer, if the difference is larger that a certain value, then decrease the value or even just drop it.
I'm actually working on a project with the MPU-6050 right now, but unfortunatly I can't show the full source code, but I will just copy paste the relevant sections:
This goes into setup:
Wire.begin();
i2cWrite(0x1B,0x00); // Set Full Scale Range to ±250deg/s
i2cWrite(0x1C,0x00); // Set Full Scale Range to ±2g
i2cWrite(0x19,0x27); // Set the sample rate to 200Hz
i2cWrite(0x6B,0x01); // PLL with X axis gyroscope reference and disable sleep mode
uint8_t buf;
i2cRead(0x75,1,&buf);
if(buf != 0x68) { // Read "WHO_AM_I" register
Serial.println(F("Error reading sensor"));
while(1);
}
delay(100); // Wait for sensor to get ready
uint8_t data[4];
i2cRead(0x3D,4,data);
accY = ((data[0] << 8) | data[1]);
accZ = ((data[2] << 8) | data[3]);
double accAngle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
kalman.setAngle(accAngle); // Set starting angle
And put this in your loop:
uint8_t data[8];
while (!i2cRead(0x3D,8,data));
accY = ((data[0] << 8) | data[1]);
accZ = ((data[2] << 8) | data[3]);
gyroX = ((data[6] << 8) | data[7]);
accAngle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
gyroRate = (double)gyroX/131.0;
pitch = kalman.getAngle(accAngle, gyroRate, (double)(micros()-timer)/1000000.0); // Calculate the angle using a Kalman filter
timer = micros();
And these are the i2cRead and i2cWrite functions:
void i2cWrite(uint8_t registerAddress, uint8_t data) {
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data);
Wire.endTransmission(); // Send stop
}
uint8_t i2cRead(uint8_t registerAddress, uint8_t nbytes, uint8_t * data) {
unsigned long timeOutTime = micros();
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.endTransmission(false); // Don't release the bus
Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
for(uint8_t i = 0; i < nbytes; i++) {
if (Wire.available())
data[i] = Wire.read();
else {
timeOutTime = micros();
while (((micros() - timeOutTime) < I2C_TIMEOUT) && !Wire.available());
if (Wire.available())
data[i] = Wire.read();
else
return 0; // Error in communication
}
}
return 1;
}