diff --git a/Software/BYD-MODBUS-2-LEAF-CAN.ino b/Software/BYD-MODBUS-2-LEAF-CAN.ino
index 26168434..09468f2e 100644
--- a/Software/BYD-MODBUS-2-LEAF-CAN.ino
+++ b/Software/BYD-MODBUS-2-LEAF-CAN.ino
@@ -20,6 +20,10 @@ const int rx_queue_size = 10; // Receive Queue size
 byte mprun10 = 0; //counter 0-3
 byte mprun100 = 0; //counter 0-3
 
+CAN_frame_t LEAF_1F2 = {.MsgID = 0x1F2, LEAF_1F2.FIR.B.DLC = 8, LEAF_1F2.FIR.B.FF = CAN_frame_std, LEAF_1F2.data.u8[0] = 0x64, LEAF_1F2.data.u8[1] = 0x64,LEAF_1F2.data.u8[2] = 0x32, LEAF_1F2.data.u8[3] = 0xA0,LEAF_1F2.data.u8[4] = 0x00,LEAF_1F2.data.u8[5] = 0x0A};
+CAN_frame_t LEAF_50B = {.MsgID = 0x50B, LEAF_50B.FIR.B.DLC = 8, LEAF_50B.FIR.B.FF = CAN_frame_std, LEAF_50B.data.u8[0] = 0x00, LEAF_50B.data.u8[1] = 0x00,LEAF_50B.data.u8[2] = 0x06, LEAF_50B.data.u8[3] = 0xC0,LEAF_50B.data.u8[4] = 0x00,LEAF_50B.data.u8[5] = 0x00};
+CAN_frame_t LEAF_50C = {.MsgID = 0x50C, LEAF_50C.FIR.B.DLC = 8, LEAF_50C.FIR.B.FF = CAN_frame_std, LEAF_50C.data.u8[0] = 0x00, LEAF_50C.data.u8[1] = 0x00,LEAF_50C.data.u8[2] = 0x00, LEAF_50C.data.u8[3] = 0x00,LEAF_50C.data.u8[4] = 0x00,LEAF_50C.data.u8[5] = 0x00};
+
 //Nissan LEAF battery parameters from CAN
 #define WH_PER_GID 77 //One GID is this amount of Watt hours
 int LB_Discharge_Power_Limit = 0; //Limit in kW
@@ -77,7 +81,6 @@ uint16_t p1001_data[] = {
 	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
 };
 uint16_t i;
-static unsigned long currentMillis;
 
 // Create a ModbusRTU server instance listening on Serial2 with 2000ms timeout
 ModbusServerRTU MBserver(Serial2, 2000);
@@ -129,12 +132,11 @@ void setup()
 void loop()
 {
 	handle_can();
-	update_values();
-	currentMillis = millis();
-	if (currentMillis - previousMillisModbus >= intervalModbusTask)
+  //every 10s
+	if (millis() - previousMillisModbus >= intervalModbusTask)
 	{
-		//every 10s
-		previousMillisModbus = currentMillis;
+		previousMillisModbus = millis();
+    update_values();
 		handle_UpdateDataModbus();
 	}
 }
@@ -206,19 +208,19 @@ void handle_UpdateDataModbus()
 
 void handle_can()
 {
-	CAN_frame_t rx_frame;
+  CAN_frame_t rx_frame;
+  unsigned long currentMillis = millis();
 
-	static unsigned long currentMillis = millis();
-
-	// Receive next CAN frame from queue
-	if (xQueueReceive(CAN_cfg.rx_queue, &rx_frame, 3 * portTICK_PERIOD_MS) == pdTRUE)
-	{
-		if (rx_frame.FIR.B.FF == CAN_frame_std)
-		{
-			//printf("New standard frame");
-			switch (rx_frame.MsgID)
+  // Receive next CAN frame from queue
+  if (xQueueReceive(CAN_cfg.rx_queue, &rx_frame, 3 * portTICK_PERIOD_MS) == pdTRUE)
+  {
+    if (rx_frame.FIR.B.FF == CAN_frame_std)
+    {
+      //printf("New standard frame");
+      switch (rx_frame.MsgID)
 			{
-			case 0x1DB:
+      case 0x1DB:
+        //printf("1DB \n");
 				LB_Current = (rx_frame.data.u8[0] << 3) | (rx_frame.data.u8[1] & 0xe0) >> 5;
 				LB_Total_Voltage = ((rx_frame.data.u8[2] << 2) | (rx_frame.data.u8[3] & 0xc0) >> 6) / 2;
 				break;
@@ -245,135 +247,86 @@ void handle_can()
 					LB_Wh_Remaining = (LB_GIDS * WH_PER_GID);
 				}
 				break;
-			case 0x59E: //This message is only present on 2013+ AZE0 and upwards
+      default:
 				break;
-			case 0x5C0:
-				//todo read batt temp from here (or from active polling later)
-				break;
-			default:
-				break;
-			}
-		}
-		else
-		{
-			//printf("New extended frame");
-		}
-
-		// if (rx_frame.FIR.B.RTR == CAN_RTR)
-		// {
-		//   printf(" RTR from 0x%08X, DLC %d\r\n", rx_frame.MsgID, rx_frame.FIR.B.DLC);
-		// }
-		// else
-		// {
-		//   printf(" from 0x%08X, DLC %d, Data ", rx_frame.MsgID, rx_frame.FIR.B.DLC);
-		//   for (int i = 0; i < rx_frame.FIR.B.DLC; i++)
-		//   {
-		//     printf("0x%02X ", rx_frame.data.u8[i]);
-		//   }
-		//   printf("\n");
-		// }
-	}
+      }      
+    }
+    else
+    {
+      //printf("New extended frame");
+    }
+  }
 	// Send 100ms CAN Message
 	if (currentMillis - previousMillis100 >= interval100)
 	{
 		previousMillis100 = currentMillis;
+
+    ESP32Can.CANWriteFrame(&LEAF_50B); //Always send 50B as a static message
+
 		mprun100++;
 		if (mprun100 > 3)
 		{
 			mprun100 = 0;
 		}
 
-		CAN_frame_t tx_frame;
-		tx_frame.FIR.B.FF = CAN_frame_std;
-		tx_frame.MsgID = 0x50B;
-		tx_frame.FIR.B.DLC = 8;
-		tx_frame.data.u8[0] = 0x00;
-		tx_frame.data.u8[1] = 0x00;
-		tx_frame.data.u8[2] = 0x06;
-		tx_frame.data.u8[3] = 0xC0; //HCM_WakeUpSleepCmd = Wakeup
-		tx_frame.data.u8[4] = 0x00;
-		tx_frame.data.u8[5] = 0x00;
-		tx_frame.data.u8[6] = 0x00;
-		tx_frame.data.u8[7] = 0x00;
-
-		ESP32Can.CANWriteFrame(&tx_frame);
-		Serial.println("CAN send 50B done");
-
-		tx_frame.MsgID = 0x50C;
-		tx_frame.FIR.B.DLC = 8;
-		tx_frame.data.u8[0] = 0x00;
-		tx_frame.data.u8[1] = 0x00;
-		tx_frame.data.u8[2] = 0x00;
-		tx_frame.data.u8[3] = 0x00;
-		tx_frame.data.u8[4] = 0x00;
 		if (mprun100 == 0)
 		{
-			tx_frame.data.u8[5] = 0x00;
-			tx_frame.data.u8[6] = 0x5D;
-			tx_frame.data.u8[7] = 0xC8;
+			LEAF_50C.data.u8[5] = 0x00;
+			LEAF_50C.data.u8[6] = 0x5D;
+			LEAF_50C.data.u8[7] = 0xC8;
 		}
-		if (mprun100 == 1)
+		else if(mprun100 == 1)
 		{
-			tx_frame.data.u8[5] = 0x01;
-			tx_frame.data.u8[6] = 0x5D;
-			tx_frame.data.u8[7] = 0x5F;
+			LEAF_50C.data.u8[5] = 0x01;
+			LEAF_50C.data.u8[6] = 0x5D;
+			LEAF_50C.data.u8[7] = 0x5F;
 		}
-		if (mprun100 == 2)
+		else if(mprun100 == 2)
 		{
-			tx_frame.data.u8[5] = 0x02;
-			tx_frame.data.u8[6] = 0x5D;
-			tx_frame.data.u8[7] = 0x63;
+			LEAF_50C.data.u8[5] = 0x02;
+			LEAF_50C.data.u8[6] = 0x5D;
+			LEAF_50C.data.u8[7] = 0x63;
 		}
-		if (mprun100 == 3)
+		else if(mprun100 == 3)
 		{
-			tx_frame.data.u8[5] = 0x03;
-			tx_frame.data.u8[6] = 0x5D;
-			tx_frame.data.u8[7] = 0xF4;
+			LEAF_50C.data.u8[5] = 0x03;
+			LEAF_50C.data.u8[6] = 0x5D;
+			LEAF_50C.data.u8[7] = 0xF4;
 		}
-		ESP32Can.CANWriteFrame(&tx_frame);
-		Serial.println("CAN send 50C done");
+		ESP32Can.CANWriteFrame(&LEAF_50C);
 	}
-
+  //Send 10ms message
 	if (currentMillis - previousMillis10 >= interval10)
-	{
+	{ 
 		previousMillis10 = currentMillis;
+
+    if(mprun10 == 0)
+    {
+      LEAF_1F2.data.u8[6] = 0x00;
+      LEAF_1F2.data.u8[7] = 0x8F;
+    }
+    else if(mprun10 == 1)
+    {
+      LEAF_1F2.data.u8[6] = 0x01;
+      LEAF_1F2.data.u8[7] = 0x80;
+    }
+    else if(mprun10 == 2)
+    {
+      LEAF_1F2.data.u8[6] = 0x02;
+      LEAF_1F2.data.u8[7] = 0x81;
+    }
+    else if(mprun10 == 3)
+    {
+      LEAF_1F2.data.u8[6] = 0x03;
+      LEAF_1F2.data.u8[7] = 0x82;
+    }
+    ESP32Can.CANWriteFrame(&LEAF_1F2);
+
 		mprun10++;
 		if (mprun10 > 3)
 		{
 			mprun10 = 0;
 		}
-
-		CAN_frame_t tx_frame;
-		tx_frame.FIR.B.FF = CAN_frame_std;
-		tx_frame.MsgID = 0x1F2;
-		tx_frame.FIR.B.DLC = 8;
-		tx_frame.data.u8[0] = 0x64;
-		tx_frame.data.u8[1] = 0x64;
-		tx_frame.data.u8[2] = 0x32;
-		tx_frame.data.u8[3] = 0xA0;
-		tx_frame.data.u8[4] = 0x00;
-		tx_frame.data.u8[5] = 0x0A;
-		if (mprun10 == 0)
-		{
-			tx_frame.data.u8[6] = 0x00;
-			tx_frame.data.u8[7] = 0x8F;
-		}
-		if (mprun10 == 1)
-		{
-			tx_frame.data.u8[6] = 0x01;
-			tx_frame.data.u8[7] = 0x80;
-		}
-		if (mprun10 == 2)
-		{
-			tx_frame.data.u8[6] = 0x02;
-			tx_frame.data.u8[7] = 0x81;
-		}
-		if (mprun10 == 3)
-		{
-			tx_frame.data.u8[6] = 0x03;
-			tx_frame.data.u8[7] = 0x82;
-		}
-		ESP32Can.CANWriteFrame(&tx_frame);
-		Serial.println("CAN send 1F2 done");
+		//Serial.println("CAN 10ms done");
 	}
-}
+}
\ No newline at end of file
diff --git a/Software/CAN.c b/Software/CAN.c
index 0f0e57ae..b5df46eb 100644
--- a/Software/CAN.c
+++ b/Software/CAN.c
@@ -1,299 +1,300 @@
-/**
- * @section License
- *
- * The MIT License (MIT)
- *
- * Copyright (c) 2017, Thomas Barth, barth-dev.de
- *               2017, Jaime Breva, jbreva@nayarsystems.com
- * 				 2018, Michael Wagner, mw@iot-make.de
- *
- * Permission is hereby granted, free of charge, to any person
- * obtaining a copy of this software and associated documentation
- * files (the "Software"), to deal in the Software without
- * restriction, including without limitation the rights to use, copy,
- * modify, merge, publish, distribute, sublicense, and/or sell copies
- * of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be
- * included in all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
- * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
- * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
- * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- *
- */
-
-#include "CAN.h"
-
-#include "freertos/FreeRTOS.h"
-#include "freertos/queue.h"
-
-#include "esp_intr.h"
-#include "soc/dport_reg.h"
-#include <math.h>
-
-#include "driver/gpio.h"
-
-#include "can_regdef.h"
-#include "CAN_config.h"
-
-// CAN Filter - no acceptance filter
-static CAN_filter_t __filter = { Dual_Mode, 0, 0, 0, 0, 0Xff, 0Xff, 0Xff, 0Xff };
-
-static void CAN_read_frame_phy();
-static void CAN_isr(void *arg_p);
-static int CAN_write_frame_phy(const CAN_frame_t *p_frame);
-static SemaphoreHandle_t sem_tx_complete;
-
-static void CAN_isr(void *arg_p) {
-
-	// Interrupt flag buffer
-	__CAN_IRQ_t interrupt;
-	BaseType_t higherPriorityTaskWoken = pdFALSE;
-
-	// Read interrupt status and clear flags
-	interrupt = MODULE_CAN->IR.U;
-
-	// Handle RX frame available interrupt
-	if ((interrupt & __CAN_IRQ_RX) != 0)
-		CAN_read_frame_phy(&higherPriorityTaskWoken);
-
-	// Handle TX complete interrupt
-	// Handle error interrupts.
-	if ((interrupt & (__CAN_IRQ_TX | __CAN_IRQ_ERR //0x4
-	                  | __CAN_IRQ_DATA_OVERRUN     // 0x8
-	                  | __CAN_IRQ_WAKEUP           // 0x10
-	                  | __CAN_IRQ_ERR_PASSIVE      // 0x20
-	                  | __CAN_IRQ_ARB_LOST         // 0x40
-	                  | __CAN_IRQ_BUS_ERR          // 0x80
-	                  )) != 0) {
-		xSemaphoreGiveFromISR(sem_tx_complete, &higherPriorityTaskWoken);
-	}
-
-	// check if any higher priority task has been woken by any handler
-	if (higherPriorityTaskWoken)
-		portYIELD_FROM_ISR();
-}
-
-static void CAN_read_frame_phy(BaseType_t *higherPriorityTaskWoken) {
-
-	// byte iterator
-	uint8_t __byte_i;
-
-	// frame read buffer
-	CAN_frame_t __frame;
-
-	// check if we have a queue. If not, operation is aborted.
-	if (CAN_cfg.rx_queue == NULL) {
-		// Let the hardware know the frame has been read.
-		MODULE_CAN->CMR.B.RRB = 1;
-		return;
-	}
-
-	// get FIR
-	__frame.FIR.U = MODULE_CAN->MBX_CTRL.FCTRL.FIR.U;
-
-	// check if this is a standard or extended CAN frame
-	// standard frame
-	if (__frame.FIR.B.FF == CAN_frame_std) {
-
-		// Get Message ID
-		__frame.MsgID = _CAN_GET_STD_ID;
-
-		// deep copy data bytes
-		for (__byte_i = 0; __byte_i < __frame.FIR.B.DLC; __byte_i++)
-			__frame.data.u8[__byte_i] = MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.data[__byte_i];
-	}
-	// extended frame
-	else {
-
-		// Get Message ID
-		__frame.MsgID = _CAN_GET_EXT_ID;
-
-		// deep copy data bytes
-		for (__byte_i = 0; __byte_i < __frame.FIR.B.DLC; __byte_i++)
-			__frame.data.u8[__byte_i] = MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.data[__byte_i];
-	}
-
-	// send frame to input queue
-	xQueueSendToBackFromISR(CAN_cfg.rx_queue, &__frame, higherPriorityTaskWoken);
-
-	// Let the hardware know the frame has been read.
-	MODULE_CAN->CMR.B.RRB = 1;
-}
-
-static int CAN_write_frame_phy(const CAN_frame_t *p_frame) {
-
-	// byte iterator
-	uint8_t __byte_i;
-
-	// copy frame information record
-	MODULE_CAN->MBX_CTRL.FCTRL.FIR.U = p_frame->FIR.U;
-
-	// standard frame
-	if (p_frame->FIR.B.FF == CAN_frame_std) {
-
-		// Write message ID
-		_CAN_SET_STD_ID(p_frame->MsgID);
-
-		// Copy the frame data to the hardware
-		for (__byte_i = 0; __byte_i < p_frame->FIR.B.DLC; __byte_i++)
-			MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.data[__byte_i] = p_frame->data.u8[__byte_i];
-
-	}
-	// extended frame
-	else {
-
-		// Write message ID
-		_CAN_SET_EXT_ID(p_frame->MsgID);
-
-		// Copy the frame data to the hardware
-		for (__byte_i = 0; __byte_i < p_frame->FIR.B.DLC; __byte_i++)
-			MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.data[__byte_i] = p_frame->data.u8[__byte_i];
-	}
-
-	// Transmit frame
-	MODULE_CAN->CMR.B.TR = 1;
-
-	return 0;
-}
-
-int CAN_init() {
-
-	// Time quantum
-	double __tq;
-
-	// enable module
-	DPORT_SET_PERI_REG_MASK(DPORT_PERIP_CLK_EN_REG, DPORT_CAN_CLK_EN);
-	DPORT_CLEAR_PERI_REG_MASK(DPORT_PERIP_RST_EN_REG, DPORT_CAN_RST);
-
-	// configure TX pin
-	gpio_set_level(CAN_cfg.tx_pin_id, 1);
-	gpio_set_direction(CAN_cfg.tx_pin_id, GPIO_MODE_OUTPUT);
-	gpio_matrix_out(CAN_cfg.tx_pin_id, CAN_TX_IDX, 0, 0);
-	gpio_pad_select_gpio(CAN_cfg.tx_pin_id);
-
-	// configure RX pin
-	gpio_set_direction(CAN_cfg.rx_pin_id, GPIO_MODE_INPUT);
-	gpio_matrix_in(CAN_cfg.rx_pin_id, CAN_RX_IDX, 0);
-	gpio_pad_select_gpio(CAN_cfg.rx_pin_id);
-
-	// set to PELICAN mode
-	MODULE_CAN->CDR.B.CAN_M = 0x1;
-
-	// synchronization jump width is the same for all baud rates
-	MODULE_CAN->BTR0.B.SJW = 0x1;
-
-	// TSEG2 is the same for all baud rates
-	MODULE_CAN->BTR1.B.TSEG2 = 0x1;
-
-	// select time quantum and set TSEG1
-	switch (CAN_cfg.speed) {
-	case CAN_SPEED_1000KBPS:
-		MODULE_CAN->BTR1.B.TSEG1 = 0x4;
-		__tq = 0.125;
-		break;
-
-	case CAN_SPEED_800KBPS:
-		MODULE_CAN->BTR1.B.TSEG1 = 0x6;
-		__tq = 0.125;
-		break;
-
-	case CAN_SPEED_200KBPS:
-		MODULE_CAN->BTR1.B.TSEG1 = 0xc;
-		MODULE_CAN->BTR1.B.TSEG2 = 0x5;
-		__tq = 0.25;
-		break;
-
-	default:
-		MODULE_CAN->BTR1.B.TSEG1 = 0xc;
-		__tq = ((float) 1000 / CAN_cfg.speed) / 16;
-	}
-
-	// set baud rate prescaler
-	MODULE_CAN->BTR0.B.BRP = (uint8_t) round((((APB_CLK_FREQ * __tq) / 2) - 1) / 1000000) - 1;
-
-	/* Set sampling
-	 * 1 -> triple; the bus is sampled three times; recommended for low/medium speed buses     (class A and B) where
-	 * filtering spikes on the bus line is beneficial 0 -> single; the bus is sampled once; recommended for high speed
-	 * buses (SAE class C)*/
-	MODULE_CAN->BTR1.B.SAM = 0x1;
-
-	// enable all interrupts
-	MODULE_CAN->IER.U = 0xef; //ESP32 V3 0XEF     ESP32 NOT V3 0XFF
-
-	 // Set acceptance filter	
-	MODULE_CAN->MOD.B.AFM = __filter.FM;	
-    MODULE_CAN->MBX_CTRL.ACC.CODE[0] = __filter.ACR0;
-    MODULE_CAN->MBX_CTRL.ACC.CODE[1] = __filter.ACR1;
-    MODULE_CAN->MBX_CTRL.ACC.CODE[2] = __filter.ACR2;
-    MODULE_CAN->MBX_CTRL.ACC.CODE[3] = __filter.ACR3;
-    MODULE_CAN->MBX_CTRL.ACC.MASK[0] = __filter.AMR0;
-    MODULE_CAN->MBX_CTRL.ACC.MASK[1] = __filter.AMR1;
-    MODULE_CAN->MBX_CTRL.ACC.MASK[2] = __filter.AMR2;
-    MODULE_CAN->MBX_CTRL.ACC.MASK[3] = __filter.AMR3;
-
-	// set to normal mode
-	MODULE_CAN->OCR.B.OCMODE = __CAN_OC_NOM;
-
-	// clear error counters
-	MODULE_CAN->TXERR.U = 0;
-	MODULE_CAN->RXERR.U = 0;
-	(void) MODULE_CAN->ECC;
-
-	// clear interrupt flags
-	(void) MODULE_CAN->IR.U;
-
-	// install CAN ISR
-	esp_intr_alloc(ETS_CAN_INTR_SOURCE, 0, CAN_isr, NULL, NULL);
-
-	// allocate the tx complete semaphore
-	sem_tx_complete = xSemaphoreCreateBinary();
-
-	// Showtime. Release Reset Mode.
-	MODULE_CAN->MOD.B.RM = 0;
-
-	return 0;
-}
-
-int CAN_write_frame(const CAN_frame_t *p_frame) {
-	if (sem_tx_complete == NULL) {
-		return -1;
-	}
-
-	// Write the frame to the controller
-	CAN_write_frame_phy(p_frame);
-
-	// wait for the frame tx to complete
-	xSemaphoreTake(sem_tx_complete, portMAX_DELAY);
-
-	return 0;
-}
-
-int CAN_stop() {
-	// enter reset mode
-	MODULE_CAN->MOD.B.RM = 1;
-
-	return 0;
-}
-
-int CAN_config_filter(const CAN_filter_t* p_filter) {
-	
-	__filter.FM = p_filter->FM;	
-    __filter.ACR0 = p_filter->ACR0;
-    __filter.ACR1 = p_filter->ACR1;
-    __filter.ACR2 = p_filter->ACR2;
-    __filter.ACR3 = p_filter->ACR3;
-    __filter.AMR0 = p_filter->AMR0;
-    __filter.AMR1 = p_filter->AMR1;
-    __filter.AMR2 = p_filter->AMR2;
-    __filter.AMR3 = p_filter->AMR3;
-	
-	return 0;
+/**
+ * @section License
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2017, Thomas Barth, barth-dev.de
+ *               2017, Jaime Breva, jbreva@nayarsystems.com
+ * 				 2018, Michael Wagner, mw@iot-make.de
+ *
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use, copy,
+ * modify, merge, publish, distribute, sublicense, and/or sell copies
+ * of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ */
+
+#include "CAN.h"
+
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+
+#include "esp_intr.h"
+#include "soc/dport_reg.h"
+#include <math.h>
+
+#include "driver/gpio.h"
+
+#include "can_regdef.h"
+#include "CAN_config.h"
+
+// CAN Filter - no acceptance filter
+static CAN_filter_t __filter = { Dual_Mode, 0, 0, 0, 0, 0Xff, 0Xff, 0Xff, 0Xff };
+
+static void CAN_read_frame_phy();
+static void CAN_isr(void *arg_p);
+static int CAN_write_frame_phy(const CAN_frame_t *p_frame);
+static SemaphoreHandle_t sem_tx_complete;
+
+static void CAN_isr(void *arg_p) {
+
+	// Interrupt flag buffer
+	__CAN_IRQ_t interrupt;
+	BaseType_t higherPriorityTaskWoken = pdFALSE;
+
+	// Read interrupt status and clear flags
+	interrupt = MODULE_CAN->IR.U;
+
+	// Handle RX frame available interrupt
+	if ((interrupt & __CAN_IRQ_RX) != 0)
+		CAN_read_frame_phy(&higherPriorityTaskWoken);
+
+	// Handle TX complete interrupt
+	// Handle error interrupts.
+	if ((interrupt & (__CAN_IRQ_TX | __CAN_IRQ_ERR //0x4
+	                  | __CAN_IRQ_DATA_OVERRUN     // 0x8
+	                  | __CAN_IRQ_WAKEUP           // 0x10
+	                  | __CAN_IRQ_ERR_PASSIVE      // 0x20
+	                  | __CAN_IRQ_ARB_LOST         // 0x40
+	                  | __CAN_IRQ_BUS_ERR          // 0x80
+	                  )) != 0) {
+		xSemaphoreGiveFromISR(sem_tx_complete, &higherPriorityTaskWoken);
+	}
+
+	// check if any higher priority task has been woken by any handler
+	if (higherPriorityTaskWoken)
+		portYIELD_FROM_ISR();
+}
+
+static void CAN_read_frame_phy(BaseType_t *higherPriorityTaskWoken) {
+
+	// byte iterator
+	uint8_t __byte_i;
+
+	// frame read buffer
+	CAN_frame_t __frame;
+
+	// check if we have a queue. If not, operation is aborted.
+	if (CAN_cfg.rx_queue == NULL) {
+		// Let the hardware know the frame has been read.
+		MODULE_CAN->CMR.B.RRB = 1;
+		return;
+	}
+
+	// get FIR
+	__frame.FIR.U = MODULE_CAN->MBX_CTRL.FCTRL.FIR.U;
+
+	// check if this is a standard or extended CAN frame
+	// standard frame
+	if (__frame.FIR.B.FF == CAN_frame_std) {
+
+		// Get Message ID
+		__frame.MsgID = _CAN_GET_STD_ID;
+
+		// deep copy data bytes
+		for (__byte_i = 0; __byte_i < __frame.FIR.B.DLC; __byte_i++)
+			__frame.data.u8[__byte_i] = MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.data[__byte_i];
+	}
+	// extended frame
+	else {
+
+		// Get Message ID
+		__frame.MsgID = _CAN_GET_EXT_ID;
+
+		// deep copy data bytes
+		for (__byte_i = 0; __byte_i < __frame.FIR.B.DLC; __byte_i++)
+			__frame.data.u8[__byte_i] = MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.data[__byte_i];
+	}
+
+	// send frame to input queue
+	xQueueSendToBackFromISR(CAN_cfg.rx_queue, &__frame, higherPriorityTaskWoken);
+
+	// Let the hardware know the frame has been read.
+	MODULE_CAN->CMR.B.RRB = 1;
+}
+
+static int CAN_write_frame_phy(const CAN_frame_t *p_frame) {
+
+	// byte iterator
+	uint8_t __byte_i;
+
+	// copy frame information record
+	MODULE_CAN->MBX_CTRL.FCTRL.FIR.U = p_frame->FIR.U;
+
+	// standard frame
+	if (p_frame->FIR.B.FF == CAN_frame_std) {
+
+		// Write message ID
+		_CAN_SET_STD_ID(p_frame->MsgID);
+
+		// Copy the frame data to the hardware
+		for (__byte_i = 0; __byte_i < p_frame->FIR.B.DLC; __byte_i++)
+			MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.data[__byte_i] = p_frame->data.u8[__byte_i];
+
+	}
+	// extended frame
+	else {
+
+		// Write message ID
+		_CAN_SET_EXT_ID(p_frame->MsgID);
+
+		// Copy the frame data to the hardware
+		for (__byte_i = 0; __byte_i < p_frame->FIR.B.DLC; __byte_i++)
+			MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.data[__byte_i] = p_frame->data.u8[__byte_i];
+	}
+
+	// Transmit frame
+	MODULE_CAN->CMR.B.TR = 1;
+
+	return 0;
+}
+
+int CAN_init() {
+
+	// Time quantum
+	double __tq;
+
+	// enable module
+	DPORT_SET_PERI_REG_MASK(DPORT_PERIP_CLK_EN_REG, DPORT_CAN_CLK_EN);
+	DPORT_SET_PERI_REG_MASK(DPORT_PERIP_RST_EN_REG, DPORT_CAN_RST); //Added https://github.com/miwagner/ESP32-Arduino-CAN/pull/37/commits/feccb722866fbdcc7628b941efe9f79295b0cf81
+	DPORT_CLEAR_PERI_REG_MASK(DPORT_PERIP_RST_EN_REG, DPORT_CAN_RST);
+
+	// configure TX pin
+	gpio_set_level(CAN_cfg.tx_pin_id, 1);
+	gpio_set_direction(CAN_cfg.tx_pin_id, GPIO_MODE_OUTPUT);
+	gpio_matrix_out(CAN_cfg.tx_pin_id, CAN_TX_IDX, 0, 0);
+	gpio_pad_select_gpio(CAN_cfg.tx_pin_id);
+
+	// configure RX pin
+	gpio_set_direction(CAN_cfg.rx_pin_id, GPIO_MODE_INPUT);
+	gpio_matrix_in(CAN_cfg.rx_pin_id, CAN_RX_IDX, 0);
+	gpio_pad_select_gpio(CAN_cfg.rx_pin_id);
+
+	// set to PELICAN mode
+	MODULE_CAN->CDR.B.CAN_M = 0x1;
+
+	// synchronization jump width is the same for all baud rates
+	MODULE_CAN->BTR0.B.SJW = 0x1;
+
+	// TSEG2 is the same for all baud rates
+	MODULE_CAN->BTR1.B.TSEG2 = 0x1;
+
+	// select time quantum and set TSEG1
+	switch (CAN_cfg.speed) {
+	case CAN_SPEED_1000KBPS:
+		MODULE_CAN->BTR1.B.TSEG1 = 0x4;
+		__tq = 0.125;
+		break;
+
+	case CAN_SPEED_800KBPS:
+		MODULE_CAN->BTR1.B.TSEG1 = 0x6;
+		__tq = 0.125;
+		break;
+
+	case CAN_SPEED_200KBPS:
+		MODULE_CAN->BTR1.B.TSEG1 = 0xc;
+		MODULE_CAN->BTR1.B.TSEG2 = 0x5;
+		__tq = 0.25;
+		break;
+
+	default:
+		MODULE_CAN->BTR1.B.TSEG1 = 0xc;
+		__tq = ((float) 1000 / CAN_cfg.speed) / 16;
+	}
+
+	// set baud rate prescaler
+	MODULE_CAN->BTR0.B.BRP = (uint8_t) round((((APB_CLK_FREQ * __tq) / 2) - 1) / 1000000) - 1;
+
+	/* Set sampling
+	 * 1 -> triple; the bus is sampled three times; recommended for low/medium speed buses     (class A and B) where
+	 * filtering spikes on the bus line is beneficial 0 -> single; the bus is sampled once; recommended for high speed
+	 * buses (SAE class C)*/
+	MODULE_CAN->BTR1.B.SAM = 0x1;
+
+	// enable all interrupts
+	MODULE_CAN->IER.U = 0xef; //ESP32 V3 0XEF     ESP32 NOT V3 0XFF
+
+	 // Set acceptance filter	
+	MODULE_CAN->MOD.B.AFM = __filter.FM;	
+    MODULE_CAN->MBX_CTRL.ACC.CODE[0] = __filter.ACR0;
+    MODULE_CAN->MBX_CTRL.ACC.CODE[1] = __filter.ACR1;
+    MODULE_CAN->MBX_CTRL.ACC.CODE[2] = __filter.ACR2;
+    MODULE_CAN->MBX_CTRL.ACC.CODE[3] = __filter.ACR3;
+    MODULE_CAN->MBX_CTRL.ACC.MASK[0] = __filter.AMR0;
+    MODULE_CAN->MBX_CTRL.ACC.MASK[1] = __filter.AMR1;
+    MODULE_CAN->MBX_CTRL.ACC.MASK[2] = __filter.AMR2;
+    MODULE_CAN->MBX_CTRL.ACC.MASK[3] = __filter.AMR3;
+
+	// set to normal mode
+	MODULE_CAN->OCR.B.OCMODE = __CAN_OC_NOM;
+
+	// clear error counters
+	MODULE_CAN->TXERR.U = 0;
+	MODULE_CAN->RXERR.U = 0;
+	(void) MODULE_CAN->ECC;
+
+	// clear interrupt flags
+	(void) MODULE_CAN->IR.U;
+
+	// install CAN ISR
+	esp_intr_alloc(ETS_CAN_INTR_SOURCE, 0, CAN_isr, NULL, NULL);
+
+	// allocate the tx complete semaphore
+	sem_tx_complete = xSemaphoreCreateBinary();
+
+	// Showtime. Release Reset Mode.
+	MODULE_CAN->MOD.B.RM = 0;
+
+	return 0;
+}
+
+int CAN_write_frame(const CAN_frame_t *p_frame) {
+	if (sem_tx_complete == NULL) {
+		return -1;
+	}
+
+	// Write the frame to the controller
+	CAN_write_frame_phy(p_frame);
+
+	// wait for the frame tx to complete
+	xSemaphoreTake(sem_tx_complete, portMAX_DELAY);
+
+	return 0;
+}
+
+int CAN_stop() {
+	// enter reset mode
+	MODULE_CAN->MOD.B.RM = 1;
+
+	return 0;
+}
+
+int CAN_config_filter(const CAN_filter_t* p_filter) {
+	
+	__filter.FM = p_filter->FM;	
+    __filter.ACR0 = p_filter->ACR0;
+    __filter.ACR1 = p_filter->ACR1;
+    __filter.ACR2 = p_filter->ACR2;
+    __filter.ACR3 = p_filter->ACR3;
+    __filter.AMR0 = p_filter->AMR0;
+    __filter.AMR1 = p_filter->AMR1;
+    __filter.AMR2 = p_filter->AMR2;
+    __filter.AMR3 = p_filter->AMR3;
+	
+	return 0;
 }
\ No newline at end of file
diff --git a/Software/config.h b/Software/config.h
index b9e9c612..0dbf9d6d 100644
--- a/Software/config.h
+++ b/Software/config.h
@@ -4,8 +4,8 @@
 // PIN
 #define PIN_5V_EN 16
 
-#define CAN_TX_PIN 26
-#define CAN_RX_PIN 27
+#define CAN_TX_PIN 27
+#define CAN_RX_PIN 26
 #define CAN_SE_PIN 23
 
 #define RS485_EN_PIN 17 // 17 /RE