Чойто не работает то? Используй таймер а не SYSTICK, т.к. последний забирается самой RTOS для организации многозадачности.