diff --git a/CCU/Core/Inc/stm32g4xx_it.h b/CCU/Core/Inc/stm32g4xx_it.h
index 79c4e4fc..0e522cc6 100644
--- a/CCU/Core/Inc/stm32g4xx_it.h
+++ b/CCU/Core/Inc/stm32g4xx_it.h
@@ -55,8 +55,6 @@ void SVC_Handler(void);
 void DebugMon_Handler(void);
 void PendSV_Handler(void);
 void SysTick_Handler(void);
-void FDCAN1_IT0_IRQHandler(void);
-void FDCAN2_IT0_IRQHandler(void);
 /* USER CODE BEGIN EFP */
 
 /* USER CODE END EFP */
diff --git a/CCU/Core/Src/stm32g4xx_it.c b/CCU/Core/Src/stm32g4xx_it.c
index 115420fa..9b1037e6 100644
--- a/CCU/Core/Src/stm32g4xx_it.c
+++ b/CCU/Core/Src/stm32g4xx_it.c
@@ -187,41 +187,6 @@ void SysTick_Handler(void)
 	/* USER CODE END SysTick_IRQn 1 */
 }
 
-/******************************************************************************/
-/* STM32G4xx Peripheral Interrupt Handlers                                    */
-/* Add here the Interrupt Handlers for the used peripherals.                  */
-/* For the available peripheral interrupt handler names,                      */
-/* please refer to the startup file (startup_stm32g4xx.s).                    */
-/******************************************************************************/
-
-/**
- * @brief This function handles FDCAN1 interrupt 0.
- */
-void FDCAN1_IT0_IRQHandler(void)
-{
-	/* USER CODE BEGIN FDCAN1_IT0_IRQn 0 */
-
-	/* USER CODE END FDCAN1_IT0_IRQn 0 */
-	HAL_FDCAN_IRQHandler(&hfdcan1);
-	/* USER CODE BEGIN FDCAN1_IT0_IRQn 1 */
-
-	/* USER CODE END FDCAN1_IT0_IRQn 1 */
-}
-
-/**
- * @brief This function handles FDCAN2 interrupt 0.
- */
-void FDCAN2_IT0_IRQHandler(void)
-{
-	/* USER CODE BEGIN FDCAN2_IT0_IRQn 0 */
-
-	/* USER CODE END FDCAN2_IT0_IRQn 0 */
-	HAL_FDCAN_IRQHandler(&hfdcan2);
-	/* USER CODE BEGIN FDCAN2_IT0_IRQn 1 */
-
-	/* USER CODE END FDCAN2_IT0_IRQn 1 */
-}
-
 /* USER CODE BEGIN 1 */
 
 /* USER CODE END 1 */
diff --git a/ECU/Application/Inc/CANutils.h b/ECU/Application/Inc/CANutils.h
new file mode 100644
index 00000000..f5195fa8
--- /dev/null
+++ b/ECU/Application/Inc/CANutils.h
@@ -0,0 +1,15 @@
+#include <stdint.h>
+
+#include "GR_OLD_BUS_ID.h"
+#include "GR_OLD_MSG_ID.h"
+#include "GR_OLD_NODE_ID.h"
+#include "StateData.h"
+
+#ifndef CANUTILS_H
+#define CANUTILS_H
+
+#define ECU_STATE_DATA_SEND_INTERVAL_MS 250
+void ECU_CAN_Send(GR_OLD_BUS_ID bus, GR_OLD_NODE_ID destNode, GR_OLD_MSG_ID messageID, void *data, uint32_t size);
+void SendECUStateDataOverCAN(ECU_StateData *stateData);
+
+#endif
diff --git a/ECU/Application/Inc/StateData.h b/ECU/Application/Inc/StateData.h
index 5094af7e..df956a43 100644
--- a/ECU/Application/Inc/StateData.h
+++ b/ECU/Application/Inc/StateData.h
@@ -18,32 +18,93 @@
  * @remark It is passed to each state function on tick to allow state-specific
  * logic to access and modify the ECU's operational data.
  */
-typedef struct ECU_StateData {
-	// DON'T TOUCH YET
-	GR_OLD_ECU_STATUS_1_MSG ecuStatus1;
-	GR_OLD_ECU_STATUS_2_MSG ecuStatus2;
-	GR_OLD_ECU_STATUS_3_MSG ecuStatus3;
+
+#define SAFE_VOLTAGE_LIMIT 60
+#define BUTTON_REFRESH_RATE_MS 100
+
+typedef union {
+	struct {
+		uint8_t ECUState;
+		uint8_t StatusBits[3];
+		uint8_t PowerLevelTorqueMap;
+		uint8_t MaxCellTemp;
+		uint8_t AccumulatorStateOfCharge;
+		uint8_t GLVStateOfCharge;
+		uint16_t TractiveSystemVoltage;
+		uint16_t VehicleSpeed;
+		uint16_t FRWheelRPM;
+		uint16_t FLWheelRPM;
+		uint16_t RRWheelRPM;
+		uint16_t RLWheelRPM;
+	};
+
+	struct {
+		uint8_t ECUStatusMsgOne[8];
+		uint8_t ECUStatusMsgTwo[8];
+		uint8_t ECUStatusMsgThree[4];
+	};
+} ECU_StateDataToSend;
+
+typedef volatile struct ECU_StateData {
+
+	// TODO: Remove unneeded states
+
+	uint32_t millisSinceBoot;
 
 	int32_t dischargeStartMillis;
-	uint32_t lastECUStatusMsgTick;
+	uint32_t lastECUStatusMsgMillis;
 	uint32_t lastTSSIFlash;
 	int32_t last_drive_active_control_ms;
+
 	float min_amk_heat_cap_throttle_percent;
+	float ts_voltage;
+	float max_cell_temp; /** Temperature of hottest cell, celsius */
+
+	float vehicle_speed; /** Vehicle speed, MPH */
+	float fr_wheel_rpm;  /** FR wheel, RPM */
+	float fl_wheel_rpm;  /** FL wheel, RPM */
+	float rr_wheel_rpm;  /** RRv wheel, RPM */
+	float rl_wheel_rpm;  /** RL wheel, RPM */
+
+	// 0.5V when things go to shit (X_OK low) (BAD)
+	// 3V when things almost poggers (X_OK high but SDC not reset) (BAD)
+	// 2.4V when things are actually poggers (X_OK high and SDC is not triggered)
+	float ams_sense;
+	float imd_sense;
+	float bspd_sense;
+
+	float estop_sense;
 	uint16_t driving_heat_capacity_1;
 	uint16_t driving_heat_capacity_2;
 	uint16_t APPS1_Signal;
 	uint16_t APPS2_Signal;
 	uint16_t Brake_R_Signal;
 	uint16_t Brake_F_Signal;
-	uint8_t acu_error_warning_bits;
+	uint16_t STEERING_ANGLE_SIGNAL;
+	uint8_t status_bits[3];
+	int8_t ping_block[3];	      /** Node timeout status bits (1=OK, 0=Timeout) */
+	uint8_t powerlevel_torquemap; /** Power lvl (4b) & torque map (4b) */
+	uint8_t tractivebattery_soc;
+	uint8_t glv_soc;
+	uint8_t bcu_error_warning_bits;
 	uint8_t inverter_fault_map;
-	bool bse_apps_violation;
-	bool ts_active_button_engaged;
-	bool rtd_button_engaged;
-} ECU_StateData; // FIXME Add comments to each data field with descriptions and
-		 // rules (eg -1 = invalid?, etc)
-		 // Will also need to add information from ADC into this struct
-		 // --- such as the APPS and Brake signals after doing smoothing
-		 // and whatnot to get the values sane
+	bool ts_active;
+	bool rtd;
+	bool prev_ts_active_button_state;
+	bool prev_rtd_button_state;
+	uint32_t prev_ts_press_millis;
+	uint32_t prev_rtd_press_millis;
+	bool ir_plus;
+	bool ir_minus;
+	bool bcu_software_latch;
+
+	bool bms_light;
+	bool imd_light;
+	bool tssi_red;
+
+	uint32_t tssi_red_blinking_current_cycle_starting_millis;
+
+	GR_ECU_State ecu_state;
+} ECU_StateData;
 
 #endif
diff --git a/ECU/Application/Inc/StateTicks.h b/ECU/Application/Inc/StateTicks.h
index 6844ceb8..e1f1c194 100644
--- a/ECU/Application/Inc/StateTicks.h
+++ b/ECU/Application/Inc/StateTicks.h
@@ -1,9 +1,14 @@
 #include "StateData.h"
 #include "StateMachine.h"
+#include "adc.h"
+#include "can.h"
 
 #ifndef _STATE_TICKS_H_
 #define _STATE_TICKS_H_
 
+extern CANHandle *primary_can;
+extern CANHandle *data_can;
+
 /**
  * @brief Tick function for the ECU state machine.
  *
@@ -35,6 +40,17 @@ void ECU_GLV_Off(ECU_StateData *stateData);
  */
 void ECU_GLV_On(ECU_StateData *stateData);
 
+/**
+ * @brief Handles the transition from GLV On to Precharge Engaged state.
+ *
+ * Initiates the precharge process by switching to the Precharge Engaged state.
+ *
+ * @param stateData Pointer to the ECU state data structure.
+ *
+ * @return void
+ */
+void ECU_Precharge_Start(ECU_StateData *stateData);
+
 /**
  * @brief State handler for the Precharge Engaged state.
  *
@@ -68,6 +84,18 @@ void ECU_Precharge_Complete(ECU_StateData *stateData);
  */
 void ECU_Drive_Active(ECU_StateData *stateData);
 
+/**
+ * @brief Init function for ECU_Tractive_System_Discharge_Start.
+ *
+ * Resets Tractive System discharge timer and switches on the Tractive System
+ * Discharge state.
+ *
+ * @param stateData Pointer to the ECU state data structure.
+ *
+ * @return void
+ */
+void ECU_Tractive_System_Discharge_Start(ECU_StateData *stateData);
+
 /**
  * @brief State handler for the Tractive System Discharge state.
  *
diff --git a/ECU/Application/Inc/StateUtils.h b/ECU/Application/Inc/StateUtils.h
index 7dcfeedf..3e79cc45 100644
--- a/ECU/Application/Inc/StateUtils.h
+++ b/ECU/Application/Inc/StateUtils.h
@@ -6,6 +6,10 @@
 #ifndef _STATE_UTILS_H_
 #define _STATE_UTILS_H_
 
+/// @brief Get the current time in milliseconds since system start
+/// @return Current time in milliseconds
+uint32_t MillisecondsSinceBoot(void);
+
 // Constants
 #define BRAKE_F_MIN 0	    // TODO: need to be determined FIXME: Rename better
 #define BRAKE_F_MAX 4095    // TODO: need to be determined FIXME: Rename better
@@ -19,12 +23,20 @@
 #define APPS_PROPORTION 2.0f // TODO: Need to be experimentally determined
 #define APPS_OFFSET 250.0f   // TODO: Need to be experimentally determined
 
+#define MAX_CURRENT_AMPS 42.0f	       // TODO: Change as appropriate
+#define MAX_REVERSE_CURRENT_AMPS 20.0f // TODO: Change as appropriate
+
+void setSoftwareLatch(bool close);
+
 // Checks stateData for critical errors
-bool CriticalError(const ECU_StateData *stateData);
-bool CommunicationError(const ECU_StateData *stateData);
-bool APPS_BSE_Violation(const ECU_StateData *stateData);
-bool PressingBrake(const ECU_StateData *stateData);
-float CalcBrakePercent(const ECU_StateData *stateData);
-float CalcPedalTravel(const ECU_StateData *stateData);
+bool CriticalError(volatile const ECU_StateData *stateData);
+bool bmsFailure(volatile const ECU_StateData *stateData);
+bool imdFailure(volatile const ECU_StateData *stateData);
+bool CommunicationError(volatile const ECU_StateData *stateData);
+bool APPS_BSE_Violation(volatile const ECU_StateData *stateData);
+bool PressingBrake(volatile const ECU_StateData *stateData);
+float CalcBrakePercent(volatile const ECU_StateData *stateData);
+float CalcPedalTravel(volatile const ECU_StateData *stateData);
+bool vehicle_is_moving(volatile const ECU_StateData *stateData);
 
 #endif
diff --git a/ECU/Application/Src/CANdler.c b/ECU/Application/Src/CANdler.c
index c60608e8..c6358bc7 100644
--- a/ECU/Application/Src/CANdler.c
+++ b/ECU/Application/Src/CANdler.c
@@ -7,6 +7,9 @@
 #include "GR_OLD_NODE_ID.h"
 #include "Logomatic.h"
 #include "StateData.h"
+#include "bitManipulations.h"
+
+extern ECU_StateData stateLump;
 
 void ReportBadMessageLength(GR_OLD_BUS_ID bus_id, GR_OLD_MSG_ID msg_id, GR_OLD_NODE_ID sender_id)
 {
@@ -31,6 +34,7 @@ void ECU_CAN_MessageHandler(ECU_StateData *state_data, GR_OLD_BUS_ID bus_id, GR_
 			}
 			LOGOMATIC("Received from %02X on bus %d: %.*s\n", sender_id, bus_id, (int)data_length, data);
 			break;
+
 		case MSG_DEBUG_FD:
 			if (data_length > sizeof(GR_OLD_DEBUG_2_MSG)) {
 				ReportBadMessageLength(bus_id, msg_id, sender_id);
@@ -38,6 +42,7 @@ void ECU_CAN_MessageHandler(ECU_StateData *state_data, GR_OLD_BUS_ID bus_id, GR_
 			}
 			LOGOMATIC("Received from %02X on bus %d: %.*s\n", sender_id, bus_id, (int)data_length, data);
 			break;
+
 		case MSG_PING:
 			if (data_length != sizeof(GR_OLD_PING_MSG)) {
 				ReportBadMessageLength(bus_id, msg_id, sender_id);
@@ -45,33 +50,39 @@ void ECU_CAN_MessageHandler(ECU_StateData *state_data, GR_OLD_BUS_ID bus_id, GR_
 			}
 			// TODO See Issue #143
 			break;
-		case MSG_ACU_STATUS_1:
+
+		case MSG_BCU_STATUS_1:
 			if (data_length != sizeof(GR_OLD_BCU_STATUS_1_MSG)) {
 				ReportBadMessageLength(bus_id, msg_id, sender_id);
 				break;
 			}
 			GR_OLD_BCU_STATUS_1_MSG *bcu_status_1 = (GR_OLD_BCU_STATUS_1_MSG *)data;
-			state_data->ecuStatus1.tractivebattery_soc = bcu_status_1->tractivebattery_soc;
-			state_data->ecuStatus1.glv_soc = bcu_status_1->glv_soc;
-			state_data->ecuStatus2.ts_voltage = bcu_status_1->ts_voltage;
+			state_data->tractivebattery_soc = bcu_status_1->tractivebattery_soc * 0.01;
+			state_data->glv_soc = bcu_status_1->glv_soc * 20 / 51;
+			state_data->ts_voltage = bcu_status_1->ts_voltage * 0.01;
 			break;
-		case MSG_ACU_STATUS_2:
+
+		case MSG_BCU_STATUS_2:
 			if (data_length != sizeof(GR_OLD_BCU_STATUS_2_MSG)) {
 				ReportBadMessageLength(bus_id, msg_id, sender_id);
 				break;
 			}
 			GR_OLD_BCU_STATUS_2_MSG *bcu_status_2 = (GR_OLD_BCU_STATUS_2_MSG *)data;
-			state_data->ecuStatus1.max_cell_temp = bcu_status_2->max_cell_temp;
-			state_data->acu_error_warning_bits = bcu_status_2->error_bits;
+			state_data->max_cell_temp = bcu_status_2->max_cell_temp * 0.25;
+			state_data->bcu_error_warning_bits = bcu_status_2->error_bits;
+			state_data->ir_minus = GETBIT(bcu_status_2->precharge_bits, 1);
+			state_data->ir_plus = GETBIT(bcu_status_2->precharge_bits, 2);
+			state_data->bcu_software_latch = GETBIT(bcu_status_2->precharge_bits, 3);
 			break;
+
 		case MSG_INVERTER_STATUS_1:
 			if (data_length != sizeof(GR_OLD_INVERTER_STATUS_1_MSG)) {
 				ReportBadMessageLength(bus_id, msg_id, sender_id);
 				break;
 			}
 			GR_OLD_INVERTER_STATUS_1_MSG *inverter_status_1 = (GR_OLD_INVERTER_STATUS_1_MSG *)data;
-			state_data->ecuStatus3.rl_wheel_rpm = inverter_status_1->motor_rpm;
-			state_data->ecuStatus3.rr_wheel_rpm = inverter_status_1->motor_rpm;
+			state_data->rl_wheel_rpm = inverter_status_1->motor_rpm - 32768;
+			state_data->rr_wheel_rpm = inverter_status_1->motor_rpm - 32768;
 			break;
 		case MSG_INVERTER_STATUS_3:
 			if (data_length != sizeof(GR_OLD_INVERTER_STATUS_3_MSG)) {
@@ -81,22 +92,13 @@ void ECU_CAN_MessageHandler(ECU_StateData *state_data, GR_OLD_BUS_ID bus_id, GR_
 			GR_OLD_INVERTER_STATUS_3_MSG *inverter_status_3 = (GR_OLD_INVERTER_STATUS_3_MSG *)data;
 			state_data->inverter_fault_map = inverter_status_3->fault_bits;
 			break;
-		case MSG_DASH_STATUS:
-			if (data_length != sizeof(GR_OLD_DASH_STATUS_MSG)) {
-				ReportBadMessageLength(bus_id, msg_id, sender_id);
-				break;
-			}
-			GR_OLD_DASH_STATUS_MSG *dash_status = (GR_OLD_DASH_STATUS_MSG *)data;
-			state_data->ts_active_button_engaged = dash_status->ts_button;
-			state_data->rtd_button_engaged = dash_status->rtd_button;
-			break;
 		case MSG_STEERING_STATUS:
 			if (data_length != sizeof(GR_OLD_STEERING_STATUS_MSG)) {
 				ReportBadMessageLength(bus_id, msg_id, sender_id);
 				break;
 			}
 			GR_OLD_STEERING_STATUS_MSG *steering_status = (GR_OLD_STEERING_STATUS_MSG *)data;
-			state_data->ecuStatus1.powerlevel_torquemap = steering_status->encoder_bits;
+			state_data->powerlevel_torquemap = steering_status->encoder_bits;
 			break;
 		default:
 			ReportUnhandledMessage(bus_id, msg_id, sender_id);
diff --git a/ECU/Application/Src/CANutils.c b/ECU/Application/Src/CANutils.c
new file mode 100644
index 00000000..73ca2d78
--- /dev/null
+++ b/ECU/Application/Src/CANutils.c
@@ -0,0 +1,118 @@
+#include "CANutils.h"
+
+#include "GR_OLD_BUS_ID.h"
+#include "GR_OLD_MSG_DAT.h"
+#include "GR_OLD_MSG_ID.h"
+#include "Logomatic.h"
+#include "StateData.h"
+#include "StateTicks.h"
+#include "StateUtils.h"
+#include "can.h"
+#include "main.h"
+#include "string.h"
+
+uint32_t lastTickECUStateDataSent = 0;
+
+void ECU_CAN_Send(GR_OLD_BUS_ID bus, GR_OLD_NODE_ID destNode, GR_OLD_MSG_ID messageID, void *data, uint32_t size)
+{
+	if (size > FDCAN_MAX_DATA_BYTES) {
+		size = FDCAN_MAX_DATA_BYTES;
+		LOGOMATIC("Tried to send more than 64 bytes over CAN");
+	}
+
+	uint32_t ID = ((0xFF & LOCAL_GR_ID) << 20) & ((0xFFF & messageID) << 8) & (0xFF & destNode);
+
+	FDCAN_TxHeaderTypeDef header = {
+	    .Identifier = ID,
+	    .IdType = FDCAN_STANDARD_ID,
+	    .TxFrameType = FDCAN_DATA_FRAME,
+	    .ErrorStateIndicator = FDCAN_ESI_ACTIVE,
+	    .DataLength = size,
+	    .BitRateSwitch = FDCAN_BRS_OFF,
+	    .TxEventFifoControl = FDCAN_NO_TX_EVENTS,
+	    .MessageMarker = 0,
+	};
+
+	FDCANTxMessage msg = {0};
+	msg.tx_header = header;
+
+	memcpy(&(msg.data), data, size);
+
+	switch (bus) {
+		case GR_OLD_BUS_PRIMARY:
+			can_send(primary_can, &msg);
+			break;
+		case GR_OLD_BUS_DATA:
+			can_send(data_can, &msg);
+			break;
+		default:
+			LOGOMATIC("CAN: Invalid bus ID %d\n", bus);
+			break;
+	}
+}
+
+// TODO: If you try to send anything but control messages, you are cooked buddy
+// Doesn't actually use Motorola order for multiple fields, just sends the bytes in reverse order
+/*void ECU_Write_DTI(uint16_t msgID, uint8_t data[], uint32_t length)
+{
+	if ((MSG_DTI_CONTROL_10 & 0xFF) != 0x16) {
+		LOGOMATIC("NOT A DTI MESSAGE");
+	}
+
+	FDCAN_TxHeaderTypeDef TxHeader = {
+    .IdType = FDCAN_EXTENDED_ID,
+    .TxFrameType = FDCAN_DATA_FRAME,
+    .ErrorStateIndicator = FDCAN_ESI_ACTIVE, // honestly this might be a value you have to read from a node
+	// FDCAN_ESI_ACTIVE is just a state that assumes there are minimal errors
+    .BitRateSwitch = FDCAN_BRS_OFF,
+    .TxEventFifoControl = FDCAN_NO_TX_EVENTS, // change to FDCAN_STORE_TX_EVENTS if you need to store info regarding transmitted messages
+    .MessageMarker = 0 // also change this to a real address if you change fifo control
+	};
+
+    TxHeader.Identifier = msgID;
+    TxHeader.DataLength = length;
+
+    TxHeader.IdType = FDCAN_EXTENDED_ID;
+
+    TxHeader.FDFormat = FDCAN_CLASSIC_CAN;
+
+    uint8_t temp;
+    for(uint16_t i = 0; i < length / 2; ++i)
+    {
+	temp = data[i];
+	data[i] = data[length - i - 1];
+	data[length - i - 1] = temp;
+    }
+
+	can_send(primary_can, &msg);
+}*/
+
+void SendECUStateDataOverCAN(ECU_StateData *stateData)
+{
+	uint32_t currentTime = MillisecondsSinceBoot();
+
+	if (lastTickECUStateDataSent > currentTime - ECU_STATE_DATA_SEND_INTERVAL_MS) {
+		return;
+	}
+
+	lastTickECUStateDataSent = currentTime;
+
+	ECU_StateDataToSend messages = {.ECUState = stateData->ecu_state,
+					.StatusBits = {stateData->status_bits[0], stateData->status_bits[1], stateData->status_bits[2]},
+					.PowerLevelTorqueMap = stateData->powerlevel_torquemap,
+					.MaxCellTemp = (uint8_t)(stateData->max_cell_temp * 4),
+					.AccumulatorStateOfCharge = (uint8_t)(stateData->tractivebattery_soc * 51 / 20),
+					.GLVStateOfCharge = (uint8_t)(stateData->glv_soc * 51 / 20),
+					.TractiveSystemVoltage = (uint16_t)(stateData->ts_voltage * 100),
+					.VehicleSpeed = (uint16_t)(stateData->vehicle_speed * 100),
+					.FRWheelRPM = (uint16_t)(stateData->fr_wheel_rpm * 10 + 32768),
+					.FLWheelRPM = (uint16_t)(stateData->fl_wheel_rpm * 10 + 32768),
+					.RRWheelRPM = (uint16_t)(stateData->rr_wheel_rpm * 10 + 32768),
+					.RLWheelRPM = (uint16_t)(stateData->rl_wheel_rpm * 10 + 32768)};
+
+	LOGOMATIC("Sending ECU State Data over CAN");
+
+	ECU_CAN_Send(GR_OLD_BUS_PRIMARY, GR_ALL, MSG_ECU_STATUS_1, (void *)&messages.ECUStatusMsgOne, sizeof(messages.ECUStatusMsgOne));
+	ECU_CAN_Send(GR_OLD_BUS_PRIMARY, GR_ALL, MSG_ECU_STATUS_2, (void *)&messages.ECUStatusMsgTwo, sizeof(messages.ECUStatusMsgTwo));
+	ECU_CAN_Send(GR_OLD_BUS_PRIMARY, GR_ALL, MSG_ECU_STATUS_3, (void *)&messages.ECUStatusMsgThree, sizeof(messages.ECUStatusMsgThree));
+}
diff --git a/ECU/Application/Src/StateTicks.c b/ECU/Application/Src/StateTicks.c
index a8cf0738..bd2537fb 100644
--- a/ECU/Application/Src/StateTicks.c
+++ b/ECU/Application/Src/StateTicks.c
@@ -1,10 +1,17 @@
 #include "StateTicks.h"
 
+#include "CANutils.h"
+#include "GR_OLD_BUS_ID.h"
+#include "GR_OLD_MSG_DAT.h"
+#include "GR_OLD_MSG_ID.h"
+#include "GR_OLD_NODE_ID.h"
 #include "Logomatic.h"
 #include "StateData.h"
 #include "StateMachine.h"
 #include "StateUtils.h"
-#include "Unused.h"
+#include "adc.h"
+#include "can.h"
+#include "stm32g4xx_ll_gpio.h"
 
 /**
  * @brief The ECU state data lump.
@@ -14,13 +21,43 @@
  *
  * @remark Intentionally not a globally accessible variable
  */
+
+CANHandle *primary_can;
+CANHandle *data_can;
+
 ECU_StateData stateLump = {0};
 
-void ECU_State_Tick(void)
+#define ECU_STATUS_MSG_PERIOD_MILLIS (1000)
+// EV.5.6.3: The Discharge Circuit must be designed to handle the maximum Tractive System voltage for minimum 15 seconds
+#define TRACTIVE_SYSTEM_MAX_PERMITTED_DISCHARGE_TIME_MILLIS (15000)
+
+void ECU_State_Tick()
 {
-	LOGOMATIC("ECU Current State: %d\n", stateLump.ecuStatus1.ecu_status);
+	stateLump.millisSinceBoot = MillisecondsSinceBoot();
+
+	if (stateLump.millisSinceBoot - stateLump.lastECUStatusMsgMillis >= ECU_STATUS_MSG_PERIOD_MILLIS) {
+		LOGOMATIC("ECU Current State: %d\n", stateLump.ecu_state);
+		stateLump.lastECUStatusMsgMillis = stateLump.millisSinceBoot;
+	}
+
+	if (bmsFailure(&stateLump) || imdFailure(&stateLump)) {
+		stateLump.tssi_red = true;
+	}
 
-	switch (stateLump.ecuStatus1.ecu_status) {
+	// EV.5.11.5: Flash, 2 Hz to 5 Hz, 50% duty cycle
+	//     Here we chose a period of 350ms
+	if (stateLump.tssi_red) {
+		LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_5);
+		if (stateLump.millisSinceBoot - stateLump.tssi_red_blinking_current_cycle_starting_millis < 175) {
+			LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_6);
+		} else if (stateLump.millisSinceBoot - stateLump.tssi_red_blinking_current_cycle_starting_millis < 350) {
+			LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_6);
+		} else {
+			stateLump.tssi_red_blinking_current_cycle_starting_millis = stateLump.millisSinceBoot;
+		}
+	}
+
+	switch (stateLump.ecu_state) {
 		case GR_GLV_OFF:
 			ECU_GLV_Off(&stateLump);
 			break;
@@ -40,9 +77,9 @@ void ECU_State_Tick(void)
 			ECU_Tractive_System_Discharge(&stateLump);
 			break;
 		default:
-			LOGOMATIC("ECU Current State Unknown: %d\n", stateLump.ecuStatus1.ecu_status);
+			LOGOMATIC("ECU Current State Unknown: %d\n", stateLump.ecu_state);
 			LOGOMATIC("ECU: Resetting to GLV On\n");
-			stateLump.ecuStatus1.ecu_status = GR_GLV_ON;
+			stateLump.ecu_state = GR_GLV_ON;
 			break;
 	}
 }
@@ -57,75 +94,77 @@ transitioning state
 void ECU_GLV_Off(ECU_StateData *stateData)
 {
 	UNUSED(stateData);
-	// TODO Implement functionality
-	// ERROR --> GLV_OFF should never be reached
+	LOGOMATIC("ECU_GLV_Off state reached... this should never happen!");
+	setSoftwareLatch(0); // TODO: need???
+			     // TODO ERROR --> GLV_OFF should never be reached
 }
 
 void ECU_GLV_On(ECU_StateData *stateData)
 {
-	UNUSED(stateData);
-	/*
-	if(stateData->TractiveSystemVoltage >= 60){ // should never happen but
-	has to be accounted for stateData->currentState = GR_TS_DISCHARGE;
-		emit an error
-		break;
+	LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_5);
+	LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_6);
+
+	if (stateData->ts_voltage >= SAFE_VOLTAGE_LIMIT) {
+		ECU_Tractive_System_Discharge_Start(stateData);
+		LOGOMATIC("Error: TS Voltage >= 60!");
+		return;
 	}
-	*/
 
-	// TODO Implement functionality
-	if (stateData->ts_active_button_engaged) {
-		stateData->ecuStatus1.ecu_status = GR_PRECHARGE_ENGAGED;
+	if (stateData->ts_active /* && stateData->ir_plus*/) { // TOOD Talk to Owen if this is correct for precharge start confirmation
+		ECU_Precharge_Start(stateData);
+		return;
 	}
 }
 
+void ECU_Precharge_Start(ECU_StateData *stateData)
+{
+	/*send message to BCU to start precharging*/
+	FDCANTxMessage msg = {.tx_header = {.Identifier = 0x00A,
+					    .IdType = FDCAN_STANDARD_ID,
+					    .TxFrameType = FDCAN_DATA_FRAME,
+					    .ErrorStateIndicator = FDCAN_ESI_ACTIVE,
+					    .DataLength = 1,
+					    .BitRateSwitch = FDCAN_BRS_OFF,
+					    .TxEventFifoControl = FDCAN_NO_TX_EVENTS,
+					    .MessageMarker = 0}};
+	msg.data[0] = 1; // Go TS Active/Precharge
+	can_send(primary_can, &msg);
+	stateData->ecu_state = GR_PRECHARGE_ENGAGED;
+	return;
+}
+
 void ECU_Precharge_Engaged(ECU_StateData *stateData)
 {
-	UNUSED(stateData);
-	if (stateData->ecuStatus2.ts_voltage > 60) {
-		// Go to TS discharge
-		stateData->ecuStatus1.ecu_status = GR_TS_DISCHARGE;
-		// Emit an error
+	if (stateData->ir_plus) {
+		stateData->ecu_state = GR_PRECHARGE_COMPLETE;
+		return;
+	}
+
+	if (!stateData->ts_active || CommunicationError(stateData)) {
+		ECU_Tractive_System_Discharge_Start(stateData);
 		return;
 	}
-	// TODO Implement functionality
-	/*if(not TS Active || Communication Error (CAN)){
-		stateData->currentState = GR_TS_DISCHARGE
-		break;
-	}*/
-	/*if(1 Isolation relay close && second isolation relay close){ --> CAN!
-		stateData->currentState = GR_PRECHARGE_COMPLETE
-	}*/
 }
 
 void ECU_Precharge_Complete(ECU_StateData *stateData)
 {
-	UNUSED(stateData);
-	// TODO Implement functionality
-	/*
-		On but idle
-
-		If Tractive System (TS) active/Critical Error --> Tractive
-	    System Discharge If Brake & RTD (Ready to Drive) --> Drive Active
-	*/
-	/*
-	if (TS pressed or critical error) {
-		stateData->currentState = GR_TS_DISCHARGE
-		emit error
-		break;
-	}
-	*/
-	/*
-	if(PressingBrake(stateData) && stateData->RTD){
-		stateData->currentState = GR_DRIVE_ACTIVE;
+	if (!stateData->ts_active || CriticalError(stateData)) {
+		ECU_Tractive_System_Discharge_Start(stateData);
+		LOGOMATIC("Error: Critical Error Occurred. Discharging Tractive System.");
+		setSoftwareLatch(0);
+		return;
 	}
-	*/
 
-	// Pseudocode
+	if (PressingBrake(stateData) && stateData->rtd) {
+		stateData->ecu_state = GR_DRIVE_ACTIVE;
+		GR_OLD_INVERTER_CONFIG_MSG message = {.max_ac_current = 0xFFFF, .max_dc_current = 0xFFFF, .abs_max_motor_rpm = 0xFFFF, .motor_direction = 0};
+		ECU_CAN_Send(GR_OLD_BUS_PRIMARY, GR_GR_INVERTER_1, MSG_INVERTER_CONFIG, &message, sizeof(message));
+		return;
+	}
 }
 
 void ECU_Drive_Active(ECU_StateData *stateData)
 {
-	UNUSED(stateData);
 	// TODO Implement functionality
 	/*
 		If APPS/BSE Violation --> Don't drive until resolved (no state
@@ -144,28 +183,67 @@ void ECU_Drive_Active(ECU_StateData *stateData)
 		}
 		if (!RTD) {
 			stateData->currentState = GR_PRECHARGE_COMPLETE
-			emit a warning if not moving
+			emit a warning if moving
 			break
 		}
 		and then we drive the car
 		 - calcPedalTravel func :p
 		 - make tuna-ble function
 	*/
+
+	if (!stateData->ts_active || CriticalError(stateData)) {
+		ECU_Tractive_System_Discharge_Start(stateData);
+		LOGOMATIC("Error: Critical Error Occured. Discharging Tractive System.");
+		setSoftwareLatch(0);
+		return;
+	}
+
+	if (!stateData->rtd) {
+		stateData->ecu_state = GR_PRECHARGE_COMPLETE;
+		if (vehicle_is_moving(stateData)) {
+			LOGOMATIC("Warning: Vehicle is moving during state transition.");
+		}
+		return;
+	}
+
+	float torque_request = CalcPedalTravel(stateData) * MAX_CURRENT_AMPS;
+	// If you are pressing the brake, then you have the negativetorque request calculated
+	bool brakePressed = PressingBrake(stateData);
+	torque_request -= brakePressed * CalcBrakePercent(stateData) * MAX_REVERSE_CURRENT_AMPS; // This is negative current
+
+	GR_OLD_INVERTER_COMMAND_MSG message = {.ac_current = torque_request * 100 + 32768, .dc_current = torque_request * 100 + 32768, .drive_enable = 1, .rpm_limit = 0};
+	ECU_CAN_Send(GR_OLD_BUS_PRIMARY, GR_GR_INVERTER_1, MSG_INVERTER_COMMAND, &message, sizeof(message));
+	if (brakePressed) {
+		LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_4);
+	} else {
+		LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_4);
+	}
+}
+
+void ECU_Tractive_System_Discharge_Start(ECU_StateData *stateData)
+{
+	stateData->ecu_state = GR_TS_DISCHARGE;
+	LOGOMATIC("ECU: BCU discharge Tractive System");
+	GR_OLD_BCU_PRECHARGE_MSG message = {.precharge = 0};
+	ECU_CAN_Send(GR_OLD_BUS_PRIMARY, GR_BCU, MSG_BCU_PRECHARGE, &message, sizeof(message));
+	stateData->dischargeStartMillis = stateData->millisSinceBoot;
 }
 
 void ECU_Tractive_System_Discharge(ECU_StateData *stateData)
 {
-	UNUSED(stateData);
-	// TODO Implement functionality of state itself
 	/*
 		Discharge the tractive system to below 60 volts
 		If TS voltage < 60 --> stateData->GLV_ON
 	*/
-	if (stateData->ecuStatus2.ts_voltage < 60) {
-		stateData->ecuStatus1.ecu_status = GR_GLV_ON;
+	if (stateData->ts_voltage < SAFE_VOLTAGE_LIMIT) {
+		stateData->ecu_state = GR_GLV_ON;
+		return;
 	}
 	/*
 		If TS fails to discharge over time then stay and emit a warning,
 	   see #129
 	*/
+	if (stateData->millisSinceBoot - stateData->dischargeStartMillis > TRACTIVE_SYSTEM_MAX_PERMITTED_DISCHARGE_TIME_MILLIS) {
+		LOGOMATIC("Warning: Tractive System fails to discharge in time.");
+	}
 }
diff --git a/ECU/Application/Src/StateUtils.c b/ECU/Application/Src/StateUtils.c
index 6e8b4604..dba7ce78 100644
--- a/ECU/Application/Src/StateUtils.c
+++ b/ECU/Application/Src/StateUtils.c
@@ -3,70 +3,98 @@
 #include <math.h>
 #include <stdbool.h>
 
+#include "Logomatic.h"
 #include "StateData.h"
-#include "Unused.h"
+#include "main.h"
 
+uint32_t MillisecondsSinceBoot(void)
+{
+	// For some reason, GetTickFreq returns period in millisecon instead of frequency
+	// See https://community.st.com/t5/stm32-mcus-embedded-software/name-amp-description-of-hal-gettickfreq-misleading/td-p/242457
+	return HAL_GetTick() * HAL_GetTickFreq();
+}
+
+// use estop_sense to detect close(?)
 void setSoftwareLatch(bool close)
 {
 	UNUSED(close);
 	// TODO Implement functionality
-	// LOGOMATIC("Setting software latch to %d\n", close);
-	/*
-	    if (close && !HAL_GPIO_ReadPin(SOFTWARE_OK_CONTROL_GPIO_Port,
-	   SOFTWARE_OK_CONTROL_Pin)) // Avoid writing pins that are already
-	   written too
-	    {
-		HAL_GPIO_WritePin(SOFTWARE_OK_CONTROL_GPIO_Port,
-	   SOFTWARE_OK_CONTROL_Pin, GPIO_PIN_SET);
-	    }
-	    else if (!close && HAL_GPIO_ReadPin(SOFTWARE_OK_CONTROL_GPIO_Port,
-	   SOFTWARE_OK_CONTROL_Pin))
-	    {
-		HAL_GPIO_WritePin(SOFTWARE_OK_CONTROL_GPIO_Port,
-	   SOFTWARE_OK_CONTROL_Pin, GPIO_PIN_RESET);
-	    }
-		*/
+	LOGOMATIC("Setting software latch to %d\n", close);
+
+	if (close && !LL_GPIO_IsInputPinSet(SOFTWARE_OK_CONTROL_GPIO_Port, SOFTWARE_OK_CONTROL_Pin)) // Avoid writing pins that are already written to
+	{
+		LL_GPIO_SetOutputPin(SOFTWARE_OK_CONTROL_GPIO_Port, SOFTWARE_OK_CONTROL_Pin);
+	} else if (!close && LL_GPIO_IsInputPinSet(SOFTWARE_OK_CONTROL_GPIO_Port, SOFTWARE_OK_CONTROL_Pin)) {
+		LL_GPIO_ResetOutputPin(SOFTWARE_OK_CONTROL_GPIO_Port, SOFTWARE_OK_CONTROL_Pin);
+	}
 }
 
-bool CriticalError(const ECU_StateData *stateData)
+bool CriticalError(volatile const ECU_StateData *stateData)
 {
-	if (stateData->ecuStatus1.max_cell_temp > 60) {
-		return true;
-	}
-	if (stateData->ecuStatus2.ts_voltage > 600) {
-		return true;
-	}
-	return false;
+	bool problem = false;
+	problem |= stateData->max_cell_temp > 60;
+	problem |= stateData->ts_voltage > 600;
+	problem |= APPS_BSE_Violation(stateData);
+	problem |= !stateData->bcu_software_latch;
+	problem |= stateData->ir_plus && !stateData->ir_minus;
+	problem |= !stateData->ir_plus && (stateData->ecu_state == GR_PRECHARGE_COMPLETE || stateData->ecu_state == GR_DRIVE_ACTIVE);
+	problem |= !stateData->ir_minus && (stateData->ecu_state == GR_PRECHARGE_ENGAGED || stateData->ecu_state == GR_PRECHARGE_COMPLETE || stateData->ecu_state == GR_DRIVE_ACTIVE);
+	problem |= (stateData->imd_sense >= 2.7) || (stateData->imd_sense <= 1.45); // TODO: find better range
+	problem |= (stateData->ams_sense >= 2.7) || (stateData->ams_sense <= 1.45); // TODO: find better range
+	return problem;
 }
 
-bool CommunicationError(const ECU_StateData *stateData)
+bool bmsFailure(volatile const ECU_StateData *stateData)
+{
+	return (stateData->ams_sense >= 2.7) || (stateData->ams_sense <= 1.45); // TODO: find better range
+}
+
+bool imdFailure(volatile const ECU_StateData *stateData)
+{
+	return (stateData->imd_sense >= 2.7) || (stateData->imd_sense <= 1.45); // TODO: find better range
+}
+
+bool CommunicationError(volatile const ECU_StateData *stateData)
 {
 	UNUSED(stateData);
-	// TODO: implement COMMS errors
+	// TODO: Check for communication errors
 	return false;
 }
 
-bool APPS_BSE_Violation(const ECU_StateData *stateData)
+bool APPS_BSE_Violation(volatile const ECU_StateData *stateData)
 {
-	// Checks 2 * APPS_1 is within 10% of APPS_2 and break + throttle at the
-	// same time
-	return fabs(stateData->APPS2_Signal - stateData->APPS1_Signal * APPS_PROPORTION - APPS_OFFSET) > stateData->APPS2_Signal * 0.1f ||
-	       (PressingBrake(stateData) && CalcPedalTravel(stateData) >= 0.25f);
+	// Checks 2 * APPS_1 is within 10% of APPS_2 and break + throttle at the same time
+	return PressingBrake(stateData) && CalcPedalTravel(stateData) >= 0.25f;
 }
 
-bool PressingBrake(const ECU_StateData *stateData)
+bool PressingBrake(volatile const ECU_StateData *stateData)
 {
-	return (stateData->Brake_F_Signal - BRAKE_F_MIN > BSE_DEADZONE * (BRAKE_F_MAX - BRAKE_F_MIN)) && (stateData->Brake_R_Signal - BRAKE_R_MIN > BSE_DEADZONE * (BRAKE_R_MAX - BRAKE_R_MIN));
+	uint16_t brakeRangeF = BRAKE_F_MAX - BRAKE_F_MIN;
+	uint16_t brakeRangeR = BRAKE_R_MAX - BRAKE_R_MIN;
+	bool brakeFpress = stateData->Brake_F_Signal - BRAKE_F_MIN > BSE_DEADZONE * brakeRangeF;
+	bool brakeRpress = stateData->Brake_R_Signal - BRAKE_R_MIN > BSE_DEADZONE * brakeRangeR;
+	return brakeFpress || brakeRpress;
 	// Ideally TCM receives values of 0 after this is no longer called xD.
 }
 
-float CalcBrakePercent(const ECU_StateData *stateData) // THIS IS NOT ACTUALLY BRAKE TRAVEL,
-						       // PRESSURE SENSORS CAPTURE BRAKE TRAVEL
+float CalcBrakePercent(volatile const ECU_StateData *stateData) // THIS IS NOT ACTUALLY BRAKE TRAVEL,
+								// PRESSURE SENSORS CAPTURE BRAKE TRAVEL
+{
+	float total_brake_range = BRAKE_F_MAX - BRAKE_F_MIN + BRAKE_R_MAX - BRAKE_R_MIN;
+	float total_brake_value = stateData->Brake_F_Signal + stateData->Brake_R_Signal - BRAKE_R_MIN - BRAKE_F_MIN;
+	return total_brake_value / total_brake_range;
+}
+
+float CalcPedalTravel(volatile const ECU_StateData *stateData)
 {
-	return (float)(stateData->Brake_F_Signal + stateData->Brake_R_Signal - BRAKE_R_MIN - BRAKE_F_MIN) / (BRAKE_F_MAX - BRAKE_F_MIN + BRAKE_R_MAX - BRAKE_R_MIN);
+	float total_signal_range = THROTTLE_MAX_1 + THROTTLE_MAX_2 - THROTTLE_MIN_1 - THROTTLE_MIN_2;
+	float total_signal_value = stateData->APPS1_Signal + stateData->APPS2_Signal - THROTTLE_MIN_2 - THROTTLE_MIN_1;
+	float travel = total_signal_value / total_signal_range;
+	return travel > 0.05 ? (travel - 0.05f) / 0.95f : 0;
 }
 
-float CalcPedalTravel(const ECU_StateData *stateData)
+bool vehicle_is_moving(volatile const ECU_StateData *stateData)
 {
-	return (float)(stateData->APPS1_Signal + stateData->APPS2_Signal - THROTTLE_MIN_2 - THROTTLE_MIN_1) / (THROTTLE_MAX_1 + THROTTLE_MAX_2 - THROTTLE_MIN_1 - THROTTLE_MIN_2);
+	const float tolerance = 0.1; // In MPH
+	return stateData->vehicle_speed > tolerance;
 }
diff --git a/ECU/CMakeLists.txt b/ECU/CMakeLists.txt
index bf16105f..a9c003c9 100644
--- a/ECU/CMakeLists.txt
+++ b/ECU/CMakeLists.txt
@@ -14,26 +14,16 @@ endif()
 get_filename_component(GR_PROJECT_NAME ${CMAKE_CURRENT_SOURCE_DIR} NAME)
 
 if(CMAKE_PRESET_NAME STREQUAL "HOOTLTest")
-	add_executable(state_machine_tick_returns)
+	add_executable(dummy_test)
 	target_include_directories(
-		state_machine_tick_returns
+		dummy_test
 		PRIVATE
 			Application/Inc
 			Core/Inc
 	)
-	target_sources(
-		state_machine_tick_returns
-		PRIVATE
-			Test/state_machine_tick_returns.c
-			Application/Src/StateTicks.c
-	)
-	target_link_libraries(
-		state_machine_tick_returns
-		PRIVATE
-			GLOBALSHARE_LIB
-			GR_OLD_CAN_MESSAGES
-	)
-	add_test(state_machine_tick_returns state_machine_tick_returns)
+	target_sources(dummy_test PRIVATE Test/dummy_test.c)
+	target_link_libraries(dummy_test PRIVATE GLOBALSHARE_LIB)
+	add_test(dummy_test dummy_test)
 
 	# FIXME Add more HOOTL tests
 endif()
@@ -71,12 +61,16 @@ target_sources(
 		Application/Src/CANdler.c
 		Application/Src/StateTicks.c
 		Application/Src/StateUtils.c
+		Application/Src/CANutils.c
 )
 
 target_link_libraries(
 	${GR_PROJECT_NAME}_USER_CODE
 	INTERFACE
 		GR_OLD_CAN_MESSAGES
+		ADC
+		PERIPHERAL_CAN_LIB
+		BitManipulations_Lib
 )
 
 target_include_directories(
diff --git a/ECU/Core/Inc/main.h b/ECU/Core/Inc/main.h
index 079e1eae..657f69f3 100644
--- a/ECU/Core/Inc/main.h
+++ b/ECU/Core/Inc/main.h
@@ -43,7 +43,7 @@ extern "C" {
 
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
-
+#include "GR_OLD_NODE_ID.h"
 /* USER CODE END Includes */
 
 /* Exported types ------------------------------------------------------------*/
@@ -58,7 +58,7 @@ extern "C" {
 
 /* Exported macro ------------------------------------------------------------*/
 /* USER CODE BEGIN EM */
-
+#define LOCAL_GR_ID GR_ECU
 /* USER CODE END EM */
 
 /* Exported functions prototypes ---------------------------------------------*/
diff --git a/ECU/Core/Inc/stm32g4xx_it.h b/ECU/Core/Inc/stm32g4xx_it.h
index 79c4e4fc..0e522cc6 100644
--- a/ECU/Core/Inc/stm32g4xx_it.h
+++ b/ECU/Core/Inc/stm32g4xx_it.h
@@ -55,8 +55,6 @@ void SVC_Handler(void);
 void DebugMon_Handler(void);
 void PendSV_Handler(void);
 void SysTick_Handler(void);
-void FDCAN1_IT0_IRQHandler(void);
-void FDCAN2_IT0_IRQHandler(void);
 /* USER CODE BEGIN EFP */
 
 /* USER CODE END EFP */
diff --git a/ECU/Core/Src/main.c b/ECU/Core/Src/main.c
index cca36d58..8784f30e 100644
--- a/ECU/Core/Src/main.c
+++ b/ECU/Core/Src/main.c
@@ -19,16 +19,26 @@
 /* Includes ------------------------------------------------------------------*/
 #include "main.h"
 
+#include "GR_OLD_BUS_ID.h"
+#include "StateData.h"
+#include "StateTicks.h"
 #include "adc.h"
 #include "dma.h"
 #include "fdcan.h"
 #include "gpio.h"
+#include "gr_adc.h"
+#include "malloc.h"
 #include "usart.h"
 
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
+#include "CANdler.h"
+#include "CANutils.h"
 #include "Logomatic.h"
 #include "StateTicks.h"
+#include "StateUtils.h"
+#include "adc.h"
+#include "can.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -52,6 +62,26 @@
 
 /* USER CODE END PV */
 
+// CAN
+
+#define CAN_TX_BUFFER_LENGTH 10
+// ADC 1
+#define WINDOW_SIZE 10 // weighted average for now can extend to other window functions
+#define NUM_SIGNALS_ADC1 7
+#define NUM_SIGNALS_ADC2 4
+#define NUM_SIGNALS_DIGITAL 8
+// TODO: check which data size to use (floats...ints...etc)
+volatile uint16_t ADC1_buffers[NUM_SIGNALS_ADC1] = {0};		      // Contains new values
+volatile uint16_t ADC2_buffers[NUM_SIGNALS_ADC2] = {0};		      // Contains new values
+uint16_t ADC1_outputs[NUM_SIGNALS_ADC1] = {0};			      // Updated averages
+uint16_t ADC2_outputs[NUM_SIGNALS_ADC2] = {0};			      // Updated averages
+uint16_t *adcDataValues[(NUM_SIGNALS_ADC1 + NUM_SIGNALS_ADC2)] = {0}; // 2D Array
+
+// DIGITAL
+
+// STATE DATA
+extern ECU_StateData stateLump;
+
 /* Private function prototypes -----------------------------------------------*/
 void SystemClock_Config(void);
 /* USER CODE BEGIN PFP */
@@ -74,6 +104,230 @@ static void ITM_Enable(void)
 }
 /* USER CODE END 0 */
 
+// TODO: state data stores stuff as either FLOATS or BOOLS...check
+void read_digital(void)
+{
+	// debouncing/latching for ts/rtd active
+	bool ts_press = LL_GPIO_IsInputPinSet(GPIOC, LL_GPIO_PIN_12);
+	bool rtd_press = LL_GPIO_IsInputPinSet(GPIOC, LL_GPIO_PIN_11);
+	uint32_t curr_time = MillisecondsSinceBoot();
+
+	if (!stateLump.prev_ts_active_button_state && ts_press && (curr_time - stateLump.prev_ts_press_millis > BUTTON_REFRESH_RATE_MS)) {
+		stateLump.ts_active = !stateLump.ts_active;
+		stateLump.prev_ts_press_millis = curr_time;
+	}
+	if (!stateLump.prev_rtd_button_state && rtd_press && (curr_time - stateLump.prev_ts_press_millis > BUTTON_REFRESH_RATE_MS)) {
+		stateLump.rtd = !stateLump.rtd;
+		stateLump.prev_rtd_press_millis = curr_time;
+	}
+
+	stateLump.prev_ts_active_button_state = ts_press;
+	stateLump.prev_rtd_button_state = rtd_press;
+
+	// TODO: inertia sense? LL_GPIO_IsInputPinSet(GPIOC, LL_GPIO_PIN_10);
+	stateLump.estop_sense = LL_GPIO_IsInputPinSet(GPIOA, LL_GPIO_PIN_15);
+}
+
+void write_state_data()
+{
+	// analog
+	// TODO: bse signal idk what to do ADC1_outputs[0]
+	// TODO: bspd signal idk what to do ADC1_outputs[1]
+	stateLump.APPS1_Signal = ADC1_outputs[2];
+	stateLump.APPS2_Signal = ADC1_outputs[3];
+	stateLump.Brake_F_Signal = ADC1_outputs[4];
+	stateLump.Brake_R_Signal = ADC1_outputs[5];
+	// TODO: Aux signal idk what to do with it ADC1_outputs[6]
+	stateLump.STEERING_ANGLE_SIGNAL = ADC2_outputs[0];
+	stateLump.bspd_sense = ADC2_outputs[1];
+	stateLump.imd_sense = ADC2_outputs[2];
+	stateLump.ams_sense = ADC2_outputs[3];
+}
+
+void ADC_Configure(void)
+{
+	// Initialize which clock source to use
+	LL_RCC_SetADCClockSource(LL_RCC_ADC12_CLKSOURCE_SYSCLK);
+	/* Peripheral clock enable */
+	LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_ADC12);
+	LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA);
+
+	// Initialize the ADC1
+	ADC_Group_Init(ADC1, PS_8); // TODO: change prescalar l8r
+	ADC_Init(ADC1, RESOLUTION_12, RIGHT);
+	ADC_Regular_Group_Init(ADC1, RANKS_7);
+
+	// TODO: INITIALIZE PIN_PORTS BETTER!!!
+	// Initialize the pins and channels
+	Pin_Ports p1 = {0};
+	p1.port = GPIOC;
+	p1.pin = LL_GPIO_PIN_0 | LL_GPIO_PIN_1 | LL_GPIO_PIN_2 | LL_GPIO_PIN_3;
+	ADC_Init_Pins(&p1);
+	Pin_Ports p2 = {0};
+	p2.port = GPIOB;
+	p2.pin = LL_GPIO_PIN_0 | LL_GPIO_PIN_1 | LL_GPIO_PIN_14;
+	ADC_Init_Pins(&p2);
+	ADC_Channel_Init(ADC1, RANK_1, ADC_CHANNEL_6, SINGLE_ENDED, SAMPLINGTIME_247CYCLES_5);
+	ADC_Channel_Init(ADC1, RANK_2, ADC_CHANNEL_7, SINGLE_ENDED, SAMPLINGTIME_247CYCLES_5);
+	ADC_Channel_Init(ADC1, RANK_3, ADC_CHANNEL_8, SINGLE_ENDED, SAMPLINGTIME_247CYCLES_5);
+	ADC_Channel_Init(ADC1, RANK_4, ADC_CHANNEL_9, SINGLE_ENDED, SAMPLINGTIME_247CYCLES_5);
+	ADC_Channel_Init(ADC1, RANK_5, ADC_CHANNEL_15, SINGLE_ENDED, SAMPLINGTIME_247CYCLES_5);
+	ADC_Channel_Init(ADC1, RANK_6, ADC_CHANNEL_12, SINGLE_ENDED, SAMPLINGTIME_247CYCLES_5);
+	ADC_Channel_Init(ADC1, RANK_7, ADC_CHANNEL_5, SINGLE_ENDED, SAMPLINGTIME_247CYCLES_5);
+
+	// Initialize ADC2
+	ADC_Init(ADC2, RESOLUTION_12, RIGHT);
+	ADC_Regular_Group_Init(ADC2, RANKS_4);
+
+	// Initialize the pins and channels
+	Pin_Ports p3 = {0};
+	p3.port = GPIOA;
+	p3.pin = LL_GPIO_PIN_15;
+	ADC_Init_Pins(&p3);
+	ADC_Channel_Init(ADC2, RANK_1, ADC_CHANNEL_15, SINGLE_ENDED, SAMPLINGTIME_247CYCLES_5);
+	ADC_Channel_Init(ADC2, RANK_2, ADC_CHANNEL_13, SINGLE_ENDED, SAMPLINGTIME_247CYCLES_5);
+	ADC_Channel_Init(ADC2, RANK_3, ADC_CHANNEL_3, SINGLE_ENDED, SAMPLINGTIME_247CYCLES_5);
+	ADC_Channel_Init(ADC2, RANK_4, ADC_CHANNEL_4, SINGLE_ENDED, SAMPLINGTIME_247CYCLES_5);
+
+	// Initialize DMA (ADC1 = CHANNEL 1, ADC2 = CHANNEL 2)
+	// DMA reads into buffer
+	DMA_Init(DMA1, LL_DMA_CHANNEL_1, LL_ADC_DMA_GetRegAddr(ADC1, LL_ADC_DMA_REG_REGULAR_DATA), ADC1_buffers, LL_DMA_PDATAALIGN_HALFWORD, LL_DMA_MDATAALIGN_HALFWORD, NUM_SIGNALS_ADC1, ADC1, HIGH);
+	LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_1);
+	DMA_Init(DMA1, LL_DMA_CHANNEL_2, LL_ADC_DMA_GetRegAddr(ADC2, LL_ADC_DMA_REG_REGULAR_DATA), ADC2_buffers, LL_DMA_PDATAALIGN_HALFWORD, LL_DMA_MDATAALIGN_HALFWORD, NUM_SIGNALS_ADC2, ADC2, HIGH);
+	LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_2);
+
+	ADC_Enable_And_Calibrate(ADC1);
+	ADC_Enable_And_Calibrate(ADC2);
+}
+
+void CAN1_rx_callback(void *data, uint32_t size, uint32_t ID)
+{
+	ECU_CAN_MessageHandler(&stateLump, GR_OLD_BUS_PRIMARY,
+			       (0x000FFF00 & ID) >> 8, // TODO: Double check
+			       (0xFF00000 & ID) >> 20, data, size);
+}
+
+void CAN2_rx_callback(void *data, uint32_t size, uint32_t ID) { ECU_CAN_MessageHandler(&stateLump, GR_OLD_BUS_DATA, (0x000FFF00 & ID) >> 8, (0xFF00000 & ID) >> 20, data, size); }
+
+void CAN_Configure()
+{
+	CANConfig canCfg;
+
+	// SHARED config ddata for CAN1 and CAN2
+	canCfg.hal_fdcan_init.ClockDivider = FDCAN_CLOCK_DIV1;
+	canCfg.hal_fdcan_init.FrameFormat = FDCAN_FRAME_FD_NO_BRS;
+	canCfg.hal_fdcan_init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION;
+	canCfg.hal_fdcan_init.Mode = FDCAN_MODE_NORMAL;
+	canCfg.hal_fdcan_init.AutoRetransmission = ENABLE;
+	canCfg.hal_fdcan_init.TransmitPause = DISABLE;
+	canCfg.hal_fdcan_init.ProtocolException = ENABLE;
+	canCfg.hal_fdcan_init.NominalPrescaler = 1;
+	canCfg.hal_fdcan_init.NominalSyncJumpWidth = 16;
+	canCfg.hal_fdcan_init.NominalTimeSeg1 = 127; // Updated for 170MHz: (1+127+42)*1 = 170 ticks -> 1 Mbps
+	canCfg.hal_fdcan_init.NominalTimeSeg2 = 42;
+	canCfg.hal_fdcan_init.DataPrescaler = 8;
+	canCfg.hal_fdcan_init.DataSyncJumpWidth = 16;
+	canCfg.hal_fdcan_init.DataTimeSeg1 = 15; // Updated for 170MHz: (1+15+5)*8 = 168 ticks -> ~5 Mbps
+	canCfg.hal_fdcan_init.DataTimeSeg2 = 5;
+	canCfg.hal_fdcan_init.StdFiltersNbr = 1;
+	canCfg.hal_fdcan_init.ExtFiltersNbr = 0;
+
+	canCfg.rx_callback = NULL;
+	canCfg.rx_interrupt_priority = 15; // TODO: Maybe make these not hardcoded
+	canCfg.tx_interrupt_priority = 15;
+	canCfg.tx_buffer_length = CAN_TX_BUFFER_LENGTH;
+
+	// RX shared settings
+	canCfg.init_rx_gpio.Mode = GPIO_MODE_AF_PP;
+	canCfg.init_rx_gpio.Pull = GPIO_PULLUP;
+	canCfg.init_rx_gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
+
+	// TX Shared settings
+	canCfg.init_tx_gpio.Mode = GPIO_MODE_AF_PP;
+	canCfg.init_tx_gpio.Pull = GPIO_NOPULL;
+	canCfg.init_tx_gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
+
+	/*FDCAN_TxHeaderTypeDef TxHeader = {
+	    .Identifier = 1,
+
+	    .IdType = FDCAN_STANDARD_ID,
+	    .TxFrameType = FDCAN_DATA_FRAME,
+	    .ErrorStateIndicator = FDCAN_ESI_ACTIVE, // honestly this might be a value you have to read from a node
+						     // FDCAN_ESI_ACTIVE is just a state that assumes there are minimal errors
+	    .DataLength = 1,
+	    .BitRateSwitch = FDCAN_BRS_OFF,
+	    .TxEventFifoControl = FDCAN_NO_TX_EVENTS, // change to FDCAN_STORE_TX_EVENTS if you need to store info regarding transmitted messages
+	    .MessageMarker = 0			      // also change this to a real address if you change fifo control
+	};
+
+	FDCANTxMessage msg = {.data = {0x80}, .tx_header = TxHeader};
+	*/
+
+	// PCLK1 from SYSCLK
+	can_set_clksource(LL_RCC_FDCAN_CLKSOURCE_PCLK1);
+
+	// CAN1 =====================================================================
+	canCfg.fdcan_instance = FDCAN1;
+	canCfg.rx_gpio = GPIOA;
+	canCfg.init_rx_gpio.Pin = GPIO_PIN_11;
+	canCfg.init_rx_gpio.Alternate = GPIO_AF9_FDCAN1;
+
+	canCfg.tx_gpio = GPIOA;
+	canCfg.init_tx_gpio.Pin = GPIO_PIN_12;
+	canCfg.init_tx_gpio.Alternate = GPIO_AF9_FDCAN1;
+
+	// RX Callback CAN1
+	canCfg.rx_callback = CAN1_rx_callback; // TODO: Make sure the wrapper for this is defined correctly
+
+	primary_can = can_init(&canCfg);
+
+	// Filter 1 Definitions
+	FDCAN_FilterTypeDef fdcan1_filter;
+
+	fdcan1_filter.IdType = FDCAN_EXTENDED_ID;
+	fdcan1_filter.FilterIndex = 0;
+	fdcan1_filter.FilterType = FDCAN_FILTER_MASK;
+	fdcan1_filter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
+	fdcan1_filter.FilterID1 = LOCAL_GR_ID; // filter messages with ECU destination
+	fdcan1_filter.FilterID2 = 0x00000FF;
+
+	fdcan1_filter.FilterIndex = 1;
+	fdcan1_filter.FilterID1 = 0xFF; // filter messages for all targets
+	HAL_FDCAN_ConfigFilter(primary_can->hal_fdcanP, &fdcan1_filter);
+
+	// CAN2 ======================================================
+	canCfg.fdcan_instance = FDCAN2;
+	canCfg.rx_gpio = GPIOB;
+	canCfg.init_rx_gpio.Pin = GPIO_PIN_12;
+	canCfg.init_rx_gpio.Alternate = GPIO_AF9_FDCAN2;
+
+	canCfg.tx_gpio = GPIOB;
+	canCfg.init_tx_gpio.Pin = GPIO_PIN_13;
+	canCfg.init_tx_gpio.Alternate = GPIO_AF9_FDCAN2;
+
+	// RX Callback CAN2
+	canCfg.rx_callback = CAN2_rx_callback; // TODO: Make sure the wrapper for this is defined correctly
+
+	// Filter definitions
+	FDCAN_FilterTypeDef fdcan2_filter;
+
+	fdcan2_filter.IdType = FDCAN_EXTENDED_ID;
+	fdcan2_filter.FilterIndex = 0;
+	fdcan2_filter.FilterType = FDCAN_FILTER_MASK;
+	fdcan2_filter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0; // TODO: check if this works during test, RXFifos may not be indpeendent (but it sur)
+	fdcan2_filter.FilterID2 = 0x00000FF;
+
+	fdcan2_filter.FilterIndex = 1;
+	fdcan2_filter.FilterID1 = 0xFF; // filter messages for all targets
+
+	data_can = can_init(&canCfg);
+
+	// accept unmatched standard and extended frames into RXFIFO0 - default behaviour
+	HAL_FDCAN_ConfigFilter(data_can->hal_fdcanP, &fdcan2_filter);
+
+	can_start(primary_can);
+	can_start(data_can);
+}
 /**
  * @brief  The application entry point.
  * @retval int
@@ -111,8 +365,22 @@ int main(void)
 	MX_ADC1_Init();
 	MX_ADC2_Init();
 	MX_LPUART1_UART_Init();
+
 	/* USER CODE BEGIN 2 */
 
+	// Set Software Latch to closed
+	setSoftwareLatch(1);
+
+	// Initialize CAN
+	CAN_Configure();
+
+	ADC_Configure();
+	for (int i = 0; i < (NUM_SIGNALS_ADC1 + NUM_SIGNALS_ADC2); i++) {
+		adcDataValues[i] = malloc(sizeof(uint16_t) * WINDOW_SIZE);
+	}
+
+	LOGOMATIC("Boot completed at %lu ms\n", MillisecondsSinceBoot());
+
 	/* USER CODE END 2 */
 
 	/* Infinite loop */
@@ -121,11 +389,19 @@ int main(void)
 		/* USER CODE END WHILE */
 
 		/* USER CODE BEGIN 3 */
+		read_digital();
+		ADC_UpdateAnalogValues(adcDataValues, ADC1_buffers, NUM_SIGNALS_ADC1, WINDOW_SIZE, ADC1_outputs);
+		ADC_UpdateAnalogValues(adcDataValues, ADC2_buffers, NUM_SIGNALS_ADC2, WINDOW_SIZE, ADC2_outputs);
+		SendECUStateDataOverCAN(&stateLump);
+		write_state_data();
 		ECU_State_Tick();
 		LOGOMATIC("Main Loop Tick Complete. I like Pi %f\n", 3.14159265);
-		LL_mDelay(250); // FIXME Reduce or remove delay
+		LL_mDelay(250); // FIXME Reduce or remove de
 	}
 	/* USER CODE END 3 */
+	for (int i = (NUM_SIGNALS_ADC1 + NUM_SIGNALS_ADC2) - 1; i >= 0; i--) {
+		free(adcDataValues[i]);
+	}
 }
 
 /**
@@ -149,6 +425,7 @@ void SystemClock_Config(void)
 	while (LL_RCC_PLL_IsReady() != 1) {}
 
 	LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL);
+
 	LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_2);
 	/* Wait till System clock is ready */
 	while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) {}
diff --git a/ECU/Core/Src/stm32g4xx_it.c b/ECU/Core/Src/stm32g4xx_it.c
index 115420fa..9b1037e6 100644
--- a/ECU/Core/Src/stm32g4xx_it.c
+++ b/ECU/Core/Src/stm32g4xx_it.c
@@ -187,41 +187,6 @@ void SysTick_Handler(void)
 	/* USER CODE END SysTick_IRQn 1 */
 }
 
-/******************************************************************************/
-/* STM32G4xx Peripheral Interrupt Handlers                                    */
-/* Add here the Interrupt Handlers for the used peripherals.                  */
-/* For the available peripheral interrupt handler names,                      */
-/* please refer to the startup file (startup_stm32g4xx.s).                    */
-/******************************************************************************/
-
-/**
- * @brief This function handles FDCAN1 interrupt 0.
- */
-void FDCAN1_IT0_IRQHandler(void)
-{
-	/* USER CODE BEGIN FDCAN1_IT0_IRQn 0 */
-
-	/* USER CODE END FDCAN1_IT0_IRQn 0 */
-	HAL_FDCAN_IRQHandler(&hfdcan1);
-	/* USER CODE BEGIN FDCAN1_IT0_IRQn 1 */
-
-	/* USER CODE END FDCAN1_IT0_IRQn 1 */
-}
-
-/**
- * @brief This function handles FDCAN2 interrupt 0.
- */
-void FDCAN2_IT0_IRQHandler(void)
-{
-	/* USER CODE BEGIN FDCAN2_IT0_IRQn 0 */
-
-	/* USER CODE END FDCAN2_IT0_IRQn 0 */
-	HAL_FDCAN_IRQHandler(&hfdcan2);
-	/* USER CODE BEGIN FDCAN2_IT0_IRQn 1 */
-
-	/* USER CODE END FDCAN2_IT0_IRQn 1 */
-}
-
 /* USER CODE BEGIN 1 */
 
 /* USER CODE END 1 */
diff --git a/ECU/RelevantInfo.md b/ECU/RelevantInfo.md
new file mode 100644
index 00000000..a372772e
--- /dev/null
+++ b/ECU/RelevantInfo.md
@@ -0,0 +1,31 @@
+# Relevant Ports and Pins
+
+-- ANALOG IN --
+ADC 1 (ADC_1 BUFFERS array is IN ORDER from top to bottom of this list)
+BSE_SIGNAL (8): PC0 -> ADC12_IN6 (ADC 1 and ADC 2)
+BSPD_SIGNAL (9): PC1 -> ADC12_IN7
+APPS1_SIGNAL (10): PC2 -> ADC12_IN8
+APPS2_SIGNAL (11): PC3 -> ADC12_IN9
+BRAKE_F_SIGNAL (24): PB0 -> ADC1_IN15
+BRAKE_R_SIGNAL (25): PB1 -> ADC1_IN12
+AUX_SIGNAL (36): PB14 -> ADC1_IN5
+
+ADC 2
+STEERING_ANGLE_SIGNAL (37): PB15 -> ADC2_IN15
+BSPD_SENSE (19): PA5 -> ADC2_IN13
+IMD_SENSE (20): PA6 -> ADC2_IN3
+AMS_SENSE (21): PA7 -> ADC2_IN4 (actually is the BMS!! (ACU))
+
+-- DIGITAL IN --
+TS_ACTIVE_BTN_SENSE (54): PC12
+RTD_BTN_SENSE (53): PC11
+INERTIA_SW_SENSE (52): PC10
+ESTOP_SENSE (51): PA15
+
+-- DIGITAL OUT --
+RTD_CONTROL (60): PB7
+TSSI_R_CONTROL (59): PB6
+TSSI_G_CONTROL (58): PB5
+BRAKE_CONTROL (57): PB4
+TS_ACTIVE_BTN_LED_CONTROL (43): PA9
+RTD_BTN_LED_CONTROL (42): PA8
diff --git a/ECU/Test/dummy_test.c b/ECU/Test/dummy_test.c
new file mode 100644
index 00000000..f1080e5e
--- /dev/null
+++ b/ECU/Test/dummy_test.c
@@ -0,0 +1,7 @@
+#include "Logomatic.h"
+
+int main(void)
+{
+	LOGOMATIC("Dummy test executed successfully.\n");
+	return 0;
+}
diff --git a/ECU/Test/state_machine_tick_returns.c b/ECU/Test/state_machine_tick_returns.c
deleted file mode 100644
index 05ee525d..00000000
--- a/ECU/Test/state_machine_tick_returns.c
+++ /dev/null
@@ -1,13 +0,0 @@
-#include "Logomatic.h"
-#include "StateTicks.h"
-
-int main(void)
-{
-	// Simulate state ticks, if creates its own infinite loop then something
-	// is wrong
-	for (int i = 0; i < 5; i++) {
-		ECU_State_Tick();
-	}
-
-	return 0;
-}
diff --git a/G4ADCTESTING/Core/Src/main.c b/G4ADCTESTING/Core/Src/main.c
index 9444bf83..fa160eee 100644
--- a/G4ADCTESTING/Core/Src/main.c
+++ b/G4ADCTESTING/Core/Src/main.c
@@ -104,7 +104,8 @@ void ADC_Configure(void)
 	ADC_Channel_Init(ADC1, RANK_1, ADC_CHANNEL_1, SINGLE_ENDED, SAMPLINGTIME_247CYCLES_5);
 
 	// Initialize DMA
-	DMA_Init(DMA1, LL_DMA_CHANNEL_1, LL_ADC_DMA_GetRegAddr(ADC1, LL_ADC_DMA_REG_REGULAR_DATA), (uint32_t)&buffers, LL_DMA_PDATAALIGN_HALFWORD, LL_DMA_MDATAALIGN_HALFWORD, NUM_SIGNALS, ADC1, HIGH);
+	DMA_Init(DMA1, LL_DMA_CHANNEL_1, LL_ADC_DMA_GetRegAddr(ADC1, LL_ADC_DMA_REG_REGULAR_DATA), (volatile void *)buffers, LL_DMA_PDATAALIGN_HALFWORD, LL_DMA_MDATAALIGN_HALFWORD, NUM_SIGNALS, ADC1,
+		 HIGH);
 	LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_1);
 
 	ADC_Enable_And_Calibrate(ADC1);
diff --git a/G4CANTESTING/Core/Src/main.c b/G4CANTESTING/Core/Src/main.c
index fdf71472..c659e039 100644
--- a/G4CANTESTING/Core/Src/main.c
+++ b/G4CANTESTING/Core/Src/main.c
@@ -145,7 +145,7 @@ int main(void)
 		// Receive on GPIOs
 		// HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, toggleze ? GPIO_PIN_SET
 		// : GPIO_PIN_RESET); HAL_Delay(1000); msg.data[0] = toggleze ?
-		// 0x00 : 0x80; can_send(can2Handle, &msg);
+		// 0x00 : 0x80; can_send(data_can, &msg);
 
 		// RCC->CFGR |= RCC_CFGR_SW;
 		/* USER CODE BEGIN 3 */
diff --git a/Lib/FancyLayers-RENAME/ADC/Inc/gr_adc.h b/Lib/FancyLayers-RENAME/ADC/Inc/gr_adc.h
index 2fee7c0a..d6ba62d3 100644
--- a/Lib/FancyLayers-RENAME/ADC/Inc/gr_adc.h
+++ b/Lib/FancyLayers-RENAME/ADC/Inc/gr_adc.h
@@ -176,7 +176,7 @@ typedef enum {
 	DMA_CHANNEL_8 = LL_DMA_CHANNEL_8
 } DMA_Channel;
 
-void DMA_Init(DMA_TypeDef *DMA, DMA_Channel channel, uint32_t src_address, uint32_t dest_address, uint32_t p_data_size, uint32_t m_data_size, uint32_t num_data, ADC_TypeDef *ADC,
+void DMA_Init(DMA_TypeDef *DMA, DMA_Channel channel, uint32_t src_address, volatile void *dest_address, uint32_t p_data_size, uint32_t m_data_size, uint32_t num_data, ADC_TypeDef *ADC,
 	      DMA_Priority priority);
 
 /*
diff --git a/Lib/FancyLayers-RENAME/ADC/Src/gr_adc.c b/Lib/FancyLayers-RENAME/ADC/Src/gr_adc.c
index 22eeaf29..9a455cf4 100644
--- a/Lib/FancyLayers-RENAME/ADC/Src/gr_adc.c
+++ b/Lib/FancyLayers-RENAME/ADC/Src/gr_adc.c
@@ -107,16 +107,16 @@ void ADC_Set_Common_Clock(ADC_Common_TypeDef *ADC_Common, CommonClock commonCloc
 CommonClock ADC_Get_Common_Clock(ADC_Common_TypeDef *ADC_Common) { return LL_ADC_GetCommonClock(ADC_Common); }
 // note to self: these are not valid errors; they appear in vscode but not on
 // compile	<-- To be clear I do not have any errors anywhere at all...
-void DMA_Init(DMA_TypeDef *DMA, DMA_Channel channel, uint32_t src_address, uint32_t dest_address, uint32_t p_data_size, uint32_t m_data_size, uint32_t num_data, ADC_TypeDef *ADC,
+void DMA_Init(DMA_TypeDef *DMA, DMA_Channel channel, uint32_t src_address, volatile void *dest_address, uint32_t p_data_size, uint32_t m_data_size, uint32_t num_data, ADC_TypeDef *ADC,
 	      DMA_Priority priority)
 {
 	LL_DMA_InitTypeDef config = {0};
 	config.PeriphOrM2MSrcAddress = src_address;
-	config.MemoryOrM2MDstAddress = dest_address;
+	config.MemoryOrM2MDstAddress = (uint32_t)dest_address;
 	config.Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
 	config.Mode = LL_DMA_MODE_CIRCULAR;
 	config.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT;
-	config.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_NOINCREMENT;
+	config.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
 	config.PeriphOrM2MSrcDataSize = p_data_size;
 	config.MemoryOrM2MDstDataSize = m_data_size;
 	config.NbData = num_data;
@@ -140,10 +140,10 @@ void DMA_Init(DMA_TypeDef *DMA, DMA_Channel channel, uint32_t src_address, uint3
 
 // NOTE: DMA init is still using NOINCREMENT
 // TODO: Add int n to consider last n values
-int num = 0;
-uint8_t filled = 0;
 void ADC_UpdateAnalogValues(uint16_t **adcDataValues, volatile uint16_t *new_values, int num_signals, int window_size, uint16_t *weighted_output)
 {
+	static int num = 0;
+	static uint8_t filled = 0;
 	for (int i = 0; i < num_signals; ++i) {
 		weighted_output[i] += (new_values[i] - (filled ? adcDataValues[i][num] : 0)) / window_size; // Update the average
 		adcDataValues[i][num] = new_values[i];
diff --git a/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_BUS_ID.h b/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_BUS_ID.h
index b1c108c3..b4828dc6 100644
--- a/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_BUS_ID.h
+++ b/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_BUS_ID.h
@@ -11,7 +11,7 @@ typedef enum {
 	GR_OLD_BUS_PRIMARY = 1,
 	/** Data Bus */
 	GR_OLD_BUS_DATA = 2,
-	/** Charging Cart and ACU Bus */
+	/** Charger and BCU Bus */
 	GR_OLD_BUS_CHARGING = 3,
 } GR_OLD_BUS_ID;
 
diff --git a/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_MSG_DAT.h b/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_MSG_DAT.h
index f2e1967c..a8fc9627 100644
--- a/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_MSG_DAT.h
+++ b/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_MSG_DAT.h
@@ -48,7 +48,7 @@ typedef struct {
 	uint16_t rl_wheel_rpm; /** RL wheel, 0.1x-3276.8 RPM */
 } GR_OLD_ECU_STATUS_3_MSG;
 
-/** ACU Status 1 */
+/** BCU Status 1 */
 typedef struct {
 	uint16_t tractivebattery_voltage; /** All cells sum, 0.01V */
 	uint16_t ts_voltage;		  /** TS output voltage, 0.01V */
@@ -57,7 +57,7 @@ typedef struct {
 	uint8_t glv_soc;		  /** GLV SoC, 20x/51=% */
 } GR_OLD_BCU_STATUS_1_MSG;
 
-/** ACU Status 2 */
+/** BCU Status 2 */
 typedef struct {
 	uint8_t voltage_20v;	  /** 20V GLV, 0.1V */
 	uint8_t voltage_12v;	  /** 12V supply, 0.1V */
@@ -68,7 +68,7 @@ typedef struct {
 	uint8_t precharge_bits;	  /** Precharge & relay states */
 } GR_OLD_BCU_STATUS_2_MSG;
 
-/** ACU Status 3 */
+/** BCU Status 3 */
 typedef struct {
 	uint16_t hv_input_voltage;  /** 600V input, 0.01V */
 	uint16_t hv_output_voltage; /** 20V output, 0.01V */
@@ -76,24 +76,24 @@ typedef struct {
 	uint16_t hv_output_current; /** 20V output, 0.001A */
 } GR_OLD_BCU_STATUS_3_MSG;
 
-/** ACU Precharge cmd */
+/** BCU Precharge cmd */
 typedef struct {
 	uint8_t precharge; /** Set TS active (0=shutdown, 1=precharge) */
 } GR_OLD_BCU_PRECHARGE_MSG;
 
-/** ACU Config Charge Params */
+/** BCU Config Charge Params */
 typedef struct {
 	uint16_t charge_voltage; /** Target charge V, 0.1V */
 	uint16_t charge_current; /** Target charge I, 0.1A */
 } GR_OLD_BCU_CONFIG_CHARGE_PARAMS_MSG;
 
-/** ACU Config Operational Params */
+/** BCU Config Operational Params */
 typedef struct {
 	uint8_t min_cell_voltage; /** Min cell V thresh, 0.01x+2 V */
 	uint8_t max_cell_voltage; /** Max cell temp thresh, 0.25°C */
 } GR_OLD_BCU_CONFIG_OPS_PARAMS_MSG;
 
-/** ACU Cell Data 1 - Cells 0-31 */
+/** BCU Cell Data 1 - Cells 0-31 */
 typedef struct {
 	struct {
 		uint8_t voltage;     /** Cell V, 0.01x+2 V */
@@ -101,7 +101,7 @@ typedef struct {
 	} cells[32];
 } GR_OLD_BCU_CELL_DATA_1_MSG;
 
-/** ACU Cell Data 2 - Cells 32-63 */
+/** BCU Cell Data 2 - Cells 32-63 */
 typedef struct {
 	struct {
 		uint8_t voltage;     /** Cell V, 0.01x+2 V */
@@ -109,7 +109,7 @@ typedef struct {
 	} cells[32];
 } GR_OLD_BCU_CELL_DATA_2_MSG;
 
-/** ACU Cell Data 3 - Cells 64-95 */
+/** BCU Cell Data 3 - Cells 64-95 */
 typedef struct {
 	struct {
 		uint8_t voltage;     /** Cell V, 0.01x+2 V */
@@ -117,7 +117,7 @@ typedef struct {
 	} cells[32];
 } GR_OLD_BCU_CELL_DATA_3_MSG;
 
-/** ACU Cell Data 4 - Cells 96-127 */
+/** BCU Cell Data 4 - Cells 96-127 */
 typedef struct {
 	struct {
 		uint8_t voltage;     /** Cell V, 0.01x+2 V */
@@ -125,7 +125,7 @@ typedef struct {
 	} cells[32];
 } GR_OLD_BCU_CELL_DATA_4_MSG;
 
-/** ACU Cell Data 5 - Cells 128-159 */
+/** BCU Cell Data 5 - Cells 128-159 */
 typedef struct {
 	struct {
 		uint8_t voltage;     /** Cell V, 0.01x+2 V */
diff --git a/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_MSG_ID.h b/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_MSG_ID.h
index 6b28db74..88007ee6 100644
--- a/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_MSG_ID.h
+++ b/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_MSG_ID.h
@@ -9,17 +9,17 @@ typedef enum {
 	MSG_ECU_STATUS_2 = 0x004,
 	MSG_ECU_STATUS_3 = 0x005,
 	MSG_ECU_CONFIG = 0x006,
-	MSG_ACU_STATUS_1 = 0x007,
-	MSG_ACU_STATUS_2 = 0x008,
-	MSG_ACU_STATUS_3 = 0x009,
-	MSG_ACU_PRECHARGE = 0x00A,
-	MSG_ACU_CONFIG_CHARGE_PARAMETERS = 0x00B,
-	MSG_ACU_CONFIG_OPERATIONAL_PARAMETERS = 0x00C,
-	MSG_ACU_CELL_DATA_1 = 0x00D,
-	MSG_ACU_CELL_DATA_2 = 0x00E,
-	MSG_ACU_CELL_DATA_3 = 0x00F,
-	MSG_ACU_CELL_DATA_4 = 0x010,
-	MSG_ACU_CELL_DATA_5 = 0x011,
+	MSG_BCU_STATUS_1 = 0x007,
+	MSG_BCU_STATUS_2 = 0x008,
+	MSG_BCU_STATUS_3 = 0x009,
+	MSG_BCU_PRECHARGE = 0x00A,
+	MSG_BCU_CONFIG_CHARGE_PARAMETERS = 0x00B,
+	MSG_BCU_CONFIG_OPERATIONAL_PARAMETERS = 0x00C,
+	MSG_BCU_CELL_DATA_1 = 0x00D,
+	MSG_BCU_CELL_DATA_2 = 0x00E,
+	MSG_BCU_CELL_DATA_3 = 0x00F,
+	MSG_BCU_CELL_DATA_4 = 0x010,
+	MSG_BCU_CELL_DATA_5 = 0x011,
 	MSG_DC_DC_STATUS = 0x012,
 	MSG_INVERTER_STATUS_1 = 0x013,
 	MSG_INVERTER_STATUS_2 = 0x014,
diff --git a/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_NODE_ID.h b/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_NODE_ID.h
index de6f5d99..e0f7c9c5 100644
--- a/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_NODE_ID.h
+++ b/Lib/FancyLayers-RENAME/GRCAN/TemporaryHoldover/Inc/GR_OLD_NODE_ID.h
@@ -8,7 +8,7 @@ typedef enum {
 	GR_IMD = 0x00,
 	GR_DEBUGGER = 0x01,
 	GR_ECU = 0x02,
-	GR_ACU = 0x03,
+	GR_BCU = 0x03,
 	GR_TCM = 0x04,
 	GR_DASH_PANEL = 0x05,
 	GR_STEERING_WHEEL = 0x06,
diff --git a/Lib/Peripherals/CAN/Inc/can.h b/Lib/Peripherals/CAN/Inc/can.h
index d37d614d..60015549 100644
--- a/Lib/Peripherals/CAN/Inc/can.h
+++ b/Lib/Peripherals/CAN/Inc/can.h
@@ -13,7 +13,7 @@
 
 // RX Callback must perform a deep copy of the data
 //
-typedef void (*CAN_RXCallback)(void *data, uint32_t size);
+typedef void (*CAN_RXCallback)(void *data, uint32_t size, uint32_t ID);
 typedef struct {
 	// can baud rate is set by fdcan prescaler and RCC clock configurations
 	FDCAN_GlobalTypeDef *fdcan_instance; // Base address of FDCAN peripheral in memory (FDCAN1, FDCAN2, FDCAN3 macros)
diff --git a/Lib/Peripherals/CAN/README.md b/Lib/Peripherals/CAN/README.md
index 012ab1a1..0bbd7008 100644
--- a/Lib/Peripherals/CAN/README.md
+++ b/Lib/Peripherals/CAN/README.md
@@ -6,22 +6,18 @@ void can_set_clksource(uint32_t LL_RCC_FDCAN_CLKSOURCE); //ex LL_RCC_FDCAN_CLKSO
 
 int can_start(CANHandle*handle);
 int can_stop(CANHandle*handle);
-int can_send(CANHandle*handle, FDCANMessage* buffer, size_t send);  
+int can_send(CANHandle*handle, FDCANMessage* buffer, size_t send);
 int can_release(CANHandle* handle); //deinit circular buffer and turn off can peripheral and gpios
 int can_add_filter(CANHandle* handle, HAL_FDCAN_FilterTypeDef * filter);
-int can_add_global_filter(CANHandle* handle, HAL_FDCAN_FilterTypeDef* filter); 
+int can_add_global_filter(CANHandle* handle, HAL_FDCAN_FilterTypeDef* filter);
 
 //alternatively instead use the HAL filter functions
-//HAL_FDCAN_ConfigGlobalFilter(canHandle->hal_fdcanP, filterTypeDef) 
+//HAL_FDCAN_ConfigGlobalFilter(canHandle->hal_fdcanP, filterTypeDef)
 //HAL_FDCAN_ConfigFilter(ca)
 
 If no filters are set, the default behaviour is to accept all standard and extended frames into the RXFIFO0
 
-
-
-
-
-PROBLEMS: 
+PROBLEMS:
 Verify ISR safety, no race conditions, atomic read/writes
    - Interrupts keep firing while trying to can_release()
     - Could try to set the NVIC register to selectively disable interrupts (preferably using a bitmask)
@@ -29,31 +25,27 @@ Verify ISR safety, no race conditions, atomic read/writes
     - particularly can_start, can_stop
     - can_release
 - Freeing within ISRs whenever popping from CircularBuffer (yes its faster, than stack copies, but heap is getting fragmented)
-- ISRS might take too long to resolve because popping and freeing circular buffer. 
+- ISRS might take too long to resolve because popping and freeing circular buffer.
 
 - HARDCODE Platform Usage Flag for compiler definitions
-- CAN.H expects #STM32G4 to be defined, 
-
+- CAN.H expects #STM32G4 to be defined,
 
 - RX Callback must perform deep copy of data supplied to it - could also malloc, but not safe to do inside ISRs
--  
-
 
 -Shouldn't disable GPIOs in the MSP layers when releasing, might affect other peripherals
 
-
 IDEAS for other features:
 - abstract to different STM families besides STM32G4
 - Rx Buffering
 - TX Buffering policy, do we spread them out over multiple TX buffers
-- DMA support for copying from circular buffer, circular buffer could then be stack allocated 
+- DMA support for copying from circular buffer, circular buffer could then be stack allocated
 - Smaller can headers for tx and rx (right now its just use the TXHeaderTypeDef)
 - TX FIFO vs Queue policy (only allow FIFOS)
 - Add support for RXFifo1
 
 
 TESTING- ----------------------------------------------
-USE LOGOMATIC for return status - 
+USE LOGOMATIC for return status -
 either returns through semihosting or debug cores
 LOGOMATIC is defined platform by platform
 
@@ -67,7 +59,7 @@ Testing framework
 - use LOGOMATIC to return errors or throw asserts
 
 - Platform testing, such as in G4PERTESTING just needs include "can_test.h" and call top level function
-in main. 
+in main.
 
 Two approaches:
 Platform centric
@@ -75,7 +67,7 @@ Platform centric
 - This approach is better because we can abstract the logging and debug method
 
 Library Centric Testing:
-- Test the implementation in each library. 
+- Test the implementation in each library.
 
 HAL_Rewrite:
 - Alternatively, rewrite without using HAL, just use CMSIS definitions. 
diff --git a/Lib/Peripherals/CAN/Src/can.c b/Lib/Peripherals/CAN/Src/can.c
index 1bea5c95..9663e1a3 100644
--- a/Lib/Peripherals/CAN/Src/can.c
+++ b/Lib/Peripherals/CAN/Src/can.c
@@ -398,7 +398,7 @@ void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
 
 		// stack allocation should be fine? Callback needs to terminate first before stack is popped
 		// should switch this over to malloc at some point to avoid double copies?
-		handle->rx_callback(rx_data, rx_header.DataLength);
+		handle->rx_callback(rx_data, rx_header.DataLength, rx_header.Identifier);
 	}
 
 	/* whoopsie, don't need the rx buffer yet
diff --git a/Lib/Peripherals/CAN/Test/can_tests.c b/Lib/Peripherals/CAN/Test/can_tests.c
index 6e436cc4..79b4b5e5 100644
--- a/Lib/Peripherals/CAN/Test/can_tests.c
+++ b/Lib/Peripherals/CAN/Test/can_tests.c
@@ -12,17 +12,16 @@ int can_test_instance(FDCAN_HandleTypeDef fdcan_handle)
 	return 0;
 }
 
-void can_test_rx_callback2(void *data, uint32_t size)
+void can_test_rx_callback2(void *data, uint32_t size, uint32_t ID)
 {
-	LOGOMATIC("CAN2 Got data! Size %ld, data[0] = 0x%x\n", size, *(char *)data);
+	LOGOMATIC("CAN2 Got data! Size %ld, ID 0x%lx, data[0] = 0x%x\n", size, ID, *(char *)data);
 	// Is within an ISR, so needs to exit quickly
 	return;
 }
 
-void can_test_rx_callback1(void *data, uint32_t size)
+void can_test_rx_callback1(void *data, uint32_t size, uint32_t ID)
 {
-	LOGOMATIC("CAN1 Got data! Size %ld, data[0] = 0x%x\n", size, *(char *)data);
-
+	LOGOMATIC("CAN1 Got data! Size %ld, ID 0x%lx, data[0] = 0x%x\n", size, ID, *(char *)data);
 	// Is within an ISR, so needs to exit quickly
 	return;
 }
@@ -73,7 +72,7 @@ int can_test(void)
 	// Not testing filters at the moment
 	// FDCAN_FilterTypeDef filter;
 
-	// can_add_filter(can2Handle, &filter);
+	// can_add_filter(data_can, &filter);
 	/* USER CODE END 2 */
 
 	FDCAN_TxHeaderTypeDef TxHeader = {
@@ -109,10 +108,10 @@ int can_test(void)
 
 	canCfg.rx_callback = can_test_rx_callback1; // PLEASE SET
 
-	CANHandle *can1Handle = can_init(&canCfg);
-	HAL_FDCAN_ConfigGlobalFilter(can1Handle->hal_fdcanP, 0, 0, 0, 0);
+	CANHandle *primary_can = can_init(&canCfg);
+	HAL_FDCAN_ConfigGlobalFilter(primary_can->hal_fdcanP, 0, 0, 0, 0);
 
-	can_start(can1Handle);
+	can_start(primary_can);
 
 #endif
 #ifdef FDCAN2
@@ -136,21 +135,21 @@ int can_test(void)
 	// filter.FilterID1 = 0x00;
 	// filter.FilterID2 = 0x02;
 
-	CANHandle *can2Handle = can_init(&canCfg);
+	CANHandle *data_can = can_init(&canCfg);
 
 	// accept unmatched standard and extended frames into RXFIFO0 - default behaviour
-	HAL_FDCAN_ConfigGlobalFilter(can2Handle->hal_fdcanP, 0, 0, 0, 0);
+	HAL_FDCAN_ConfigGlobalFilter(data_can->hal_fdcanP, 0, 0, 0, 0);
 
 	// not accepting filters
-	// can_add_filter(can2Handle, &filter);
+	// can_add_filter(data_can, &filter);
 
 	// API Testing
 	// can_init(&canCfg);
 
-	can_start(can2Handle);
+	can_start(data_can);
 
-	can_send(can2Handle, &msg);
-	// can_release(can2Handle);
+	can_send(data_can, &msg);
+	// can_release(data_can);
 
 #endif
 #ifdef FDCAN3
@@ -160,10 +159,10 @@ int can_test(void)
 	while (1) {
 		HAL_Delay(1000);
 		msg.data[0] = 0x2;
-		can_send(can1Handle, &msg);
+		can_send(primary_can, &msg);
 		HAL_Delay(1000);
 		msg.data[0] = 0x10;
-		can_send(can2Handle, &msg);
+		can_send(data_can, &msg);
 	}
 
 	return 0;