diff --git a/src/main.cpp b/src/main.cpp
index 56f7334..958b6f9 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -11,8 +11,13 @@ void sendERPM(); // function to change the speed DON'T FORGET TO EDIT THE ID OF
 #define CAN_MC_TIMEOUT 100  /* Timeout of the invertors */
 
 /* Invertor CAN packet IDs */
-#define CAN_MC_ID_ERPM   0x03  /* Message to set ERPM */
-#define CAN_MC_ID_ENABLE 0x0C  /* Message to enable/disable drive */
+#define CAN_MC_ID_ERPM        0x03  /* Message to set ERPM */
+#define CAN_MC_ID_ENABLE      0x0C  /* Message to enable/disable drive */
+#define CAN_MC_ID_GEN_DATA_1  0x20  /* Message that contains ERPM, duty cycle and input voltage of drives */
+#define CAN_MC_ID_GEN_DATA_2  0x21  /* Message that contains AC and DC current of drives */
+#define CAN_MC_ID_GEN_DATA_3  0x22  /* Message that contains temperature and fault codes of drives */
+#define CAN_MC_ID_GEN_DATA_4  0x23  /* Message that contains FOC algorithm values of drives */
+#define CAN_MC_ID_GEN_DATA_5  0x24  /* Message that contains various signals of drives */
 
 /* CAN variables */
 FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> can_lv;  /* LV CAN bus is connected to controller 1 */
@@ -51,6 +56,58 @@ uint64_t time_last_can_mc_send_enable = 0;  /* Timestamp of last enable/disable
 uint64_t time_last_can_mc_send_erpm = 0;    /* Timestamp of last erpm message that was sent to motorcontrollers (to limit amount of messages) */
 #define CAN_MC_ERPM_PERIOD 25               /* Time between ERPM messages */
 
+/* Drive data (see datasheet or CAN manual) */
+int32_t mcl_erpm, mcr_erpm;
+int16_t mcl_duty, mcr_duty;
+int16_t mcl_voltage, mcr_voltage;
+int16_t mcl_ac_current, mcr_ac_current;
+int16_t mcl_dc_current, mcr_dc_current;
+int16_t mcl_ctrl_temp, mcr_ctrl_temp;
+int16_t mcl_motor_temp, mcr_motor_temp;
+char mcl_fault_code, mcr_fault_code;
+int32_t mcl_foc_id, mcr_foc_id;
+int32_t mcl_foc_iq, mcr_foc_iq;
+int8_t mcl_throttle, mcr_throttle;
+int8_t mcl_brake, mcr_brake;
+char mcl_gpio, mcr_gpio;
+char mcl_enabled, mcr_enabled;
+char mcl_limits_1, mcr_limits_1;
+char mcl_limits_2, mcr_limits_2;
+uint8_t mcl_can_version, mcr_can_version;
+uint32_t time_last_mcl_data = 0, time_last_mcr_data = 0;
+
+/* Fault IDs (used in 2nd data field of Critical Fault message on LV CAN) */
+#define CAN_FAULT_ID              0x05  /* Fault ID for D0 field of Critical Fault message, signifies that this is a TC fault */
+#define FAULT_EXTERNAL_CRITICAL   0x00  /* CAN critical fault from other LVS module, will not be transmitted */
+#define FAULT_INV_OVERVOLT        0x01  /* Fault from invertor: overvoltage */
+#define FAULT_INV_UNDERVOLT       0x02  /* Fault from invertor: undervoltage */
+#define FAULT_INV_DRV             0x03  /* Fault from invertor: transistor or drive error */
+#define FAULT_INV_OVERCURRENT     0x04  /* Fault from invertor: ABS overcurrent */
+#define FAULT_INV_CTLR_OVERTEMP   0x05  /* Fault from invertor: controller overtemp */
+#define FAULT_INV_MOTOR_OVERTEMP  0x06  /* Fault from invertor: motor overtemp */
+#define FAULT_INV_SENS_WIRE       0x07  /* Fault from invertor: sensor wire fault */
+#define FAULT_INV_SENS_GENERAL    0x08  /* Fault from invertor: sensor general fault */
+#define FAULT_INV_CAN_ERROR       0x09  /* Fault from invertor: CAN command error */
+#define FAULT_INV_ANALOG_INPUT    0x0A  /* Fault from invertor: analog input error */
+#define FAULT_RTD_TIMEOUT         0x11  /* Did not receive RTD state in short enough time, assume data loss */
+#define FAULT_APPS_BPPS_TIMEOUT   0x12  /* Did not receive APPS/BPPS values in short enough time, assume data loss */
+
+/* SAFETY */
+void enter_safe_state(char fault_id) {
+	/* Set all state variables to safe value */
+	tc_state = TC_SAFE_STATE;
+	rtd_state = RTD_INACTIVE;
+
+	/* Send information to LVS */
+	if (fault_id == FAULT_EXTERNAL_CRITICAL) return; /* Do not resend fault that is not ours */
+	CAN_message_t m;
+	m.id = CAN_ID_CRITICAL_FAULT | CAN_MODULE_ID;
+	m.buf[0] = CAN_FAULT_ID;
+	m.buf[1] = fault_id;
+	can_lv.write(m);
+}
+
+/* CAN */
 void handle_can_lv_message(const CAN_message_t &msg) {
 	/* Callback for LV CAN messages */
 
@@ -74,6 +131,89 @@ void handle_can_lv_message(const CAN_message_t &msg) {
 
 void handle_can_mc_message(const CAN_message_t &msg) {
 	/* Callback for MC CAN messages */
+
+	uint8_t node_id = (msg.id & 0xff);
+	uint8_t packet_id = ((msg.id >> 8) & 0xff);
+	if (node_id == MC_L_ID) {
+		/* Left invertor */
+		if (packet_id == CAN_MC_ID_GEN_DATA_1) {
+			/* Parse data */
+			mcl_erpm = (msg.buf[0] << 24) | (msg.buf[1] << 16) | (msg.buf[2] << 8) | (msg.buf[3]);
+			mcl_duty = (msg.buf[4] << 8) | (msg.buf[5]);
+			mcl_voltage = (msg.buf[6] << 8) | (msg.buf[7]);
+			time_last_mcl_data = millis();
+		}
+		else if (packet_id == CAN_MC_ID_GEN_DATA_2) {
+			/* Parse data */
+			mcl_ac_current = (msg.buf[0] << 8) | (msg.buf[1]);
+			mcl_dc_current = (msg.buf[2] << 8) | (msg.buf[3]);
+			time_last_mcl_data = millis();
+		}
+		else if (packet_id == CAN_MC_ID_GEN_DATA_3) {
+			/* Parse data */
+			mcl_ctrl_temp = (msg.buf[0] << 8) | (msg.buf[1]);
+			mcl_motor_temp = (msg.buf[2] << 8) | (msg.buf[3]);
+			mcl_fault_code = msg.buf[4];
+			time_last_mcl_data = millis();
+		}
+		else if (packet_id == CAN_MC_ID_GEN_DATA_4) {
+			/* Parse data */
+			mcl_foc_id = (msg.buf[0] << 24) | (msg.buf[1] << 16) | (msg.buf[2] << 8) | (msg.buf[3]);
+			mcl_foc_iq = (msg.buf[4] << 24) | (msg.buf[5] << 16) | (msg.buf[6] << 8) | (msg.buf[7]);
+			time_last_mcl_data = millis();
+		}
+		else if (packet_id == CAN_MC_ID_GEN_DATA_5) {
+			/* Parse data */
+			mcl_throttle = msg.buf[0];
+			mcl_brake = msg.buf[1];
+			mcl_gpio = msg.buf[2];
+			mcl_enabled = msg.buf[3];
+			mcl_limits_1 = msg.buf[4];
+			mcl_limits_2 = msg.buf[5];
+			mcl_can_version = msg.buf[7];
+			time_last_mcl_data = millis();
+		}
+	}
+	else if (node_id == MC_R_ID) {
+		/* Right invertor */
+		if (packet_id == CAN_MC_ID_GEN_DATA_1) {
+			/* Parse data */
+			mcr_erpm = (msg.buf[0] << 24) | (msg.buf[1] << 16) | (msg.buf[2] << 8) | (msg.buf[3]);
+			mcr_duty = (msg.buf[4] << 8) | (msg.buf[5]);
+			mcr_voltage = (msg.buf[6] << 8) | (msg.buf[7]);
+			time_last_mcr_data = millis();
+		}
+		else if (packet_id == CAN_MC_ID_GEN_DATA_2) {
+			/* Parse data */
+			mcr_ac_current = (msg.buf[0] << 8) | (msg.buf[1]);
+			mcr_dc_current = (msg.buf[2] << 8) | (msg.buf[3]);
+			time_last_mcr_data = millis();
+		}
+		else if (packet_id == CAN_MC_ID_GEN_DATA_3) {
+			/* Parse data */
+			mcr_ctrl_temp = (msg.buf[0] << 8) | (msg.buf[1]);
+			mcr_motor_temp = (msg.buf[2] << 8) | (msg.buf[3]);
+			mcr_fault_code = msg.buf[4];
+			time_last_mcr_data = millis();
+		}
+		else if (packet_id == CAN_MC_ID_GEN_DATA_4) {
+			/* Parse data */
+			mcr_foc_id = (msg.buf[0] << 24) | (msg.buf[1] << 16) | (msg.buf[2] << 8) | (msg.buf[3]);
+			mcr_foc_iq = (msg.buf[4] << 24) | (msg.buf[5] << 16) | (msg.buf[6] << 8) | (msg.buf[7]);
+			time_last_mcr_data = millis();
+		}
+		else if (packet_id == CAN_MC_ID_GEN_DATA_5) {
+			/* Parse data */
+			mcr_throttle = msg.buf[0];
+			mcr_brake = msg.buf[1];
+			mcr_gpio = msg.buf[2];
+			mcr_enabled = msg.buf[3];
+			mcr_limits_1 = msg.buf[4];
+			mcr_limits_2 = msg.buf[5];
+			mcr_can_version = msg.buf[7];
+			time_last_mcr_data = millis();
+		}
+	}
 }
 
 
@@ -197,5 +337,7 @@ void loop() {
 	can_mc.events();    /* Handle CAN MC events */
 	check_heartbeat();  /* Do heartbeat stuffz */
 	update_drives();    /* Do motorcontroller shit */
+
+	/* TODO: check drive faults */
 }