Add support for MPU6050 to firmware