while debugging a friend (and FreeIMU user) instability problem on his 4copter we discovered after some bloody debugging hours that, with the current MultiWii MPU6050 initialization code, the DLPF settings aren't actually stored in the sensor memory. This means that any DLPF setting is actually lost and never used by the sensor. Affected versions are 2.0 and further developers versions till today svn.
I'm still not sure why this happens, but changing the initialization order of the instructions seems to correctly fix the problem. The idea is to set the 0x1A CONFIG register with the appropriate DLPF setting as last configuration instruction.
Fixed code for MultiWii 2.0 is below. Uncomment the debug2 instruction to see for yourself that the value has been actually stored in sensor (the same debug instruction can be put at the end of the original initialization routine to show that no DLPF setting is stored with the original code).
Patch for today dev available at http://sprunge.us/eaMW
If you wanna test this on 2.0, replace original MPU6050 Gyro_init() function with the following:
Code: Select all
// ************************************************************************************************************
// I2C Gyroscope and Accelerometer MPU6050
// ************************************************************************************************************
#if defined(MPU6050)
void Gyro_init() {
TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
i2c_writeReg(MPU6050_ADDRESS, 0x6B, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(5);
i2c_writeReg(MPU6050_ADDRESS, 0x19, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
i2c_writeReg(MPU6050_ADDRESS, 0x6B, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
i2c_writeReg(MPU6050_ADDRESS, 0x1B, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// enable I2C bypass for AUX I2C
#if defined(MAG)
i2c_writeReg(MPU6050_ADDRESS, 0x6A, 0x00); //USER_CTRL -- DMP_EN=0 ; FIFO_EN=0 ; I2C_MST_EN=0 (I2C bypass mode) ; I2C_IF_DIS=0 ; FIFO_RESET=0 ; I2C_MST_RESET=0 ; SIG_COND_RESET=0
i2c_writeReg(MPU6050_ADDRESS, 0x37, 0x02); //INT_PIN_CFG -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=1 ; CLKOUT_EN=0
#endif
i2c_writeReg(MPU6050_ADDRESS, 0x1A, MPU6050_DLPF_CFG); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
//debug2 = i2c_readReg(MPU6050_ADDRESS, 0x1A);
//debug2 = MPU6050_DLPF_CFG;
}
Hope this helps,
Fabio Varesano
p.s.: thanks Sergio for the moral support