Bug-fixes for FreeRTOS