diff --git a/Firmware/temperature.cpp b/Firmware/temperature.cpp
index 8dcfc9a9c..e156734b8 100644
--- a/Firmware/temperature.cpp
+++ b/Firmware/temperature.cpp
@@ -2070,10 +2070,17 @@ else {                                            // ambient temperature is stan
  
 #if (defined(FANCHECK) && defined(TACH_0) && (TACH_0 > -1))
 void check_fans() {
+#ifdef FAN_SOFT_PWM
+	if (READ(TACH_0) != fan_state[0]) {
+		if(fan_measuring) fan_edge_counter[0] ++;
+		fan_state[0] = !fan_state[0];
+	}
+#else //FAN_SOFT_PWM
 	if (READ(TACH_0) != fan_state[0]) {
 		fan_edge_counter[0] ++;
 		fan_state[0] = !fan_state[0];
 	}
+#endif
 	//if (READ(TACH_1) != fan_state[1]) {
 	//	fan_edge_counter[1] ++;
 	//	fan_state[1] = !fan_state[1];
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 8c2b52813..9614c9a4d 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -7352,15 +7352,13 @@ static bool lcd_selftest_fan_dialog(int _fan)
 		setExtruderAutoFanState(EXTRUDER_0_AUTO_FAN_PIN, 1); //extruder fan
 #ifdef FAN_SOFT_PWM
 		extruder_autofan_last_check = _millis();
+		fan_measuring = true;
 #endif //FAN_SOFT_PWM
 		_delay(2000);				//delay_keep_alive would turn off extruder fan, because temerature is too low
-#ifdef FAN_SOFT_PWM
-		countFanSpeed();
-		if (!fan_speed[0]) _result = false;
-#else //FAN_SOFT_PWM
+
 		manage_heater();			//count average fan speed from 2s delay and turn off fans
 		if (!fan_speed[0]) _result = false;
-#endif //FAN_SOFT_PWM
+
 		
 		printf_P(PSTR("Test 1:\n"));
 		printf_P(PSTR("Print fan speed: %d \n"), fan_speed[1]);