Huge refactor to make mpu6050 work

I2C lib was changed, by some reaon if we reuse the opened bus
the write messas have no effect,

Also add a new message to protocol to send debug messages.
Make possible send a second message, before the first one was transmit