Working on the ChibiOS version of the code. Initial states are in.