diff --git a/ivec_RTE/src/ivec_rte.c b/ivec_RTE/src/ivec_rte.c
index ea5ff19..a04e08f 100644
--- a/ivec_RTE/src/ivec_rte.c
+++ b/ivec_RTE/src/ivec_rte.c
@@ -237,15 +237,15 @@ void vCanFilterSaveVal(uint8_t ucIdx, uint32_t Filter, bool isExtended)
     filterCount = ucIdx;
     // Store filter value
     filterValues[filterCount] = Filter;
-    isExtendedID[maskCount++] = isExtended;
-    if(isExtended)
-    {
-        extendedFilter++;
-    }
-    else
-    {
-        standardFilter++;
-    }
+//    isExtendedID[filterCount] = isExtended;
+//    if(isExtended)
+//    {
+//        extendedFilter++;
+//    }
+//    else
+//    {
+//        standardFilter++;
+//    }
 
 }
 
@@ -254,8 +254,8 @@ void vCanFilterSaveVal(uint8_t ucIdx, uint32_t Filter, bool isExtended)
 void vCanFilterReset() {
     uint32_t i;
 
-    // Disable CAN before reconfiguring filters
-    xECU_CANDeInit(CANFD0);
+
+    DL_MCAN_setOpMode(CANFD0, DL_MCAN_OPERATION_MODE_SW_INIT);
 
     // Reset all standard ID filters
     for (i = 0; i < MAX_FILTERS; i++) {
@@ -273,16 +273,35 @@ void vCanFilterReset() {
         DL_MCAN_addExtMsgIDFilter(CANFD0, i, &extFilterElement);
     }
 
-    // Re-enable CAN after resetting filters
-    xECU_CANInit(CANFD0,g_u16CanSpeed);
+    /* Set Extended ID Mask. */
+    DL_MCAN_setExtIDAndMask(CANFD0, (0x1FFFFFFFU));
+    /* Take MCAN out of the SW initialization mode */
+    DL_MCAN_setOpMode(CANFD0, DL_MCAN_OPERATION_MODE_NORMAL);
+
+
 }
 
 // Function to configure CAN filters
 void vCanConfigFilter() {
 
     xECU_CanReInit(CANFD0,g_u16CanSpeed);
+
     DL_MCAN_setOpMode(CANFD0, DL_MCAN_OPERATION_MODE_SW_INIT);
 
+    for (int i = 0; i <= maskCount; i++)
+    {
+        if((filterValues[i]  > 0x7FF) || (maskValues[i]  > 0x7FF))
+        {
+            isExtendedID[i] = 1;
+            extendedFilter++;
+        }
+        else
+        {
+            isExtendedID[i] = 0;
+            standardFilter++;
+        }
+    }
+
     gMCAN0MsgRAMConfigParamsFiltered.lse = extendedFilter; //0;  // Set dynamically based on actual filters
     gMCAN0MsgRAMConfigParamsFiltered.lss = standardFilter; //1;  // Set dynamically based on actual filters
     DL_MCAN_config(CANFD0, (DL_MCAN_ConfigParams*) &gMCAN0ConfigParamsFiltered);
@@ -290,9 +309,8 @@ void vCanConfigFilter() {
 
    uint8_t extendedFilterNumber = 0;
    uint8_t stadardFilterNumber = 0;
-    for (int i = 0; i < maskCount; i++) {
-//        filterValues[0] = 0x3ff;
-//        maskValues[0] = 0x400;
+
+    for (int i = 0; i <= maskCount; i++) {
         if (isExtendedID[i]) {
             // Extended ID filter
             DL_MCAN_ExtMsgIDFilterElement extFilterElement;
@@ -300,7 +318,7 @@ void vCanConfigFilter() {
             extFilterElement.efid2 = maskValues[i];
             extFilterElement.efec = 001;
             extFilterElement.eft = 10;
-            DL_MCAN_addExtMsgIDFilter(CANFD0, i, (DL_MCAN_StdMsgIDFilterElement *) &extFilterElement);
+            DL_MCAN_addExtMsgIDFilter(CANFD0, extendedFilterNumber, (DL_MCAN_StdMsgIDFilterElement *) &extFilterElement);
             extendedFilterNumber++;
 
         }
@@ -309,8 +327,6 @@ void vCanConfigFilter() {
             DL_MCAN_StdMsgIDFilterElement stdFilterElement;
             stdFilterElement.sfid1 = filterValues[i];
             stdFilterElement.sfid2 = maskValues[i];
-//            stdFilterElement.sfid1 = 0xc8;
-//            stdFilterElement.sfid2 = 0x7fe;
             stdFilterElement.sfec = 001;
             stdFilterElement.sft = 10;
             DL_MCAN_addStdMsgIDFilter(CANFD0, stadardFilterNumber,(DL_MCAN_StdMsgIDFilterElement *) &stdFilterElement);
@@ -326,7 +342,7 @@ void vCanConfigFilter() {
 
 
     // Reset counters after applying filters
-    maskCount = 0;
+    maskCount = 1;
     filterCount = 0;
     extendedFilterNumber = 0;
     stadardFilterNumber = 0;
@@ -371,6 +387,7 @@ void vRTE_UARTDataProcess(void)
                     vCanFilterSaveVal((pucBuf[PKT_HEADER+1] - 1), filterId, isExtended);
                     if( pucBuf[PKT_HEADER+2] )//All filter received. Trigger Filter Settings
                     {
+                        vCanFilterReset();
                         vCanConfigFilter();
                     }
                     else