Huge refactor to make mpu6050 work

I2C lib was changed, by some reason if we reuse the opened bus
to write to some register, this set have no effect.

Also add a new message to protocol to send debug messages.

Changed double vars to float to reduce program size.
Make possible send a second message, before the first one was transmit