Fixed MPU6000 driver and comms code