diff --git a/src/stm32/can.c b/src/stm32/can.c
index ff88edb9..a82ad688 100644
--- a/src/stm32/can.c
+++ b/src/stm32/can.c
@@ -135,7 +135,11 @@ canbus_send(uint32_t id, uint32_t len, uint8_t *data)
         irq_enable();
         return -1;
     }
-    int mbox = (tsr & CAN_TSR_CODE) >> CAN_TSR_CODE_Pos;
+    int mbox = 2;
+    if (tsr & CAN_TSR_TME0)
+        mbox = 0;
+    else if (tsr & CAN_TSR_TME1)
+        mbox = 1;
     CAN_TxMailBox_TypeDef *mb = &SOC_CAN->sTxMailBox[mbox];
 
     /* Set up the DLC */