Corrected the main to prevent constant semaphore acquiring/releasing that was freezing the message queue. Refactoring, adding some comments, minor improvements.
Merge branch 'Sensors' of https://github.com/uCtrl/Firmware into msghandler