Hi thanks for the advise, I'am going to try to get the sensor under a heat-gun later today! So for the moment I will post my code!
////// --------- GYRO & ACCEL -----------//////
#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
/* IMU Data */
int16_t accX, accY, accZ;
int16_t gyroX, gyroY, gyroZ;
int16_t tempRaw;
float accXangle;//, accYangle; // Angle calculate using the accelerometer
float gyroXangle; // Angle calculate using the gyro only
float kalAngleX; // Calculated angle using a Kalman filter
float CurrentAngle;
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
////// --------- GYRO & ACCEL - SETUP -----------/////
Wire.begin();
TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
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; // 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 = (i2cData[0] << 8) | i2cData[1];
accY = (i2cData[2] << 8) | i2cData[3];
accZ = (i2cData[4] << 8) | i2cData[5];
kalmanX.setAngle(accXangle); // Set starting angle
gyroXangle = accXangle;
timer = micros();
////// --------- GYRO & ACCEL- LOOP -----------//////
/* Update all the values */
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]);
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
double gyroXrate = (double)gyroX/131.0;
CurrentAngle = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000);
timer = micros();
I hope this could help? do you need anything else? With that I use the I2C Lib by Kristian Lauszus, TKJ Electronics. and the Kalmanfilter Lib by Kristian Lauszus, TKJ Electronics as well, thos i didn't change at all.
PS: I just got another idea:
I have an motor shield mounted on my arduino (SparkFun Monster Moto Shield - DEV-10182 - SparkFun Electronics) which is connected to a 12V power source (3A Max). The shield drives the two Pololu motors. So when the robot fell it didn't stop the motor right away and as they were being blocked there would have been a significant raise of current on the lines of the motor shield. So normally that should be isolated from all the in-and outputs of the arduino but what if that somehow cause a change in tension or current in the system that damaged the sensor?