re-aranged code and added comments to be more consistant with chibios styles