Common: Fixed bugs with command interpreter

Fixed some bugs with the command interpreter. Tested the code with a
meg328p and it seems to work.