diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index bf3ac1806..f048360f8 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -1245,7 +1245,8 @@ void setup()
   else if (eeprom_read_byte((uint8_t*)EEPROM_WIZARD_ACTIVE) == 0) { //dont show calibration status messages if wizard is currently active
 
 	  if (calibration_status() == CALIBRATION_STATUS_ASSEMBLED ||
-		  calibration_status() == CALIBRATION_STATUS_UNKNOWN) {
+		  calibration_status() == CALIBRATION_STATUS_UNKNOWN ||
+	      calibration_status() == CALIBRATION_STATUS_XYZ_CALIBRATION){
 		  // Reset the babystepping values, so the printer will not move the Z axis up when the babystepping is enabled.
 		  eeprom_update_word((uint16_t*)EEPROM_BABYSTEP_Z, 0);
 		  // Show the message.
@@ -3639,14 +3640,16 @@ void process_commands()
 			calibration_status_store(CALIBRATION_STATUS_CALIBRATED);
             break;
 
-		case 88: //just for test
+		/*case 88: //just for test
 			SERIAL_ECHOPGM("Calibration status:");
 			MYSERIAL.println(int(calibration_status()));
 			if (code_seen('S')) codenum = code_value();
 			calibration_status_store(codenum);
 			SERIAL_ECHOPGM("Calibration status:");
 			MYSERIAL.println(int(calibration_status()));
+			eeprom_write_byte((uint8_t*)EEPROM_WIZARD_ACTIVE, 1); //run wizard
 			break;
+		*/
 #endif  // ENABLE_MESH_BED_LEVELING
             
             
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 5ca340c16..075d39b95 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -2810,30 +2810,25 @@ void lcd_wizard() {
 
 void lcd_wizard(int state) {
 
-	/*
-	if (eeprom_read_byte((uint8_t*)EEPROM_TEMP_CAL_ACTIVE) == 255) {
-		eeprom_write_byte((uint8_t*)EEPROM_TEMP_CAL_ACTIVE, 0);
-	*/
-	eeprom_write_byte((uint8_t*)EEPROM_WIZARD_ACTIVE, 1);
 
-#define WIZARD_END 255
 
-	//int state = 0;
 	bool end = false;
 	int wizard_event;
 	const char *msg = NULL;
-
 	while (!end) {
 		switch (state) { 
 		case 0: // run wizard?
 			wizard_event = lcd_show_multiscreen_message_yes_no_and_wait_P(MSG_WIZARD_WELCOME, false, true);
 			if (wizard_event) {
 				state = 1;
-				//eeprom_write_byte((uint8_t*)EEPROM_WIZARD_ACTIVE, 1);
-			}else end = true;
+				eeprom_write_byte((uint8_t*)EEPROM_WIZARD_ACTIVE, 1);
+			}
+			else {
+				eeprom_write_byte((uint8_t*)EEPROM_WIZARD_ACTIVE, 0);
+				end = true;
+			}
 			break;
 		case 1: // restore calibration status
-
 			switch (calibration_status()) {
 			case CALIBRATION_STATUS_ASSEMBLED: state = 2; break; //run selftest
 			case CALIBRATION_STATUS_XYZ_CALIBRATION: state = 3; break; //run xyz cal.
@@ -2842,10 +2837,14 @@ void lcd_wizard(int state) {
 			case CALIBRATION_STATUS_CALIBRATED: end = true; break;
 			default: state = 2; break; //if calibration status is unknown, run wizard from the beginning
 			}
+			break;
 		case 2: //selftest
 			lcd_show_fullscreen_message_and_wait_P(MSG_WIZARD_SELFTEST);
 			wizard_event = lcd_selftest();
-			if (wizard_event) state = 3;
+			if (wizard_event) {
+				calibration_status_store(CALIBRATION_STATUS_XYZ_CALIBRATION);
+				state = 3;
+			}
 			else end = true;
 			break;
 		case 3: //xyz cal.
@@ -2905,28 +2904,6 @@ void lcd_wizard(int state) {
 			end = true;
 			break;
 
-		/*
-			// Freshly assembled, needs to peform a self-test and the XYZ calibration.
-	CALIBRATION_STATUS_ASSEMBLED = 255,
-
-	// For the wizard: self test has been performed, now the XYZ calibration is needed.
-	CALIBRATION_STATUS_XYZ_CALIBRATION = 250,
-
-	// For the wizard: factory assembled, needs to run Z calibration.
-	CALIBRATION_STATUS_Z_CALIBRATION = 240,
-
-	// The XYZ calibration has been performed, now it remains to run the V2Calibration.gcode.
-	CALIBRATION_STATUS_LIVE_ADJUST = 230,
-
-    // Calibrated, ready to print.
-    CALIBRATION_STATUS_CALIBRATED = 1,
-
-    // Legacy: resetted by issuing a G86 G-code.
-    // This value can only be expected after an upgrade from the initial MK2 firmware releases.
-    // Currently the G86 sets the calibration status to 
-    CALIBRATION_STATUS_UNKNOWN = 0,
-		*/
-
 		default: break;
 		}
 	}
@@ -2957,7 +2934,7 @@ void lcd_wizard(int state) {
 		break;
 
 	default: 
-		lcd_show_fullscreen_message_and_wait_P(MSG_WIZARD_QUIT);
+		msg = MSG_WIZARD_QUIT;
 		break;
 	
 	}