Cleanup hardware queue

Some code cleanup. 
Same as earlier - there would be problems wit Atmel Studio...