diff --git a/Software/Software.ino b/Software/Software.ino
index 06809a25..24b47835 100644
--- a/Software/Software.ino
+++ b/Software/Software.ino
@@ -54,6 +54,7 @@ unsigned long previousMillisUpdateVal = 0;
 // CAN parameters
 CAN_device_t CAN_cfg;          // CAN Config
 const int rx_queue_size = 10;  // Receive Queue size
+volatile bool send_ok = 0;
 
 #ifdef DUAL_CAN
 #include "src/lib/pierremolinaro-acan2515/ACAN2515.h"
@@ -944,7 +945,10 @@ void transmit_can(CAN_frame* tx_frame, int interface) {
       for (uint8_t i = 0; i < MCP2518Frame.len; i++) {
         MCP2518Frame.data[i] = tx_frame->data.u8[i];
       }
-      canfd.tryToSend(MCP2518Frame);
+      send_ok = canfd.tryToSend(MCP2518Frame);
+      if (!send_ok) {
+        set_event(EVENT_CANFD_BUFFER_FULL, interface);
+      }
 #else   // Interface not compiled, and settings try to use it
       set_event(EVENT_INTERFACE_MISSING, interface);
 #endif  //CAN_FD
diff --git a/Software/src/battery/KIA-E-GMP-BATTERY.cpp b/Software/src/battery/KIA-E-GMP-BATTERY.cpp
index 1f553511..2c4204f2 100644
--- a/Software/src/battery/KIA-E-GMP-BATTERY.cpp
+++ b/Software/src/battery/KIA-E-GMP-BATTERY.cpp
@@ -6,13 +6,14 @@
 #include "KIA-E-GMP-BATTERY.h"
 
 /* Do not change code below unless you are sure what you are doing */
+static unsigned long previousMillis10ms = 0;   // will store last time a 10ms CAN Message was send
 static unsigned long previousMillis20ms = 0;   // will store last time a 20ms CAN Message was send
 static unsigned long previousMillis30ms = 0;   // will store last time a 30ms CAN Message was send
 static unsigned long previousMillis100ms = 0;  // will store last time a 100ms CAN Message was send
 static unsigned long previousMillis200ms = 0;  // will store last time a 200ms CAN Message was send
 static unsigned long previousMillis500ms = 0;  // will store last time a 500ms CAN Message was send
 static unsigned long previousMillis1s = 0;     // will store last time a 1s CAN Message was send
-static unsigned long previousMillis2s = 0;     // will store last time a 2s CAN Message was send
+static unsigned long previousMillis10s = 0;    // will store last time a 10s CAN Message was send
 
 #define MAX_CELL_VOLTAGE 4250  //Battery is put into emergency stop if one cell goes over this value
 #define MIN_CELL_VOLTAGE 2950  //Battery is put into emergency stop if one cell goes below this value
@@ -58,7 +59,8 @@ static uint8_t batteryManagementMode = 0;
 static uint8_t BMS_ign = 0;
 static uint8_t batteryRelay = 0;
 static uint8_t waterleakageSensor = 164;
-static uint8_t startedUp = false;
+static bool startedUp = false;
+static bool ok_start_polling_battery = false;
 static uint8_t counter_200 = 0;
 static uint8_t KIA_7E4_COUNTER = 0x01;
 static int8_t temperature_water_inlet = 0;
@@ -69,6 +71,522 @@ static uint8_t ticks_200ms_counter = 0;
 static uint8_t EGMP_1CF_counter = 0;
 static uint8_t EGMP_3XF_counter = 0;
 
+/* These messages are needed for contactor closing */
+unsigned long startMillis;
+uint8_t messageIndex = 0;
+uint8_t messageDelays[63] = {0,   0,   5,   10,  10,  15,  19,  19,  20,  20,  25,  30,  30,  35,  40,  40,
+                             45,  49,  49,  50,  50,  52,  53,  53,  54,  55,  60,  60,  65,  67,  67,  70,
+                             70,  75,  77,  77,  80,  80,  85,  90,  90,  95,  100, 100, 105, 110, 110, 115,
+                             119, 119, 120, 120, 125, 130, 130, 135, 140, 140, 145, 149, 149, 150, 150};
+CAN_frame message_1 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0x62, 0x36, 0x8c, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_2 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0xd4, 0x1b, 0x8c, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_3 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0x24, 0x9b, 0x7b, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_4 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0x24, 0x6f, 0x8d, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_5 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0x92, 0x42, 0x8d, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_6 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0xd7, 0x05, 0x7c, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_7 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x30A,
+    .data = {0xb1, 0xe0, 0x26, 0x08, 0x54, 0x01, 0x04, 0x15, 0x00, 0x1a, 0x76, 0x00, 0x25, 0x01, 0x10, 0x27,
+             0x4f, 0x06, 0x18, 0x04, 0x33, 0x15, 0x34, 0x28, 0x00, 0x00, 0x10, 0x06, 0x21, 0x00, 0x4b, 0x06}};
+
+CAN_frame message_8 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x320,
+    .data = {0xc6, 0xab, 0x26, 0x41, 0x00, 0x00, 0x01, 0x3c, 0xac, 0x0d, 0x40, 0x20, 0x05, 0xc8, 0xa0, 0x03,
+             0x40, 0x20, 0x2b, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_9 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0xee, 0x84, 0x8e, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_10 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0x58, 0xa9, 0x8e, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+CAN_frame message_11 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0x91, 0x5c, 0x7d, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_12 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0xa8, 0xdd, 0x8f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_13 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0x1e, 0xf0, 0x8f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_14 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0x5b, 0xb7, 0x7e, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_15 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0xec, 0x6d, 0x90, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_16 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0x5a, 0x40, 0x90, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_17 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0x1d, 0xee, 0x7f, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_18 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x2B5,
+    .data = {0xbd, 0xb2, 0x42, 0x00, 0x00, 0x00, 0x00, 0x80, 0x59, 0x00, 0x2b, 0x00, 0x00, 0x04, 0x00, 0x00,
+             0xfa, 0xd0, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x8f, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_19 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x2E0,
+    .data = {0xc1, 0xf2, 0x42, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x00, 0x00,
+             0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x70, 0x01, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_20 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0xaa, 0x34, 0x91, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+CAN_frame message_21 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0x1c, 0x19, 0x91, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_22 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x2D5,
+    .data = {0x79, 0xfb, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0b, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00,
+             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x34, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_23 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x2EA,
+    .data = {0x6e, 0xbb, 0xa0, 0x0d, 0x04, 0x01, 0x00, 0x00, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+             0xc7, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_24 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x306,
+    .data = {0x00, 0x00, 0x00, 0xd2, 0x06, 0x92, 0x05, 0x34, 0x07, 0x8e, 0x08, 0x73, 0x05, 0x80, 0x05, 0x83,
+             0x05, 0x73, 0x05, 0x80, 0x05, 0xed, 0x01, 0xdd, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_25 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x308,
+    .data = {0xbe, 0x84, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff,
+             0x75, 0x6c, 0x86, 0x0d, 0xfb, 0xdf, 0x03, 0x36, 0xc3, 0x86, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_26 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0x6b, 0xa2, 0x80, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_27 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0x60, 0xdf, 0x92, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_28 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0xd6, 0xf2, 0x92, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_29 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0x2d, 0xfb, 0x81, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_30 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x33A,
+    .data = {0x1a, 0x23, 0x26, 0x10, 0x27, 0x4f, 0x06, 0x00, 0xf8, 0x1b, 0x19, 0x04, 0x30, 0x01, 0x00, 0x06,
+             0x00, 0x00, 0x00, 0x2e, 0x2d, 0x81, 0x25, 0x20, 0x00, 0x42, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+CAN_frame message_31 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x350,
+    .data = {0x26, 0x82, 0x26, 0xf4, 0x01, 0x00, 0x00, 0x50, 0x90, 0x15, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_32 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0x26, 0x86, 0x93, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_33 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0x90, 0xab, 0x93, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_34 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0xe7, 0x10, 0x82, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_35 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x2E5,
+    .data = {0x69, 0x8a, 0x3f, 0x01, 0x00, 0x00, 0x00, 0x15, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_36 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x3B5,
+    .data = {0xa3, 0xc8, 0x9f, 0x00, 0x00, 0x00, 0x00, 0x36, 0x37, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+             0xc7, 0x02, 0x00, 0x00, 0x00, 0x00, 0x6a, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_37 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0xd5, 0x18, 0x94, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_38 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0x63, 0x35, 0x94, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_39 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0xa1, 0x49, 0x83, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_40 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0x93, 0x41, 0x95, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+CAN_frame message_41 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0x25, 0x6c, 0x95, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_42 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0x52, 0xd7, 0x84, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_43 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0x59, 0xaa, 0x96, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_44 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0xef, 0x87, 0x96, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_45 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0x14, 0x8e, 0x85, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_46 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0x1f, 0xf3, 0x97, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_47 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0xa9, 0xde, 0x97, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_48 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0xde, 0x65, 0x86, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_49 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x30A,
+    .data = {0xd3, 0x11, 0x27, 0x08, 0x54, 0x01, 0x04, 0x15, 0x00, 0x1a, 0x76, 0x00, 0x25, 0x01, 0x10, 0x27,
+             0x4f, 0x06, 0x19, 0x04, 0x33, 0x15, 0x34, 0x28, 0x00, 0x00, 0x10, 0x06, 0x21, 0x00, 0x4b, 0x06}};
+
+CAN_frame message_50 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x320,
+    .data = {0x80, 0xf2, 0x27, 0x41, 0x00, 0x00, 0x01, 0x3c, 0xac, 0x0d, 0x40, 0x20, 0x05, 0xc8, 0xa0, 0x03,
+             0x40, 0x20, 0x2b, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+CAN_frame message_51 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0x9e, 0x87, 0x98, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_52 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0x28, 0xaa, 0x98, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_53 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0x98, 0x3c, 0x87, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_54 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0xd8, 0xde, 0x99, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_55 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0x6e, 0xf3, 0x99, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_56 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0x19, 0x48, 0x88, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_57 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0x12, 0x35, 0x9a, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_58 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x120,
+    .data = {0xa4, 0x18, 0x9a, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x37, 0x35, 0x37, 0x37,
+             0xc9, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_59 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x19A,
+    .data = {0x5f, 0x11, 0x89, 0x55, 0x44, 0x64, 0xd8, 0x1b, 0x40, 0x20, 0x00, 0x00, 0x00, 0x00, 0x11, 0x52,
+             0x00, 0x12, 0x02, 0x64, 0x00, 0x00, 0x00, 0x08, 0x13, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00}};
+
+CAN_frame message_60 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x2B5,
+    .data = {0xfb, 0xeb, 0x43, 0x00, 0x00, 0x00, 0x00, 0x80, 0x59, 0x00, 0x2b, 0x00, 0x00, 0x04, 0x00, 0x00,
+             0xfa, 0xd0, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x8f, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_61 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x2C0,
+    .data = {0xcc, 0xcd, 0xa2, 0x21, 0x00, 0xa1, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7d, 0x00,
+             0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_62 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x2E0,
+    .data = {0x87, 0xab, 0x43, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x00, 0x00,
+             0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x70, 0x01, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+CAN_frame message_63 = {
+    .FD = true,
+    .ext_ID = false,
+    .DLC = 32,
+    .ID = 0x10A,
+    .data = {0x54, 0x6c, 0x9b, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x01, 0x00, 0x00, 0x36, 0x39, 0x35, 0x35,
+             0xc9, 0x02, 0x00, 0x00, 0x10, 0x00, 0x00, 0x35, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00}};
+CAN_frame* messages[] = {&message_1,  &message_2,  &message_3,  &message_4,  &message_5,  &message_6,  &message_7,
+                         &message_8,  &message_9,  &message_10, &message_11, &message_12, &message_13, &message_14,
+                         &message_15, &message_16, &message_17, &message_18, &message_19, &message_20, &message_21,
+                         &message_22, &message_23, &message_24, &message_25, &message_26, &message_27, &message_28,
+                         &message_29, &message_30, &message_31, &message_32, &message_33, &message_34, &message_35,
+                         &message_36, &message_37, &message_38, &message_39, &message_40, &message_41, &message_42,
+                         &message_43, &message_44, &message_45, &message_46, &message_47, &message_48, &message_49,
+                         &message_50, &message_51, &message_52, &message_53, &message_54, &message_55, &message_56,
+                         &message_57, &message_58, &message_59, &message_60, &message_61, &message_62, &message_63};
+
+/* These messages are rest of the vehicle messages, to reduce number of active fault codes */
 CAN_frame EGMP_1CF = {.FD = true,
                       .ext_ID = false,
                       .DLC = 8,
@@ -417,6 +935,7 @@ void update_values_battery() {  //This function maps all the values fetched via
 }
 
 void receive_can_battery(CAN_frame rx_frame) {
+  startedUp = true;
   switch (rx_frame.ID) {
     case 0x055:
       datalayer.battery.status.CAN_battery_still_alive = CAN_STILL_ALIVE;
@@ -648,208 +1167,66 @@ void receive_can_battery(CAN_frame rx_frame) {
 }
 
 void send_can_battery() {
-
   unsigned long currentMillis = millis();
+  if (startedUp) {
+    //Send Contactor closing message loop
+    // Check if we still have messages to send
+    if (messageIndex < sizeof(messageDelays) / sizeof(messageDelays[0])) {
 
-  //Send 20ms CANFD message
-  if (currentMillis - previousMillis20ms >= INTERVAL_20_MS) {
-    previousMillis20ms = currentMillis;
+      // Check if it's time to send the next message
+      if (currentMillis - startMillis >= messageDelays[messageIndex]) {
 
-    EGMP_1CF.data.u8[1] = (EGMP_1CF_counter % 15) * 0x10;
-    EGMP_1CF_counter++;
-    if (EGMP_1CF_counter > 0xE) {
-      EGMP_1CF_counter = 0;
-    }
-    EGMP_1CF.data.u8[0] = calculateCRC(EGMP_1CF, EGMP_1CF.DLC, 0x0A);  // Set CRC bit, initial Value 0x0A
+        // Transmit the current message
+        transmit_can(messages[messageIndex], can_config.battery);
 
-    transmit_can(&EGMP_1CF, can_config.battery);
-  }
-
-  //Send 30ms CANFD message
-  if (currentMillis - previousMillis30ms >= INTERVAL_30_MS) {
-    previousMillis30ms = currentMillis;
-
-    transmit_can(&EGMP_419, can_config.battery);  // TODO: Handle variations better
-  }
-
-  //Send 100ms CANFD message
-  if (currentMillis - previousMillis100ms >= INTERVAL_100_MS) {
-    previousMillis100ms = currentMillis;
-
-    EGMP_36F.data.u8[1] = ((EGMP_3XF_counter % 15) << 4) + 0x01;
-    EGMP_37F.data.u8[1] = ((EGMP_3XF_counter % 15) << 4);
-    EGMP_3XF_counter++;
-    if (EGMP_3XF_counter > 0xE) {
-      EGMP_3XF_counter = 0;
-    }
-    EGMP_36F.data.u8[0] = calculateCRC(EGMP_36F, EGMP_36F.DLC, 0x8A);  // Set CRC bit, initial Value 0x8A
-    EGMP_37F.data.u8[0] = calculateCRC(EGMP_37F, EGMP_37F.DLC, 0x38);  // Set CRC bit, initial Value 0x38
-
-    transmit_can(&EGMP_36F, can_config.battery);
-    transmit_can(&EGMP_37F, can_config.battery);
-  }
-
-  //Send 200ms CANFD message
-  if (currentMillis - previousMillis200ms >= INTERVAL_200_MS) {
-    previousMillis200ms = currentMillis;
-
-    transmit_can(&EGMP_4B4, can_config.battery);
-    transmit_can(&EGMP_4B5, can_config.battery);
-    transmit_can(&EGMP_4B7, can_config.battery);
-    transmit_can(&EGMP_4CC, can_config.battery);
-    transmit_can(&EGMP_4CE, can_config.battery);
-    transmit_can(&EGMP_4D8, can_config.battery);
-    transmit_can(&EGMP_4DD, can_config.battery);
-    transmit_can(&EGMP_4E7, can_config.battery);
-    transmit_can(&EGMP_4E9, can_config.battery);
-    transmit_can(&EGMP_4EA, can_config.battery);
-    transmit_can(&EGMP_4EB, can_config.battery);
-    transmit_can(&EGMP_4EC, can_config.battery);
-    transmit_can(&EGMP_4ED, can_config.battery);
-    transmit_can(&EGMP_4EE, can_config.battery);
-    transmit_can(&EGMP_4EF, can_config.battery);
-    transmit_can(&EGMP_641, can_config.battery);
-    transmit_can(&EGMP_3AA, can_config.battery);
-    transmit_can(&EGMP_3E0, can_config.battery);
-    transmit_can(&EGMP_3E1, can_config.battery);
-    transmit_can(&EGMP_422, can_config.battery);
-    transmit_can(&EGMP_444, can_config.battery);
-    transmit_can(&EGMP_405, can_config.battery);
-    transmit_can(&EGMP_410, can_config.battery);
-    transmit_can(&EGMP_411, can_config.battery);
-    transmit_can(&EGMP_412, can_config.battery);
-    transmit_can(&EGMP_412, can_config.battery);
-    transmit_can(&EGMP_413, can_config.battery);
-    transmit_can(&EGMP_414, can_config.battery);
-    transmit_can(&EGMP_416, can_config.battery);
-    transmit_can(&EGMP_417, can_config.battery);
-    transmit_can(&EGMP_418, can_config.battery);
-    transmit_can(&EGMP_3C1, can_config.battery);
-    transmit_can(&EGMP_3C2, can_config.battery);
-    transmit_can(&EGMP_4F0, can_config.battery);  //TODO: could be handled better
-    transmit_can(&EGMP_4F2, can_config.battery);  //TODO: could be handled better
-
-    if (ticks_200ms_counter < 254) {
-      ticks_200ms_counter++;
-    }
-    if (ticks_200ms_counter > 11) {
-      EGMP_412.data.u8[0] = 0x48;
-      EGMP_412.data.u8[1] = 0x10;
-      EGMP_412.data.u8[6] = 0x04;
-
-      EGMP_418.data.u8[0] = 0xCE;
-      EGMP_418.data.u8[1] = 0x30;
-      EGMP_418.data.u8[4] = 0x14;
-      EGMP_418.data.u8[5] = 0x4C;
-      if (ticks_200ms_counter > 39) {
-        EGMP_412.data.u8[0] = 0xB3;
-        EGMP_412.data.u8[1] = 0x20;
-        EGMP_412.data.u8[6] = 0x00;
-
-        EGMP_418.data.u8[0] = 0xA6;
-        EGMP_418.data.u8[1] = 0x40;
-        EGMP_418.data.u8[5] = 0x0C;
+        // Move to the next message
+        messageIndex++;
       }
     }
-    if (ticks_200ms_counter > 20) {
-      EGMP_413.data.u8[0] = 0xF5;
-      EGMP_413.data.u8[1] = 0x10;
-      EGMP_413.data.u8[3] = 0x41;
-    }
-    if (ticks_200ms_counter > 28) {
-      EGMP_4B4.data.u8[2] = 0;
-      EGMP_4B4.data.u8[3] = 0;
-    }
-    if (ticks_200ms_counter > 26) {
-      EGMP_411.data.u8[0] = 0x9E;
-      EGMP_411.data.u8[1] = 0x32;
-      EGMP_411.data.u8[7] = 0x50;
 
-      EGMP_417.data.u8[0] = 0x9E;
-      EGMP_417.data.u8[1] = 0x20;
-      EGMP_417.data.u8[4] = 0x04;
-      EGMP_417.data.u8[5] = 0x01;
+    if (messageIndex >= 63) {
+      startMillis = millis();  // Start over!
+      messageIndex = 0;
     }
-    if (ticks_200ms_counter > 32) {
-      EGMP_4CE.data.u8[0] = 0x22;
-      EGMP_4CE.data.u8[1] = 0x41;
-      EGMP_4CE.data.u8[6] = 0x47;
-      EGMP_4CE.data.u8[7] = 0x1F;
-    }
-    if (ticks_200ms_counter > 43) {
-      EGMP_4EB.data.u8[2] = 0x0D;
-      EGMP_4EB.data.u8[3] = 0x3B;
-    }
-    if (ticks_200ms_counter > 46) {
-      EGMP_4EB.data.u8[2] = 0x0E;
-      EGMP_4EB.data.u8[3] = 0x00;
-    }
-    if (ticks_200ms_counter > 24) {
-      EGMP_4ED.data.u8[1] = 0x00;
-      EGMP_4ED.data.u8[2] = 0x00;
-      EGMP_4ED.data.u8[3] = 0x00;
-      EGMP_4ED.data.u8[4] = 0x00;
-    }
-    if (ticks_200ms_counter > 21) {
-      EGMP_3E1.data.u8[0] = 0x49;
-      EGMP_3E1.data.u8[1] = 0x10;
-      EGMP_3E1.data.u8[2] = 0x12;
-      EGMP_3E1.data.u8[3] = 0x15;
 
-      EGMP_422.data.u8[0] = 0xEE;
-      EGMP_422.data.u8[1] = 0x20;
-      EGMP_422.data.u8[2] = 0x11;
-      EGMP_422.data.u8[6] = 0x04;
+    //Send 500ms CANFD message
+    if (currentMillis - previousMillis500ms >= INTERVAL_500_MS) {
+      previousMillis500ms = currentMillis;
+      // Check if sending of CAN messages has been delayed too much.
+      if ((currentMillis - previousMillis500ms >= INTERVAL_500_MS_DELAYED) && (currentMillis > BOOTUP_TIME)) {
+        set_event(EVENT_CAN_OVERRUN, (currentMillis - previousMillis500ms));
+      } else {
+        clear_event(EVENT_CAN_OVERRUN);
+      }
+      previousMillis500ms = currentMillis;
 
-      EGMP_405.data.u8[0] = 0xD2;
-      EGMP_405.data.u8[1] = 0x10;
-      EGMP_405.data.u8[5] = 0x01;
-    }
-    if (ticks_200ms_counter > 12) {
-      EGMP_444.data.u8[0] = 0xEE;
-      EGMP_444.data.u8[1] = 0x30;
-      EGMP_444.data.u8[3] = 0x20;
-      if (ticks_200ms_counter > 23) {  // TODO: Could be handled better
-        EGMP_444.data.u8[0] = 0xE4;
-        EGMP_444.data.u8[1] = 0x60;
-        EGMP_444.data.u8[2] = 0x25;
-        EGMP_444.data.u8[3] = 0x4E;
-        EGMP_444.data.u8[4] = 0x04;
+      EGMP_7E4.data.u8[3] = KIA_7E4_COUNTER;
+
+      if (ok_start_polling_battery) {
+        transmit_can(&EGMP_7E4, can_config.battery);
+      }
+
+      KIA_7E4_COUNTER++;
+      if (KIA_7E4_COUNTER > 0x0D) {  // gets up to 0x010C before repeating
+        KIA_7E4_COUNTER = 0x01;
       }
     }
-  }
-
-  //Send 500ms CANFD message
-  if (currentMillis - previousMillis500ms >= INTERVAL_500_MS) {
-
-    // Check if sending of CAN messages has been delayed too much.
-    if ((currentMillis - previousMillis500ms >= INTERVAL_500_MS_DELAYED) && (currentMillis > BOOTUP_TIME)) {
-      set_event(EVENT_CAN_OVERRUN, (currentMillis - previousMillis500ms));
-    } else {
-      clear_event(EVENT_CAN_OVERRUN);
+    //Send 1s CANFD message
+    if (currentMillis - previousMillis1s >= INTERVAL_1_S) {
+      previousMillis1s = currentMillis;
+      /* COMMENTED OUT WHILE CONTACTOR CLOSING TESTING
+        transmit_can(&EGMP_48F, can_config.battery);
+        */
     }
-    previousMillis500ms = currentMillis;
-
-    EGMP_7E4.data.u8[3] = KIA_7E4_COUNTER;
-    transmit_can(&EGMP_7E4, can_config.battery);
-
-    KIA_7E4_COUNTER++;
-    if (KIA_7E4_COUNTER > 0x0D) {  // gets up to 0x010C before repeating
-      KIA_7E4_COUNTER = 0x01;
+    //Send 10s CANFD message
+    if (currentMillis - previousMillis10s >= INTERVAL_10_S) {
+      previousMillis10s = currentMillis;
+      /* COMMENTED OUT WHILE CONTACTOR CLOSING TESTING
+        transmit_can(&EGMP_4FE, can_config.battery);
+        */
+      ok_start_polling_battery = true;
     }
   }
-  //Send 1s CANFD message
-  if (currentMillis - previousMillis1s >= INTERVAL_1_S) {
-    previousMillis1s = currentMillis;
-
-    transmit_can(&EGMP_48F, can_config.battery);
-  }
-  //Send 2s CANFD message
-  if (currentMillis - previousMillis2s >= INTERVAL_2_S) {
-    previousMillis2s = currentMillis;
-
-    transmit_can(&EGMP_4FE, can_config.battery);
-  }
 }
 
 void setup_battery(void) {  // Performs one time setup at startup
@@ -857,6 +1234,8 @@ void setup_battery(void) {  // Performs one time setup at startup
   Serial.println("Hyundai E-GMP (Electric Global Modular Platform) battery selected");
 #endif
 
+  startMillis = millis();  // Record the starting time
+
   datalayer.system.status.battery_allows_contactor_closing = true;
 
   datalayer.battery.info.number_of_cells = 192;  // TODO: will vary depending on battery
diff --git a/Software/src/devboard/utils/events.cpp b/Software/src/devboard/utils/events.cpp
index 5a438362..ed41da11 100644
--- a/Software/src/devboard/utils/events.cpp
+++ b/Software/src/devboard/utils/events.cpp
@@ -141,6 +141,7 @@ void init_events(void) {
   }
 
   events.entries[EVENT_CANFD_INIT_FAILURE].level = EVENT_LEVEL_WARNING;
+  events.entries[EVENT_CANFD_BUFFER_FULL].level = EVENT_LEVEL_WARNING;
   events.entries[EVENT_CAN_OVERRUN].level = EVENT_LEVEL_INFO;
   events.entries[EVENT_CAN_RX_FAILURE].level = EVENT_LEVEL_ERROR;
   events.entries[EVENT_CAN2_RX_FAILURE].level = EVENT_LEVEL_WARNING;
@@ -243,6 +244,8 @@ const char* get_event_message_string(EVENTS_ENUM_TYPE event) {
   switch (event) {
     case EVENT_CANFD_INIT_FAILURE:
       return "CAN-FD initialization failed. Check hardware or bitrate settings";
+    case EVENT_CANFD_BUFFER_FULL:
+      return "CAN-FD buffer overflowed. Some CAN messages were not sent. Contact developers.";
     case EVENT_CAN_OVERRUN:
       return "CAN message failed to send within defined time. Contact developers, CPU load might be too high.";
     case EVENT_CAN_RX_FAILURE:
diff --git a/Software/src/devboard/utils/events.h b/Software/src/devboard/utils/events.h
index c911b72b..570aa302 100644
--- a/Software/src/devboard/utils/events.h
+++ b/Software/src/devboard/utils/events.h
@@ -6,7 +6,7 @@
 
 // #define INCLUDE_EVENTS_TEST  // Enable to run an event test loop, see events_test_on_target.cpp
 
-#define EE_MAGIC_HEADER_VALUE 0x0012  // 0x0000 to 0xFFFF
+#define EE_MAGIC_HEADER_VALUE 0x0013  // 0x0000 to 0xFFFF
 
 #define GENERATE_ENUM(ENUM) ENUM,
 #define GENERATE_STRING(STRING) #STRING,
@@ -27,6 +27,7 @@
 
 #define EVENTS_ENUM_TYPE(XX)            \
   XX(EVENT_CANFD_INIT_FAILURE)          \
+  XX(EVENT_CANFD_BUFFER_FULL)           \
   XX(EVENT_CAN_OVERRUN)                 \
   XX(EVENT_CAN_RX_FAILURE)              \
   XX(EVENT_CAN2_RX_FAILURE)             \
diff --git a/Software/src/lib/pierremolinaro-ACAN2517FD/ACAN2517FDSettings.h b/Software/src/lib/pierremolinaro-ACAN2517FD/ACAN2517FDSettings.h
index 8e27392f..ec86560c 100644
--- a/Software/src/lib/pierremolinaro-ACAN2517FD/ACAN2517FDSettings.h
+++ b/Software/src/lib/pierremolinaro-ACAN2517FD/ACAN2517FDSettings.h
@@ -160,7 +160,7 @@ class ACAN2517FDSettings {
 //······················································································································
 
 //--- Driver transmit buffer size
-  public: uint16_t mDriverTransmitFIFOSize = 16 ; // >= 0
+  public: uint16_t mDriverTransmitFIFOSize = 22 ; // >= 0
 
 //--- Controller transmit FIFO size
   public: uint8_t mControllerTransmitFIFOSize = 1 ; // 1 ... 32