More UART changes. Correctly decoding UART message and changing modes.