Fix FMU/IO v1

Looks like the commit involving I2C timeouts broke the FMU/IO setup.

Without this revert the servos go into some strange position and do not move or follow the commands at all.