diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp
index 4864beee..641976b2 100644
--- a/Firmware/MotorControl/commands.cpp
+++ b/Firmware/MotorControl/commands.cpp
@@ -2,7 +2,8 @@
 /* Includes ------------------------------------------------------------------*/
 
 // TODO: remove this option
-//#define ENABLE_LEGACY_PROTOCOL
+// and once the legacy protocol is phased out, remove the seq-no hack in protocol.py
+#define ENABLE_LEGACY_PROTOCOL
 
 #include "low_level.h"
 #include "protocol.hpp"
@@ -102,9 +103,8 @@ public:
                 well... it's not actually. Stupid STM. */, chunk) != USBD_OK)
                 osDelay(1);
             buffer += chunk;
-            length -= chunk;printf("got packet of length %d: \r\n", length); osDelay(5); hexdump(buffer, length);
+            length -= chunk;
         }
-        //printf("USB TX done\r\n"); osDelay(5);
         return 0;
     }
 
@@ -127,7 +127,6 @@ public:
             const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
             well... it's not actually. Stupid STM. */, length) != USBD_OK)
             osDelay(1);
-        //printf("USB TX done\r\n"); osDelay(5);
         return 0;
     }
 } usb_sender;
@@ -141,9 +140,10 @@ public:
         //Check length
         if (length > UART_TX_BUFFER_SIZE)
             return -1;
-        // Check if transfer is already ongoing
-        if (huart4.gState != HAL_UART_STATE_READY)
-            return -1;
+        // Loop until the UART is ready
+        // TODO: implement ring buffer to get a more continuous stream of data
+        while (huart4.gState != HAL_UART_STATE_READY)
+            osDelay(1);
         // memcpy data into uart_tx_buf
         memcpy(tx_buf_, buffer, length);
         // Start DMA background trasnfer
diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c
index 459f89ca..a0d370b6 100644
--- a/Firmware/MotorControl/low_level.c
+++ b/Firmware/MotorControl/low_level.c
@@ -23,7 +23,6 @@
 
 /* Private defines -----------------------------------------------------------*/
 
-#define STANDALONE_MODE // Drive operates without USB communication
 // #define DEBUG_PRINT
 
 /* Private macros ------------------------------------------------------------*/
diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py
index cf1b6814..2453891b 100644
--- a/tools/odrive/protocol.py
+++ b/tools/odrive/protocol.py
@@ -156,9 +156,13 @@ class PacketFromStreamConverter(PacketSource):
                 #print("crc8 mismatch")
                 continue
 
-            packet_length = header[1]
+            packet_length = header[1] + 2
             #print("wait for {} bytes".format(packet_length))
-            return self._input.get_bytes_or_fail(packet_length, deadline)
+            packet = self._input.get_bytes_or_fail(packet_length, deadline)
+            if calc_crc16(CRC16_INIT, packet) != 0:
+                #print("crc16 mismatch")
+                continue
+            return packet[:-2]
 
 
 class Channel(PacketSink):
@@ -192,6 +196,7 @@ class Channel(PacketSink):
             endpoint_id |= 0x8000
 
         self._outbound_seq_no = ((self._outbound_seq_no + 1) & 0x7fff)
+        self._outbound_seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the legacy protocol
         seq_no = self._outbound_seq_no
         packet = struct.pack('<HHH', seq_no, endpoint_id, output_length)
         packet = packet + input