diff --git a/src/main.cpp b/src/main.cpp
index a52c1a3..7d67042 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,5 +1,15 @@
 #include "main.h"
 
+const byte FAN_STAGE_TO_HEX[16] = {0x0, 0x10, 0x20, 0x30, 0x40, 0x50, 0x60, 0x70, 0x80, 0x90, 0xA0, 0xB0, 0xC0, 0xD0, 0xE0, 0xF0};
+
+STM32_CAN Can1(CAN1, DEF);
+
+unsigned long previousCANMillis = 0;
+CAN_message_t outCanMsg;
+
+int previousACStatus = LOW;
+unsigned long acStatusChangedMillis = 0;
+
 void setup() {
     initialize();
 }
@@ -17,12 +27,15 @@ void initialize() {
 
     pinMode(AC_INPUT, INPUT_PULLDOWN);
 
-    pinMode(AC_PRESSURE_TYPE, INPUT_PULLUP);
+    pinMode(AC_PRESSURE_SENSOR_TYPE, INPUT_PULLUP);
     pinMode(AC_PRESSURE_SWITCH_HIGH, INPUT_PULLUP);
     pinMode(AC_PRESSURE_SENSOR, INPUT_ANALOG);
 
     pinMode(EXTERNAL_TEMPERATURE_SENSOR, INPUT_ANALOG);
 
+    previousACStatus = digitalRead(AC_INPUT);
+    acStatusChangedMillis = millis() - (30 * 1000);
+
     Can1.begin();
     Can1.setBaudRate(500000);
 }
@@ -34,7 +47,7 @@ void processCan(long currentMillis) {
 
     Serial.println("Processing CAN Messages...");
     processCanICL2();
-    processCanICL3();
+    processCanICL3(currentMillis);
     previousCANMillis = currentMillis;
 }
 
@@ -52,9 +65,12 @@ void processCanICL2() {
     Can1.write(outCanMsg);
 }
 
-void processCanICL3() {
+void processCanICL3(long currentMillis) {
     int acStatus = digitalRead(AC_INPUT);
     Serial.printf("AC status is: %s\n", acStatus == 1 ? "on" : "off");
+    if (acStatus != previousACStatus) {
+        acStatusChangedMillis = currentMillis;
+    }
 
     outCanMsg.id = CAN_ICL3;
     outCanMsg.len = 8;
@@ -70,7 +86,7 @@ void processCanICL3() {
             break;
     }
 
-    outCanMsg.buf[1] = calculateFanStage(acStatus);
+    outCanMsg.buf[1] = calculateFanStage(acStatus, currentMillis);
     outCanMsg.buf[2] = 0x00;
     outCanMsg.buf[3] = readTemperatureSensor();
     outCanMsg.buf[5] = 0x00;
@@ -81,8 +97,20 @@ void processCanICL3() {
     previousACStatus = acStatus;
 }
 
-byte calculateFanStage(int acStatus) {
-    if (digitalRead(AC_PRESSURE_TYPE) == HIGH) {
+byte calculateFanStage(int acStatus, long currentMillis) {
+    // eFan should change status after 30 secs of AC Status have beeing changed
+    int diff = (currentMillis - acStatusChangedMillis) * 0.001;
+    if (diff < 30) {
+        if (acStatus == HIGH) {
+            return FAN_STAGE_TO_HEX[0];
+        }
+
+        // if ac status was changed to off keep fun running for other 30secs
+        // Set the ac status to high will guarantee the fan runs on right stage
+        acStatus = HIGH;
+    }
+
+    if (digitalRead(AC_PRESSURE_SENSOR_TYPE) == HIGH) {
         return calculateFanStageWithPressureSwitch(acStatus);
     }
 
@@ -123,22 +151,15 @@ byte calculateFanStateWithPressureSensor(int acStatus) {
 }
 
 byte calculateFanStageWithPressureSwitch(int acStatus) {
+    // Pressure switch works with ground to provide a signal for second stage relay
     bool pressureIsHigh = digitalRead(AC_PRESSURE_SWITCH_HIGH) == LOW;
-    int stage = 0;
-    if (acStatus == LOW) {
-        if (pressureIsHigh) {
-            stage = FAN_STAGE_AT_HIGH_PRESSURE;
-        } else {
-            stage = 0;
-        }
 
-    } else {
-        if (pressureIsHigh) {
-            stage = FAN_STAGE_AT_HIGH_PRESSURE;
-        } else {
-            // If AC is ON eFan should run at the FAN_STAGE_LOW_PRESSURE
-            stage = FAN_STAGE_AT_LOW_PRESSURE;
-        }
+    int stage = 0;
+    if (pressureIsHigh) {
+        stage = FAN_STAGE_AT_HIGH_PRESSURE;
+    } else if (acStatus == HIGH) {
+        // If AC is ON eFan should run at the FAN_STAGE_LOW_PRESSURE
+        stage = FAN_STAGE_AT_LOW_PRESSURE;
     }
 
     Serial.printf("Fan stage %d\n", stage);
diff --git a/src/main.h b/src/main.h
index 2712a81..245919f 100644
--- a/src/main.h
+++ b/src/main.h
@@ -6,7 +6,7 @@
 
 #define AC_INPUT PA5
 
-#define AC_PRESSURE_TYPE PA4
+#define AC_PRESSURE_SENSOR_TYPE PA4
 #define AC_PRESSURE_SWITCH_HIGH PA6
 #define AC_PRESSURE_SENSOR PB0
 
@@ -15,22 +15,13 @@
 #define CAN_ICL2 0x613
 #define CAN_ICL3 0x615
 
-#define CAN_INTERVAL 200
+#define CAN_INTERVAL 200  // milliseconds
 
 #define MIN_FAN_STAGE 1
 #define MAX_FAN_STAGE 15
 
-#define FAN_STAGE_AT_LOW_PRESSURE 4
-#define FAN_STAGE_AT_HIGH_PRESSURE 12
-
-const byte FAN_STAGE_TO_HEX[15] = {0x00, 0x10, 0x20, 0x30, 0x40, 0x50, 0x60, 0x80, 0x90, 0xA0, 0xB0, 0xC0, 0xD0, 0xE0, 0xF0};
-
-STM32_CAN Can1(CAN1, DEF);
-
-unsigned long previousCANMillis = 0;
-CAN_message_t outCanMsg;
-
-int previousACStatus = LOW;
+#define FAN_STAGE_AT_LOW_PRESSURE 3
+#define FAN_STAGE_AT_HIGH_PRESSURE 10
 
 void initialize();
 void processCan(long currentMillis);
@@ -40,14 +31,14 @@ void processCan(long currentMillis);
 // https://www.ms4x.net/index.php?title=CAN_Bus_ID_0x613_ICL2
 void processCanICL2();
 // https://www.ms4x.net/index.php?title=CAN_Bus_ID_0x615_ICL3
-void processCanICL3();
+void processCanICL3(long currentMillis);
 
 /*
  * On E36 the pressure switch is a ON/OFF. On E46 a pressure sensor is used.
  * Is it possible to replace a pressure switch with a pressure sensor to achieve a more linear
  * activation of the fan stage.
  */
-byte calculateFanStage(int acStatus);
+byte calculateFanStage(int acStatus, long currentMillis);
 byte calculateFanStateWithPressureSensor(int acStatus);
 byte calculateFanStageWithPressureSwitch(int acStatus);
 byte readTemperatureSensor();