Upgraded to ChibiOS 2.6.7