Full night of work.

Moved to m-file S-functions as they are much easier to debug.

Created a ChibiOS config block to create mcuconf.h Moved some files around.