diff --git a/src/uart.c b/src/uart.c
index 970ffbf..73e0045 100644
--- a/src/uart.c
+++ b/src/uart.c
@@ -4,6 +4,7 @@
 #include "include/uart.h"
 #include "include/commands.h"
 
+/* Make uart_printf be called whenever printf() is invoked */
 static FILE stream = FDEV_SETUP_STREAM(uart_printf, NULL, _FDEV_SETUP_WRITE);
 
 void uart_init(unsigned int baud)
@@ -22,7 +23,7 @@ void uart_init(unsigned int baud)
 
 	/* Set frame format */
 	UCSR1C |= (3<<UCSZ10); // 8 data bits
-	// UCSR1C &= ~(1<<USBS1); // 1 stop bit
+	UCSR1C |= (1<<USBS1); // 2 stop bits
 }
 
 void uart_sendByte(uint8_t byte)
@@ -44,8 +45,8 @@ int uart_printf(char var, FILE *stream)
 
 ISR(USART1_RX_vect)
 {
-	char u;
-	u = UDR1;
-	
-	cmd_collect_char(u);
+	char c = UDR1;
+
+	cmd_collect_char(c);
+	uart_sendByte(c);
 }
\ No newline at end of file