diff --git a/Core/Inc/can_handler.h b/Core/Inc/can_handler.h
index fb075bc1..45c2bd83 100644
--- a/Core/Inc/can_handler.h
+++ b/Core/Inc/can_handler.h
@@ -23,6 +23,14 @@ void can1_callback(CAN_HandleTypeDef *hcan);
  */
 int8_t queue_can_msg(can_msg_t msg);
 
+/**
+ * @brief Place a CAN message in a queue at a high priority.
+ * 
+ * @param msg CAN message to be sent.
+ * @return int8_t Error code.
+ */
+int8_t queue_prio_can_msg(can_msg_t msg);
+
 /**
  * @brief Initialize CAN line 1.
  * 
diff --git a/Core/Src/can_handler.c b/Core/Src/can_handler.c
index 437e1c3a..538168ab 100644
--- a/Core/Src/can_handler.c
+++ b/Core/Src/can_handler.c
@@ -96,6 +96,18 @@ int8_t queue_can_msg(can_msg_t msg)
 				  CAN_DISPATCH_FLAG);
 }
 
+/* TODO: Find what queue_and_set_flag (in embedded base) is used in, then change to support prio */
+int8_t queue_prio_can_msg(can_msg_t msg)
+{
+	if (!can_outbound_queue)
+		return -1;
+
+	/* Set the priority flag to 1U (Higher number = higher priority) */
+	osStatus_t status = osMessageQueuePut(can_outbound_queue, &msg, 1U, 0U);
+	osThreadFlagsSet(can_dispatch_handle, CAN_DISPATCH_FLAG);
+	return status;
+}
+
 osThreadId_t can_dispatch_handle;
 const osThreadAttr_t can_dispatch_attributes = {
 	.name = "CanDispatch",
diff --git a/Core/Src/dti.c b/Core/Src/dti.c
index b4de3bdb..322be97d 100644
--- a/Core/Src/dti.c
+++ b/Core/Src/dti.c
@@ -131,7 +131,7 @@ void dti_send_brake_current(uint16_t brake_current)
 
 	/* Send CAN message */
 	memcpy(&msg.data, &brake_current, 2);
-	queue_can_msg(msg);
+	queue_prio_can_msg(msg);
 }
 
 void dti_set_speed(int32_t rpm)
@@ -145,7 +145,7 @@ void dti_set_speed(int32_t rpm)
 
 	/* Send CAN message */
 	memcpy(msg.data, &rpm, msg.len);
-	queue_can_msg(msg);
+	queue_prio_can_msg(msg);
 }
 
 void dti_set_position(int16_t angle)
@@ -157,7 +157,7 @@ void dti_set_position(int16_t angle)
 
 	/* Send CAN message */
 	memcpy(msg.data, &angle, msg.len);
-	queue_can_msg(msg);
+	queue_prio_can_msg(msg);
 }
 
 void dti_set_relative_current(int16_t relative_current)
@@ -169,7 +169,7 @@ void dti_set_relative_current(int16_t relative_current)
 
 	/* Send CAN message */
 	memcpy(msg.data, &relative_current, msg.len);
-	queue_can_msg(msg);
+	queue_prio_can_msg(msg);
 }
 
 void dti_set_relative_brake_current(int16_t relative_brake_current)
@@ -181,7 +181,7 @@ void dti_set_relative_brake_current(int16_t relative_brake_current)
 
 	/* Send CAN message */
 	memcpy(msg.data, &relative_brake_current, msg.len);
-	queue_can_msg(msg);
+	queue_prio_can_msg(msg);
 }
 
 void dti_set_digital_output(uint8_t output, bool value)
@@ -192,7 +192,7 @@ void dti_set_digital_output(uint8_t output, bool value)
 
 	/* Send CAN message */
 	memcpy(msg.data, &ctrl, msg.len);
-	queue_can_msg(msg);
+	queue_prio_can_msg(msg);
 }
 
 void dti_set_max_ac_current(int16_t current)
@@ -204,7 +204,7 @@ void dti_set_max_ac_current(int16_t current)
 
 	/* Send CAN message */
 	memcpy(msg.data, &current, msg.len);
-	queue_can_msg(msg);
+	queue_prio_can_msg(msg);
 }
 
 void dti_set_max_ac_brake_current(int16_t current)
@@ -216,7 +216,7 @@ void dti_set_max_ac_brake_current(int16_t current)
 
 	/* Send CAN message */
 	memcpy(msg.data, &current, msg.len);
-	queue_can_msg(msg);
+	queue_prio_can_msg(msg);
 }
 
 void dti_set_max_dc_current(int16_t current)
@@ -228,7 +228,7 @@ void dti_set_max_dc_current(int16_t current)
 
 	/* Send CAN message */
 	memcpy(msg.data, &current, msg.len);
-	queue_can_msg(msg);
+	queue_prio_can_msg(msg);
 }
 
 void dti_set_max_dc_brake_current(int16_t current)
@@ -240,7 +240,7 @@ void dti_set_max_dc_brake_current(int16_t current)
 
 	/* Send CAN message */
 	memcpy(msg.data, &current, msg.len);
-	queue_can_msg(msg);
+	queue_prio_can_msg(msg);
 }
 
 void dti_set_drive_enable(bool drive_enable)
@@ -249,7 +249,7 @@ void dti_set_drive_enable(bool drive_enable)
 
 	/* Send CAN message */
 	memcpy(msg.data, &drive_enable, msg.len);
-	queue_can_msg(msg);
+	queue_prio_can_msg(msg);
 }
 
 int32_t dti_get_rpm(dti_t *mc)