Fix up ChibiOS
Mods to ChibiOS