Fixed const overflow. And few tweaks in mpu6050.