Merged old code and built-in cube libraries. TODO: Convert to ChibiOS
and finish moving to HAL driver
Fixed missing file