Refacotring and slower client code
Merge branch 'Chibios' of github.com:C3MA/fc_c into Chibios