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.
It could well be that removing this one is all it needs.