diff --git a/Core/Inc/can_handler.h b/Core/Inc/can_handler.h
index 7d97fe4..c451120 100644
--- a/Core/Inc/can_handler.h
+++ b/Core/Inc/can_handler.h
@@ -17,12 +17,16 @@
 #include "can.h"
 #include "cmsis_os.h"
 
-//void can1_callback(CAN_HandleTypeDef *hcan);
+void can1_callback(CAN_HandleTypeDef *hcan);
 
 void vCanDispatch(void *pv_params);
 extern osThreadId_t can_dispatch_handle;
 extern const osThreadAttr_t can_dispatch_attributes;
 
+void vCanReceive(void *pv_params);
+extern osThreadId_t can_receive_thread;
+extern const osThreadAttr_t can_receive_attributes;
+
 int8_t queue_can_msg(can_msg_t msg);
 void can1_init();
 
diff --git a/Core/Inc/msb.h b/Core/Inc/msb.h
index d30f1b3..0ca0433 100644
--- a/Core/Inc/msb.h
+++ b/Core/Inc/msb.h
@@ -76,6 +76,7 @@ void strain2_read(uint32_t strain2);
 void motion_fx_init(void);
 void process_motion_fx(MFX_input_t *data_in, MFX_output_t *data_out,
 		       float delta_time);
+void imu_zero(uint8_t yaw_byte, uint8_t pitch_byte, uint8_t roll_byte);
 
 #endif
 
diff --git a/Core/Inc/msb_conf.h b/Core/Inc/msb_conf.h
index 00fc0ee..0abb9c6 100644
--- a/Core/Inc/msb_conf.h
+++ b/Core/Inc/msb_conf.h
@@ -17,14 +17,18 @@
 #define DELAY_CAN_DISPATCH 2
 
 // CAN IDS
-#define CANID_TEMP_SENSOR     0x602
-#define CANID_IMU_ACCEL	      0x603
-#define CANID_IMU_GYRO	      0x604
-#define CANID_STRAIN_SENSE    0x605
-#define CANID_SHOCK_SENSE     0x606
-#define CANID_TOF	      0x607
-#define CANID_WHEEL_TEMP      0x608
-#define CANID_IMU_ORIENTATION 0x609
+#define CANID_TEMP_SENSOR	 0x602
+#define CANID_IMU_ACCEL		 0x603
+#define CANID_IMU_GYRO		 0x604
+#define CANID_STRAIN_SENSE	 0x605
+#define CANID_SHOCK_SENSE	 0x606
+#define CANID_TOF		 0x607
+#define CANID_WHEEL_TEMP	 0x608
+#define CANID_IMU_ORIENTATION	 0x609
+#define CANID_IMUZERO_FRONTLEFT	 0x60A
+#define CANID_IMUZERO_FRONTRIGHT 0x60B
+#define CANID_IMUZERO_BACKLEFT	 0x60C
+#define CANID_IMUZERO_BACKRIGHT	 0x60E
 
 // Sensors to use, comment out to disable
 
@@ -35,7 +39,7 @@
 #define SENSOR_TEMP	// SHT30
 #define SENSOR_SHOCKPOT // ADC1
 #define SENSOR_STRAIN	// ADC1
-#define SENSOR_TOF       // VL6180X
+#define SENSOR_TOF	// VL6180X
 
 #define SENSOR_IMU // LSM6DSO
 
diff --git a/Core/Src/can_handler.c b/Core/Src/can_handler.c
index af8f191..09f581a 100644
--- a/Core/Src/can_handler.c
+++ b/Core/Src/can_handler.c
@@ -12,6 +12,7 @@
 #include "can_handler.h"
 #include "can.h"
 #include "msb_conf.h"
+#include "msb.h"
 
 #include "stdio.h"
 #include <assert.h>
@@ -20,10 +21,14 @@
 
 #define CAN_MSG_QUEUE_SIZE 25 /* messages */
 static osMessageQueueId_t can_outbound_queue;
+static osMessageQueueId_t can_inbound_queue;
 
-static uint16_t id_list[4] = { 0x000, 0x000, 0x000, 0x002 };
+static uint16_t id_list[4] = {
+	0x000, 0x000, 0x000, 0x002
+}; // id_list[0] is reserved for the IMU Zero CAN ID, which is added in can1_init().
 
 extern CAN_HandleTypeDef hcan1;
+extern device_loc_t device_loc;
 
 can_t *can1;
 
@@ -35,11 +40,58 @@ void can1_init()
 
 	can1->hcan = &hcan1;
 	assert(!can_init(can1));
+
+	/* Add the correct IMU Zero CAN ID to the filter depending on the location of the MSB. */
+	switch (device_loc) {
+	case DEVICE_FRONT_LEFT:
+		id_list[0] = CANID_IMUZERO_FRONTLEFT;
+		break;
+	case DEVICE_FRONT_RIGHT:
+		id_list[0] = CANID_IMUZERO_FRONTRIGHT;
+		break;
+	case DEVICE_BACK_LEFT:
+		id_list[0] = CANID_IMUZERO_BACKLEFT;
+		break;
+	case DEVICE_BACK_RIGHT:
+		id_list[0] = CANID_IMUZERO_BACKRIGHT;
+		break;
+	}
+
 	assert(!can_add_filter_standard(can1, id_list));
 #endif
 
 	can_outbound_queue =
 		osMessageQueueNew(CAN_MSG_QUEUE_SIZE, sizeof(can_msg_t), NULL);
+	can_inbound_queue =
+		osMessageQueueNew(CAN_MSG_QUEUE_SIZE, sizeof(can_msg_t), NULL);
+}
+
+/* Callback to be called when we get a CAN message */
+void can1_callback(CAN_HandleTypeDef *hcan)
+{
+	CAN_RxHeaderTypeDef rx_header;
+	can_msg_t new_msg;
+
+	/* Read in CAN message */
+	if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header,
+				 new_msg.data) != HAL_OK) {
+		printf("Failed to receive CAN message.\n");
+		return;
+	}
+
+	new_msg.len = rx_header.DLC;
+
+	if (rx_header.IDE == CAN_ID_EXT) {
+		// If the message has an extended CAN ID, save the message accordingly.
+		new_msg.id = rx_header.ExtId;
+		new_msg.id_is_extended = true;
+	} else {
+		// If the message has a standard CAN ID, save the message accordingly.
+		new_msg.id = rx_header.StdId;
+		new_msg.id_is_extended = false;
+	}
+
+	osMessageQueuePut(can_inbound_queue, &new_msg, 0U, 0U);
 }
 
 osThreadId_t can_dispatch_handle;
@@ -86,3 +138,31 @@ int8_t queue_can_msg(can_msg_t msg)
 	osMessageQueuePut(can_outbound_queue, &msg, 0U, 0U);
 	return 0;
 }
+
+osThreadId_t can_receive_thread;
+const osThreadAttr_t can_receive_attributes = {
+	.name = "CanProcessing",
+	.stack_size = 128 * 8,
+	.priority = (osPriority_t)osPriorityRealtime,
+};
+
+void vCanReceive(void *pv_params)
+{
+	can_msg_t msg;
+
+	for (;;) {
+		while (osOK ==
+		       osMessageQueueGet(can_inbound_queue, &msg, 0U, 0U)) {
+			switch (msg.id) {
+			case CANID_IMUZERO_BACKLEFT:
+			case CANID_IMUZERO_BACKRIGHT:
+			case CANID_IMUZERO_FRONTLEFT:
+			case CANID_IMUZERO_FRONTRIGHT:
+				imu_zero(msg.data[0], msg.data[1], msg.data[2]);
+				break;
+			default:
+				break;
+			}
+		}
+	}
+}
diff --git a/Core/Src/main.c b/Core/Src/main.c
index d93dac1..a65fe1f 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -233,6 +233,9 @@ int main(void)
   /* add queues, ... */
   can_dispatch_handle = osThreadNew(vCanDispatch, NULL, &can_dispatch_attributes);
   assert(can_dispatch_handle);
+
+  can_receive_thread = osThreadNew(vCanReceive, NULL, &can_receive_attributes);
+  assert(can_receive_thread);
   /* USER CODE END RTOS_QUEUES */
 
   /* Create the thread(s) */
diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c
index b3c1631..578fa6b 100644
--- a/Core/Src/monitor.c
+++ b/Core/Src/monitor.c
@@ -13,6 +13,8 @@
 #include "monitor.h"
 
 extern device_loc_t device_loc;
+extern float imu_rotation_data[3];
+extern float imu_zero_reference[3];
 
 uint16_t convert_can(uint16_t original_value, device_loc_t mode)
 {
@@ -184,9 +186,24 @@ void vIMUMonitor(void *pv_params)
 
 		process_motion_fx(&mFXInput, &mFXOutput, 0.05f);
 
-		orientation_data.yaw = (int16_t)mFXOutput.rotation[0];
-		orientation_data.pitch = (int16_t)mFXOutput.rotation[1];
-		orientation_data.roll = (int16_t)mFXOutput.rotation[2];
+		imu_rotation_data[0] = mFXOutput.rotation[0]; // Yaw
+		imu_rotation_data[1] = mFXOutput.rotation[1]; // Pitch
+		imu_rotation_data[2] = mFXOutput.rotation[2]; // Roll
+
+		/* Handle yaw zeroing */
+		float diff = mFXOutput.rotation[0] - imu_zero_reference[0];
+		if (diff < 0.0f) {
+			diff += 360.0f; // Make sure all yaw data is in the 0 to 360 degree range.
+		}
+		orientation_data.yaw = (int16_t)diff;
+
+		/* Handle pitch zeroing */
+		orientation_data.pitch = (int16_t)(mFXOutput.rotation[1] -
+						   imu_zero_reference[1]);
+
+		/* Handle roll zeroing */
+		orientation_data.roll = (int16_t)(mFXOutput.rotation[2] -
+						  imu_zero_reference[2]);
 
 #ifdef LOG_VERBOSE
 		printf("IMU Accel x: %d y: %d z: %d \r\n", accel_data.accel_x,
diff --git a/Core/Src/msb.c b/Core/Src/msb.c
index 149b926..2a89260 100644
--- a/Core/Src/msb.c
+++ b/Core/Src/msb.c
@@ -250,4 +250,27 @@ void process_motion_fx(MFX_input_t *data_in, MFX_output_t *data_out,
 
 	MotionFX_update(mFXState, data_out, data_in, &delta_time, NULL);
 }
+
+float imu_rotation_data[3] = {
+	0.0f, 0.0f, 0.0f
+}; /* Yaw, pitch, and roll. Updated by vIMUMonitor(). */
+float imu_zero_reference[3] = {
+	0.0f, 0.0f, 0.0f
+}; /* Reference values for zeroing IMU data. Defaults to 0 (i.e. no zeroing by default)*/
+void imu_zero(uint8_t yaw_byte, uint8_t pitch_byte, uint8_t roll_byte)
+{
+	/* Stores the current IMU rotation data in imu_zero_reference for selected axes only. */
+	if (yaw_byte > 0) {
+		imu_zero_reference[0] = imu_rotation_data[0];
+		printf("Zeroed yaw.\n");
+	}
+	if (pitch_byte > 0) {
+		imu_zero_reference[1] = imu_rotation_data[1];
+		printf("Zeroed pitch.\n");
+	}
+	if (roll_byte > 0) {
+		imu_zero_reference[2] = imu_rotation_data[2];
+		printf("Zeroed roll.\n");
+	}
+}
 #endif
\ No newline at end of file