Changed code to compile with chibios-21.11.x and added suggestion from [http://forum.chibios.org/viewtopic.php?t=5666](http://forum.chibios.org/viewtopic.php?t=5666)