If the intention is to run this loop in a 20 Hz timing i would note that xQueueReceive can return immediately or earlier than 5ms depending on queue content and thus the code from line 181 on would be called more often than 20 Hz.