diff --git a/Firmware/temperature.cpp b/Firmware/temperature.cpp
index 53199cd6..80356bd4 100755
--- a/Firmware/temperature.cpp
+++ b/Firmware/temperature.cpp
@@ -50,6 +50,14 @@
 #error "ADC_OVRSAMPL oversampling must match OVERSAMPLENR"
 #endif
 
+#ifdef SYSTEM_TIMER_2
+#define ENABLE_SOFT_PWM_INTERRUPT()  TIMSK2 |= (1<<OCIE2B)
+#define DISABLE_SOFT_PWM_INTERRUPT() TIMSK2 &= ~(1<<OCIE2B)
+#else //SYSTEM_TIMER_2
+#define ENABLE_SOFT_PWM_INTERRUPT()  TIMSK0 |= (1<<OCIE0B)
+#define DISABLE_SOFT_PWM_INTERRUPT() TIMSK0 &= ~(1<<OCIE0B)
+#endif //SYSTEM_TIMER_2
+
 // temperature manager timer configuration
 #define TEMP_MGR_INTV   0.27 // seconds, ~3.7Hz
 #define TIMER5_PRESCALE 256
diff --git a/Firmware/temperature.h b/Firmware/temperature.h
index 57116578..05251d85 100755
--- a/Firmware/temperature.h
+++ b/Firmware/temperature.h
@@ -24,20 +24,6 @@
 #include "Marlin.h"
 #include "config.h"
 
-
-#ifdef SYSTEM_TIMER_2
-
-#define ENABLE_SOFT_PWM_INTERRUPT()  TIMSK2 |= (1<<OCIE2B)
-#define DISABLE_SOFT_PWM_INTERRUPT() TIMSK2 &= ~(1<<OCIE2B)
-
-#else //SYSTEM_TIMER_2
-
-#define ENABLE_SOFT_PWM_INTERRUPT()  TIMSK0 |= (1<<OCIE0B)
-#define DISABLE_SOFT_PWM_INTERRUPT() TIMSK0 &= ~(1<<OCIE0B)
-
-#endif //SYSTEM_TIMER_2
-
-
 // public functions
 void soft_pwm_init(); //initialize the soft pwm isr
 void temp_mgr_init(); //initialize the temperature handler
diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp
index 45339793..b8acc948 100644
--- a/Firmware/xyzcal.cpp
+++ b/Firmware/xyzcal.cpp
@@ -138,15 +138,18 @@ pos_mm_t pos_2_mm(float pos){
 void xyzcal_meassure_enter(void)
 {
 	DBG(_n("xyzcal_meassure_enter\n"));
+
+	// disable heaters and stop motion before we initialize sm4
 	disable_heater();
-	DISABLE_SOFT_PWM_INTERRUPT();
-#if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
-	DISABLE_FANCHECK_INTERRUPT();
-#endif //(defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
+	st_synchronize();
+
+	// disable incompatible interrupts
 	DISABLE_STEPPER_DRIVER_INTERRUPT();
 #ifdef WATCHDOG
 	wdt_disable();
 #endif //WATCHDOG
+
+	// setup internal callbacks
 	sm4_stop_cb = 0;
 	sm4_update_pos_cb = xyzcal_update_pos;
 	sm4_calc_delay_cb = xyzcal_calc_delay;
@@ -156,20 +159,15 @@ void xyzcal_meassure_leave(void)
 {
 	DBG(_n("xyzcal_meassure_leave\n"));
     planner_abort_hard();
-	ENABLE_SOFT_PWM_INTERRUPT();
-#if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
-	ENABLE_FANCHECK_INTERRUPT();
-#endif //(defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
-	ENABLE_STEPPER_DRIVER_INTERRUPT();
+
+	// re-enable interrupts
 #ifdef WATCHDOG
 	wdt_enable(WDTO_4S);
 #ifdef EMERGENCY_HANDLERS
 	WDTCSR |= (1 << WDIE);
 #endif //EMERGENCY_HANDLERS
 #endif //WATCHDOG
-	sm4_stop_cb = 0;
-	sm4_update_pos_cb = 0;
-	sm4_calc_delay_cb = 0;
+	ENABLE_STEPPER_DRIVER_INTERRUPT();
 }
 
 
@@ -999,13 +997,9 @@ BedSkewOffsetDetectionResultType xyzcal_scan_and_process(){
 	return ret;
 }
 
-BedSkewOffsetDetectionResultType xyzcal_find_bed_induction_sensor_point_xy(void){
-	BedSkewOffsetDetectionResultType ret = BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
-
-    //@size=258
+BedSkewOffsetDetectionResultType xyzcal_find_bed_induction_sensor_point_xy(void) {
     // DBG(_n("xyzcal_find_bed_induction_sensor_point_xy x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
-	st_synchronize();
-
+	BedSkewOffsetDetectionResultType ret = BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
 	xyzcal_meassure_enter();
 	if (xyzcal_searchZ())
 		ret = xyzcal_scan_and_process();