First version of RTOS version
Minor cleanup