Get rid of the wait 25 msec in your consumer loop. I would also consider breaking up the different actions so that they either occur in the same loop iteration, or are queued off into different loops. As you have it now, you have your producer loop iterating every 25 msec and passing data into a queue. In the consumer loop, the loop waits until it gets data in the queue, then it analyzes it, then it goes into a state machine for the other steps. When done all those steps, it goes back and waits for the next data in the queue, there is no need to wait for 25 msec. You want this loop to run as fast as possible if there is data available, and wait if there is no data. As it stands now, the whole process will take at least 25 msec, which means if it gets behind, it will never have an opportunity to catch up.
