Tested and Fixed Command Parsing

Command parsing is now tested and works.

Made some small fixes (wrong UART)
Added an optional echo to the console RX Task.