Merge pull request #788 from PavelSindler/temp_cal_coordinates_fix

Temp cal fixes and improvements

Firmware/Marlin.h

@@ -437,6 +437,7 @@ void force_high_power_mode(bool start_high_power_section);
 #endif //TMC2130
 
 // G-codes
+void gcode_G28(bool home_x, bool home_y, bool home_z, bool calib);
 bool gcode_M45(bool onlyZ, int8_t verbosity_level);
 void gcode_M114();
 void gcode_M701();

Firmware/Marlin_main.cpp

@@ -2134,6 +2134,9 @@ void homeaxis(int axis, uint8_t cnt, uint8_t* pstep)
     }
     else if ((axis==Z_AXIS)?HOMEAXIS_DO(Z):0)
 	{
+#ifdef TMC2130
+		FORCE_HIGH_POWER_START;
+#endif	
         int axis_home_dir = home_dir(axis);
         current_position[axis] = 0;
         plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
@@ -2142,7 +2145,11 @@ void homeaxis(int axis, uint8_t cnt, uint8_t* pstep)
         plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
         st_synchronize();
 #ifdef TMC2130
-		if ((tmc2130_mode == TMC2130_MODE_NORMAL) && (READ(Z_TMC2130_DIAG) != 0)) return; //Z crash
+		if (READ(Z_TMC2130_DIAG) != 0) { //Z crash
+			FORCE_HIGH_POWER_END;
+			kill(MSG_BED_LEVELING_FAILED_POINT_LOW);
+			return; 
+		}
 #endif //TMC2130
         current_position[axis] = 0;
         plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
@@ -2154,13 +2161,20 @@ void homeaxis(int axis, uint8_t cnt, uint8_t* pstep)
         plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
         st_synchronize();
 #ifdef TMC2130
-		if ((tmc2130_mode == TMC2130_MODE_NORMAL) && (READ(Z_TMC2130_DIAG) != 0)) return; //Z crash
+		if (READ(Z_TMC2130_DIAG) != 0) { //Z crash
+			FORCE_HIGH_POWER_END;
+			kill(MSG_BED_LEVELING_FAILED_POINT_LOW);
+			return; 
+		}
 #endif //TMC2130
         axis_is_at_home(axis);
         destination[axis] = current_position[axis];
         feedrate = 0.0;
         endstops_hit_on_purpose();
         axis_known_position[axis] = true;
+#ifdef TMC2130
+		FORCE_HIGH_POWER_END;
+#endif	
     }
     enable_endstops(endstops_enabled);
 }
@@ -2326,990 +2340,997 @@ void force_high_power_mode(bool start_high_power_section) {
 }
 #endif //TMC2130
 
-bool gcode_M45(bool onlyZ, int8_t verbosity_level)
-{
-	bool final_result = false;
-	#ifdef TMC2130
-	FORCE_HIGH_POWER_START;
-	#endif // TMC2130
-	// Only Z calibration?
-	if (!onlyZ)
-	{
-		setTargetBed(0);
-		setTargetHotend(0, 0);
-		setTargetHotend(0, 1);
-		setTargetHotend(0, 2);
-		adjust_bed_reset(); //reset bed level correction
-	}
+void gcode_G28(bool home_x, bool home_y, bool home_z, bool calib){
+      st_synchronize();
 
-	// Disable the default update procedure of the display. We will do a modal dialog.
-	lcd_update_enable(false);
-	// Let the planner use the uncorrected coordinates.
-	mbl.reset();
-	// Reset world2machine_rotation_and_skew and world2machine_shift, therefore
-	// the planner will not perform any adjustments in the XY plane. 
-	// Wait for the motors to stop and update the current position with the absolute values.
-	world2machine_revert_to_uncorrected();
-	// Reset the baby step value applied without moving the axes.
-	babystep_reset();
-	// Mark all axes as in a need for homing.
-	memset(axis_known_position, 0, sizeof(axis_known_position));
+#if 0
+      SERIAL_ECHOPGM("G28, initial ");  print_world_coordinates();
+      SERIAL_ECHOPGM("G28, initial ");  print_physical_coordinates();
+#endif
 
-	// Home in the XY plane.
-	//set_destination_to_current();
-	setup_for_endstop_move();
-	lcd_display_message_fullscreen_P(_T(MSG_AUTO_HOME));
-	home_xy();
+      // Flag for the display update routine and to disable the print cancelation during homing.
+	  homing_flag = true;
+     
+      // Either all X,Y,Z codes are present, or none of them.
+      bool home_all_axes = home_x == home_y && home_x == home_z;
+      if (home_all_axes)
+        // No X/Y/Z code provided means to home all axes.
+        home_x = home_y = home_z = true;
 
-	enable_endstops(false);
-	current_position[X_AXIS] += 5;
-	current_position[Y_AXIS] += 5;
-	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS] / 40, active_extruder);
-	st_synchronize();
+#ifdef ENABLE_AUTO_BED_LEVELING
+      plan_bed_level_matrix.set_to_identity();  //Reset the plane ("erase" all leveling data)
+#endif //ENABLE_AUTO_BED_LEVELING
+            
+      // Reset world2machine_rotation_and_skew and world2machine_shift, therefore
+      // the planner will not perform any adjustments in the XY plane. 
+      // Wait for the motors to stop and update the current position with the absolute values.
+      world2machine_revert_to_uncorrected();
 
-	// Let the user move the Z axes up to the end stoppers.
-#ifdef TMC2130
-	if (calibrate_z_auto())
-	{
-#else //TMC2130
-	if (lcd_calibrate_z_end_stop_manual(onlyZ))
-	{
-#endif //TMC2130
-		refresh_cmd_timeout();
-		#ifndef STEEL_SHEET
-		if (((degHotend(0) > MAX_HOTEND_TEMP_CALIBRATION) || (degBed() > MAX_BED_TEMP_CALIBRATION)) && (!onlyZ))
-		{
-			lcd_wait_for_cool_down();
-		}
-		#endif //STEEL_SHEET
-		if(!onlyZ)
-		{
-			KEEPALIVE_STATE(PAUSED_FOR_USER);
-			#ifdef STEEL_SHEET
-			bool result = lcd_show_fullscreen_message_yes_no_and_wait_P(_T(MSG_STEEL_SHEET_CHECK), false, false);
-			if(result) lcd_show_fullscreen_message_and_wait_P(_T(MSG_REMOVE_STEEL_SHEET));
-			#endif //STEEL_SHEET
-			lcd_show_fullscreen_message_and_wait_P(_T(MSG_CONFIRM_NOZZLE_CLEAN));
-		    lcd_show_fullscreen_message_and_wait_P(_T(MSG_PAPER));
-			KEEPALIVE_STATE(IN_HANDLER);
-			lcd_display_message_fullscreen_P(_T(MSG_FIND_BED_OFFSET_AND_SKEW_LINE1));
-			lcd_implementation_print_at(0, 2, 1);
-			lcd_printPGM(_T(MSG_FIND_BED_OFFSET_AND_SKEW_LINE2));
-		}
-		// Move the print head close to the bed.
-		current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+      // For mesh bed leveling deactivate the matrix temporarily.
+      // It is necessary to disable the bed leveling for the X and Y homing moves, so that the move is performed
+      // in a single axis only.
+      // In case of re-homing the X or Y axes only, the mesh bed leveling is restored after G28.
+#ifdef MESH_BED_LEVELING
+      uint8_t mbl_was_active = mbl.active;
+      mbl.active = 0;
+      current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
+#endif
 
-		bool endstops_enabled  = enable_endstops(true);
-#ifdef TMC2130
-		tmc2130_home_enter(Z_AXIS_MASK);
-#endif //TMC2130
+      // Reset baby stepping to zero, if the babystepping has already been loaded before. The babystepsTodo value will be
+      // consumed during the first movements following this statement.
+      if (home_z)
+        babystep_undo();
 
-		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS] / 40, active_extruder);
+      saved_feedrate = feedrate;
+      saved_feedmultiply = feedmultiply;
+      feedmultiply = 100;
+      previous_millis_cmd = millis();
 
-		st_synchronize();
-#ifdef TMC2130
-		tmc2130_home_exit();
-#endif //TMC2130
-		enable_endstops(endstops_enabled);
+      enable_endstops(true);
 
-		if (st_get_position_mm(Z_AXIS) == MESH_HOME_Z_SEARCH)
-		{
+      memcpy(destination, current_position, sizeof(destination));
+      feedrate = 0.0;
 
-			int8_t verbosity_level = 0;
-			if (code_seen('V'))
-			{
-				// Just 'V' without a number counts as V1.
-				char c = strchr_pointer[1];
-				verbosity_level = (c == ' ' || c == '\t' || c == 0) ? 1 : code_value_short();
-			}
+      #if Z_HOME_DIR > 0                      // If homing away from BED do Z first
+      if(home_z)
+        homeaxis(Z_AXIS);
+      #endif
 
-			if (onlyZ)
-			{
-				clean_up_after_endstop_move();
-				// Z only calibration.
-				// Load the machine correction matrix
-				world2machine_initialize();
-				// and correct the current_position to match the transformed coordinate system.
-				world2machine_update_current();
-				//FIXME
-				bool result = sample_mesh_and_store_reference();
-				if (result)
-				{
-					if (calibration_status() == CALIBRATION_STATUS_Z_CALIBRATION)
-						// Shipped, the nozzle height has been set already. The user can start printing now.
-						calibration_status_store(CALIBRATION_STATUS_CALIBRATED);
-						final_result = true;
-					// babystep_apply();
-				}
-			}
-			else
-			{
-				// Reset the baby step value and the baby step applied flag.
-				calibration_status_store(CALIBRATION_STATUS_XYZ_CALIBRATION);
-				eeprom_update_word((uint16_t*)EEPROM_BABYSTEP_Z, 0);
-				// Complete XYZ calibration.
-				uint8_t point_too_far_mask = 0;
-				BedSkewOffsetDetectionResultType result = find_bed_offset_and_skew(verbosity_level, point_too_far_mask);
-				clean_up_after_endstop_move();
-				// Print head up.
-				current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS] / 40, active_extruder);
-				st_synchronize();
-//#ifndef NEW_XYZCAL
-				if (result >= 0)
-				{
-					#ifdef HEATBED_V2
-					sample_z();
-					#else //HEATBED_V2
-					point_too_far_mask = 0;
-					// Second half: The fine adjustment.
-					// Let the planner use the uncorrected coordinates.
-					mbl.reset();
-					world2machine_reset();
-					// Home in the XY plane.
-					setup_for_endstop_move();
-					home_xy();
-					result = improve_bed_offset_and_skew(1, verbosity_level, point_too_far_mask);
-					clean_up_after_endstop_move();
-					// Print head up.
-					current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
-					plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS] / 40, active_extruder);
-					st_synchronize();
-					// if (result >= 0) babystep_apply();					
-					#endif //HEATBED_V2
-				}
-//#endif //NEW_XYZCAL
-				lcd_update_enable(true);
-				lcd_update(2);
+      #ifdef QUICK_HOME
+      // In the quick mode, if both x and y are to be homed, a diagonal move will be performed initially.
+      if(home_x && home_y)  //first diagonal move
+      {
+        current_position[X_AXIS] = 0;current_position[Y_AXIS] = 0;
 
-				lcd_bed_calibration_show_result(result, point_too_far_mask);
-				if (result >= 0)
-				{
-					// Calibration valid, the machine should be able to print. Advise the user to run the V2Calibration.gcode.
-					calibration_status_store(CALIBRATION_STATUS_LIVE_ADJUST);
-					if (eeprom_read_byte((uint8_t*)EEPROM_WIZARD_ACTIVE) != 1) lcd_show_fullscreen_message_and_wait_P(_T(MSG_BABYSTEP_Z_NOT_SET));
-					final_result = true;
-				}
-			}
-#ifdef TMC2130
-			tmc2130_home_exit();
-#endif
-		}
-		else
-		{
-			lcd_show_fullscreen_message_and_wait_P(PSTR("Calibration failed! Check the axes and run again."));
-			final_result = false;
-		}
-	}
-	else
-	{
-		// Timeouted.
-	}
-	lcd_update_enable(true);
-#ifdef TMC2130
-	FORCE_HIGH_POWER_END;
-#endif // TMC2130
-	return final_result;
-}
+        int x_axis_home_dir = home_dir(X_AXIS);
 
-void gcode_M114()
-{
-	SERIAL_PROTOCOLPGM("X:");
-	SERIAL_PROTOCOL(current_position[X_AXIS]);
-	SERIAL_PROTOCOLPGM(" Y:");
-	SERIAL_PROTOCOL(current_position[Y_AXIS]);
-	SERIAL_PROTOCOLPGM(" Z:");
-	SERIAL_PROTOCOL(current_position[Z_AXIS]);
-	SERIAL_PROTOCOLPGM(" E:");
-	SERIAL_PROTOCOL(current_position[E_AXIS]);
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+        destination[X_AXIS] = 1.5 * max_length(X_AXIS) * x_axis_home_dir;destination[Y_AXIS] = 1.5 * max_length(Y_AXIS) * home_dir(Y_AXIS);
+        feedrate = homing_feedrate[X_AXIS];
+        if(homing_feedrate[Y_AXIS]<feedrate)
+          feedrate = homing_feedrate[Y_AXIS];
+        if (max_length(X_AXIS) > max_length(Y_AXIS)) {
+          feedrate *= sqrt(pow(max_length(Y_AXIS) / max_length(X_AXIS), 2) + 1);
+        } else {
+          feedrate *= sqrt(pow(max_length(X_AXIS) / max_length(Y_AXIS), 2) + 1);
+        }
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        st_synchronize();
 
-	SERIAL_PROTOCOLRPGM(_n(" Count X: "));////MSG_COUNT_X c=0 r=0
-	SERIAL_PROTOCOL(float(st_get_position(X_AXIS)) / axis_steps_per_unit[X_AXIS]);
-	SERIAL_PROTOCOLPGM(" Y:");
-	SERIAL_PROTOCOL(float(st_get_position(Y_AXIS)) / axis_steps_per_unit[Y_AXIS]);
-	SERIAL_PROTOCOLPGM(" Z:");
-	SERIAL_PROTOCOL(float(st_get_position(Z_AXIS)) / axis_steps_per_unit[Z_AXIS]);
-	SERIAL_PROTOCOLPGM(" E:");
-	SERIAL_PROTOCOL(float(st_get_position(E_AXIS)) / axis_steps_per_unit[E_AXIS]);
+        axis_is_at_home(X_AXIS);
+        axis_is_at_home(Y_AXIS);
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+        destination[X_AXIS] = current_position[X_AXIS];
+        destination[Y_AXIS] = current_position[Y_AXIS];
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        feedrate = 0.0;
+        st_synchronize();
+        endstops_hit_on_purpose();
 
-	SERIAL_PROTOCOLLN("");
-}
+        current_position[X_AXIS] = destination[X_AXIS];
+        current_position[Y_AXIS] = destination[Y_AXIS];
+        current_position[Z_AXIS] = destination[Z_AXIS];
+      }
+      #endif /* QUICK_HOME */
 
-void gcode_M701()
-{
-#ifdef SNMM
-	extr_adj(snmm_extruder);//loads current extruder
-#else
-	enable_z();
-	custom_message = true;
-	custom_message_type = 2;
+#ifdef TMC2130	 
+      if(home_x)
+	  {
+		if (!calib)
+			homeaxis(X_AXIS);
+		else
+			tmc2130_home_calibrate(X_AXIS);
+	  }
 
-	lcd_setstatuspgm(_T(MSG_LOADING_FILAMENT));
-	current_position[E_AXIS] += 70;
-	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 400 / 60, active_extruder); //fast sequence
+      if(home_y)
+	  {
+		if (!calib)
+	        homeaxis(Y_AXIS);
+		else
+			tmc2130_home_calibrate(Y_AXIS);
+	  }
+#endif //TMC2130
 
-	current_position[E_AXIS] += 25;
-	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 100 / 60, active_extruder); //slow sequence
-	st_synchronize();
 
-	tone(BEEPER, 500);
-	delay_keep_alive(50);
-	noTone(BEEPER);
+      if(code_seen(axis_codes[X_AXIS]) && code_value_long() != 0)
+        current_position[X_AXIS]=code_value()+add_homing[X_AXIS];
 
-	if (!farm_mode && loading_flag) {
-		bool clean = lcd_show_fullscreen_message_yes_no_and_wait_P(_T(MSG_FILAMENT_CLEAN), false, true);
+      if(code_seen(axis_codes[Y_AXIS]) && code_value_long() != 0)
+		    current_position[Y_AXIS]=code_value()+add_homing[Y_AXIS];
 
-		while (!clean) {
-			lcd_update_enable(true);
-			lcd_update(2);
-			current_position[E_AXIS] += 25;
-			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 100 / 60, active_extruder); //slow sequence
-			st_synchronize();
-			clean = lcd_show_fullscreen_message_yes_no_and_wait_P(_T(MSG_FILAMENT_CLEAN), false, true);
+      #if Z_HOME_DIR < 0                      // If homing towards BED do Z last
+        #ifndef Z_SAFE_HOMING
+          if(home_z) {
+            #if defined (Z_RAISE_BEFORE_HOMING) && (Z_RAISE_BEFORE_HOMING > 0)
+              destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1);    // Set destination away from bed
+              feedrate = max_feedrate[Z_AXIS];
+              plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
+              st_synchronize();
+            #endif // defined (Z_RAISE_BEFORE_HOMING) && (Z_RAISE_BEFORE_HOMING > 0)
+            #if (defined(MESH_BED_LEVELING) && !defined(MK1BP))  // If Mesh bed leveling, move X&Y to safe position for home
+      			  if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] )) 
+      			  {
+                homeaxis(X_AXIS);
+                homeaxis(Y_AXIS);
+      			  } 
+              // 1st mesh bed leveling measurement point, corrected.
+              world2machine_initialize();
+              world2machine(pgm_read_float(bed_ref_points_4), pgm_read_float(bed_ref_points_4+1), destination[X_AXIS], destination[Y_AXIS]);
+              world2machine_reset();
+              if (destination[Y_AXIS] < Y_MIN_POS)
+                  destination[Y_AXIS] = Y_MIN_POS;
+              destination[Z_AXIS] = MESH_HOME_Z_SEARCH;    // Set destination away from bed
+              feedrate = homing_feedrate[Z_AXIS]/10;
+              current_position[Z_AXIS] = 0;
+              enable_endstops(false);
+#ifdef DEBUG_BUILD
+              SERIAL_ECHOLNPGM("plan_set_position()");
+              MYSERIAL.println(current_position[X_AXIS]);MYSERIAL.println(current_position[Y_AXIS]);
+              MYSERIAL.println(current_position[Z_AXIS]);MYSERIAL.println(current_position[E_AXIS]);
+#endif
+              plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+#ifdef DEBUG_BUILD
+              SERIAL_ECHOLNPGM("plan_buffer_line()");
+              MYSERIAL.println(destination[X_AXIS]);MYSERIAL.println(destination[Y_AXIS]);
+              MYSERIAL.println(destination[Z_AXIS]);MYSERIAL.println(destination[E_AXIS]);
+              MYSERIAL.println(feedrate);MYSERIAL.println(active_extruder);
+#endif
+              plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
+              st_synchronize();
+              current_position[X_AXIS] = destination[X_AXIS];
+              current_position[Y_AXIS] = destination[Y_AXIS];
+              enable_endstops(true);
+              endstops_hit_on_purpose();
+              homeaxis(Z_AXIS);
+            #else // MESH_BED_LEVELING
+              homeaxis(Z_AXIS);
+            #endif // MESH_BED_LEVELING
+          }
+        #else // defined(Z_SAFE_HOMING): Z Safe mode activated.
+          if(home_all_axes) {
+            destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - X_PROBE_OFFSET_FROM_EXTRUDER);
+            destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - Y_PROBE_OFFSET_FROM_EXTRUDER);
+            destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1);    // Set destination away from bed
+            feedrate = XY_TRAVEL_SPEED/60;
+            current_position[Z_AXIS] = 0;
 
-		}
+            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+            plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
+            st_synchronize();
+            current_position[X_AXIS] = destination[X_AXIS];
+            current_position[Y_AXIS] = destination[Y_AXIS];
 
-	}
-	lcd_update_enable(true);
-	lcd_update(2);
-	lcd_setstatuspgm(_T(WELCOME_MSG));
-	disable_z();
-	loading_flag = false;
-	custom_message = false;
-	custom_message_type = 0;
-#endif
+            homeaxis(Z_AXIS);
+          }
+                                                // Let's see if X and Y are homed and probe is inside bed area.
+          if(home_z) {
+            if ( (axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]) \
+              && (current_position[X_AXIS]+X_PROBE_OFFSET_FROM_EXTRUDER >= X_MIN_POS) \
+              && (current_position[X_AXIS]+X_PROBE_OFFSET_FROM_EXTRUDER <= X_MAX_POS) \
+              && (current_position[Y_AXIS]+Y_PROBE_OFFSET_FROM_EXTRUDER >= Y_MIN_POS) \
+              && (current_position[Y_AXIS]+Y_PROBE_OFFSET_FROM_EXTRUDER <= Y_MAX_POS)) {
 
-}
-/**
- * @brief Get serial number from 32U2 processor
- *
- * Typical format of S/N is:CZPX0917X003XC13518
- *
- * Command operates only in farm mode, if not in farm mode, "Not in farm mode." is written to MYSERIAL.
- *
- * Send command ;S to serial port 0 to retrieve serial number stored in 32U2 processor,
- * reply is transmitted to serial port 1 character by character.
- * Operation takes typically 23 ms. If the retransmit is not finished until 100 ms,
- * it is interrupted, so less, or no characters are retransmitted, only newline character is send
- * in any case.
- */
-static void gcode_PRUSA_SN()
-{
-    if (farm_mode) {
-        selectedSerialPort = 0;
-        MSerial.write(";S");
-        int numbersRead = 0;
-        Timer timeout;
-        timeout.start();
+              current_position[Z_AXIS] = 0;
+              plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+              destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1);    // Set destination away from bed
+              feedrate = max_feedrate[Z_AXIS];
+              plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
+              st_synchronize();
 
-        while (numbersRead < 19) {
-            while (MSerial.available() > 0) {
-                uint8_t serial_char = MSerial.read();
-                selectedSerialPort = 1;
-                MSerial.write(serial_char);
-                numbersRead++;
-                selectedSerialPort = 0;
+              homeaxis(Z_AXIS);
+            } else if (!((axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]))) {
+                LCD_MESSAGERPGM(MSG_POSITION_UNKNOWN);
+                SERIAL_ECHO_START;
+                SERIAL_ECHOLNRPGM(MSG_POSITION_UNKNOWN);
+            } else {
+                LCD_MESSAGERPGM(MSG_ZPROBE_OUT);
+                SERIAL_ECHO_START;
+                SERIAL_ECHOLNRPGM(MSG_ZPROBE_OUT);
             }
-            if (timeout.expired(100)) break;
-        }
-        selectedSerialPort = 1;
-        MSerial.write('\n');
-#if 0
-        for (int b = 0; b < 3; b++) {
-            tone(BEEPER, 110);
-            delay(50);
-            noTone(BEEPER);
-            delay(50);
-        }
-#endif
-    } else {
-        MYSERIAL.println("Not in farm mode.");
-    }
-}
+          }
+        #endif // Z_SAFE_HOMING
+      #endif // Z_HOME_DIR < 0
 
-void process_commands()
-{
-	if (!buflen) return; //empty command
-  #ifdef FILAMENT_RUNOUT_SUPPORT
-    SET_INPUT(FR_SENS);
-  #endif
+      if(code_seen(axis_codes[Z_AXIS]) && code_value_long() != 0)
+        current_position[Z_AXIS]=code_value()+add_homing[Z_AXIS];
+      #ifdef ENABLE_AUTO_BED_LEVELING
+        if(home_z)
+          current_position[Z_AXIS] += zprobe_zoffset;  //Add Z_Probe offset (the distance is negative)
+      #endif
+      
+      // Set the planner and stepper routine positions.
+      // At this point the mesh bed leveling and world2machine corrections are disabled and current_position
+      // contains the machine coordinates.
+      plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 
-#ifdef CMDBUFFER_DEBUG
-  SERIAL_ECHOPGM("Processing a GCODE command: ");
-  SERIAL_ECHO(cmdbuffer+bufindr+CMDHDRSIZE);
-  SERIAL_ECHOLNPGM("");
-  SERIAL_ECHOPGM("In cmdqueue: ");
-  SERIAL_ECHO(buflen);
-  SERIAL_ECHOLNPGM("");
-#endif /* CMDBUFFER_DEBUG */
-  
-  unsigned long codenum; //throw away variable
-  char *starpos = NULL;
-#ifdef ENABLE_AUTO_BED_LEVELING
-  float x_tmp, y_tmp, z_tmp, real_z;
+      #ifdef ENDSTOPS_ONLY_FOR_HOMING
+        enable_endstops(false);
+      #endif
+
+      feedrate = saved_feedrate;
+      feedmultiply = saved_feedmultiply;
+      previous_millis_cmd = millis();
+      endstops_hit_on_purpose();
+#ifndef MESH_BED_LEVELING
+      // If MESH_BED_LEVELING is not active, then it is the original Prusa i3.
+      // Offer the user to load the baby step value, which has been adjusted at the previous print session.
+      if(card.sdprinting && eeprom_read_word((uint16_t *)EEPROM_BABYSTEP_Z))
+          lcd_adjust_z();
 #endif
 
-  // PRUSA GCODES
-  KEEPALIVE_STATE(IN_HANDLER);
+    // Load the machine correction matrix
+    world2machine_initialize();
+    // and correct the current_position XY axes to match the transformed coordinate system.
+    world2machine_update_current();
 
-#ifdef SNMM
-  float tmp_motor[3] = DEFAULT_PWM_MOTOR_CURRENT;
-  float tmp_motor_loud[3] = DEFAULT_PWM_MOTOR_CURRENT_LOUD;
-  int8_t SilentMode;
-#endif
-  
-  if (code_seen("M117")) { //moved to highest priority place to be able to to print strings which includes "G", "PRUSA" and "^"
-	  starpos = (strchr(strchr_pointer + 5, '*'));
-	  if (starpos != NULL)
-		  *(starpos) = '\0';
-	  lcd_setstatus(strchr_pointer + 5);
-  }
-
-#ifdef TMC2130
-	else if (strncmp_P(CMDBUFFER_CURRENT_STRING, PSTR("CRASH_"), 6) == 0)
-	{
-	  if(code_seen("CRASH_DETECTED"))
-	  {
-		  uint8_t mask = 0;
-		  if (code_seen("X")) mask |= X_AXIS_MASK;
-		  if (code_seen("Y")) mask |= Y_AXIS_MASK;
-		  crashdet_detected(mask);
-	  }
-	  else if(code_seen("CRASH_RECOVER"))
-		  crashdet_recover();
-	  else if(code_seen("CRASH_CANCEL"))
-		  crashdet_cancel();
-	}
-	else if (strncmp_P(CMDBUFFER_CURRENT_STRING, PSTR("TMC_"), 4) == 0)
-	{
-		if (strncmp_P(CMDBUFFER_CURRENT_STRING + 4, PSTR("SET_WAVE_E"), 10) == 0)
+#if (defined(MESH_BED_LEVELING) && !defined(MK1BP))
+	if (code_seen(axis_codes[X_AXIS]) || code_seen(axis_codes[Y_AXIS]) || code_seen('W') || code_seen(axis_codes[Z_AXIS]))
 		{
-			uint8_t fac = (uint8_t)strtol(CMDBUFFER_CURRENT_STRING + 14, NULL, 10);
-			tmc2130_set_wave(E_AXIS, 247, fac);
+      if (! home_z && mbl_was_active) {
+        // Re-enable the mesh bed leveling if only the X and Y axes were re-homed.
+        mbl.active = true;
+        // and re-adjust the current logical Z axis with the bed leveling offset applicable at the current XY position.
+        current_position[Z_AXIS] -= mbl.get_z(st_get_position_mm(X_AXIS), st_get_position_mm(Y_AXIS));
+      }
 		}
-		else if (strncmp_P(CMDBUFFER_CURRENT_STRING + 4, PSTR("SET_STEP_E"), 10) == 0)
+	else
 		{
-			uint8_t step = (uint8_t)strtol(CMDBUFFER_CURRENT_STRING + 14, NULL, 10);
-			uint16_t res = tmc2130_get_res(E_AXIS);
-			tmc2130_goto_step(E_AXIS, step & (4*res - 1), 2, 1000, res);
-		}
-	}
-#endif //TMC2130
-
-  else if(code_seen("PRUSA")){
-		if (code_seen("Ping")) {  //PRUSA Ping
-			if (farm_mode) {
-				PingTime = millis();
-				//MYSERIAL.print(farm_no); MYSERIAL.println(": OK");
-			}	  
-		}
-		else if (code_seen("PRN")) {
-		  MYSERIAL.println(status_number);
+			st_synchronize();
+			homing_flag = false;
+			// Push the commands to the front of the message queue in the reverse order!
+			// There shall be always enough space reserved for these commands.
+			enquecommand_front_P((PSTR("G80")));
+			//goto case_G80;
+	  }
+#endif
 
-        }else if (code_seen("FAN")) {
-            MYSERIAL.print("E0:");
-            MYSERIAL.print(60*fan_speed[0]);
-            MYSERIAL.println(" RPM");
-            MYSERIAL.print("PRN0:");
-            MYSERIAL.print(60*fan_speed[1]);
-            MYSERIAL.println(" RPM");
-            
-        }else if (code_seen("fn")) {
-		  if (farm_mode) {
-			  MYSERIAL.println(farm_no);
-		  }
-		  else {
-			  MYSERIAL.println("Not in farm mode.");
-		  }
-		  
-		}
-		else if (code_seen("thx")) {
-			no_response = false;
-		}else if (code_seen("fv")) {
-        // get file version
-        #ifdef SDSUPPORT
-        card.openFile(strchr_pointer + 3,true);
-        while (true) {
-            uint16_t readByte = card.get();
-            MYSERIAL.write(readByte);
-            if (readByte=='\n') {
-                break;
-            }
-        }
-        card.closefile();
+	  if (farm_mode) { prusa_statistics(20); };
 
-        #endif // SDSUPPORT
+	  homing_flag = false;
+#if 0
+      SERIAL_ECHOPGM("G28, final ");  print_world_coordinates();
+      SERIAL_ECHOPGM("G28, final ");  print_physical_coordinates();
+      SERIAL_ECHOPGM("G28, final ");  print_mesh_bed_leveling_table();
+#endif
+}
 
-    } else if (code_seen("M28")) {
-        trace();
-        prusa_sd_card_upload = true;
-        card.openFile(strchr_pointer+4,false);
-	} else if (code_seen("SN")) { 
-        gcode_PRUSA_SN();
-	} else if(code_seen("Fir")){
 
-      SERIAL_PROTOCOLLN(FW_VERSION);
+bool gcode_M45(bool onlyZ, int8_t verbosity_level)
+{
+	bool final_result = false;
+	#ifdef TMC2130
+	FORCE_HIGH_POWER_START;
+	#endif // TMC2130
+	// Only Z calibration?
+	if (!onlyZ)
+	{
+		setTargetBed(0);
+		setTargetHotend(0, 0);
+		setTargetHotend(0, 1);
+		setTargetHotend(0, 2);
+		adjust_bed_reset(); //reset bed level correction
+	}
 
-    } else if(code_seen("Rev")){
+	// Disable the default update procedure of the display. We will do a modal dialog.
+	lcd_update_enable(false);
+	// Let the planner use the uncorrected coordinates.
+	mbl.reset();
+	// Reset world2machine_rotation_and_skew and world2machine_shift, therefore
+	// the planner will not perform any adjustments in the XY plane. 
+	// Wait for the motors to stop and update the current position with the absolute values.
+	world2machine_revert_to_uncorrected();
+	// Reset the baby step value applied without moving the axes.
+	babystep_reset();
+	// Mark all axes as in a need for homing.
+	memset(axis_known_position, 0, sizeof(axis_known_position));
 
-      SERIAL_PROTOCOLLN(FILAMENT_SIZE "-" ELECTRONICS "-" NOZZLE_TYPE );
+	// Home in the XY plane.
+	//set_destination_to_current();
+	setup_for_endstop_move();
+	lcd_display_message_fullscreen_P(_T(MSG_AUTO_HOME));
+	home_xy();
 
-    } else if(code_seen("Lang")) {
-      lcd_force_language_selection();
-    } else if(code_seen("Lz")) {
-      EEPROM_save_B(EEPROM_BABYSTEP_Z,0);
-      
-    } else if (code_seen("SERIAL LOW")) {
-        MYSERIAL.println("SERIAL LOW");
-        MYSERIAL.begin(BAUDRATE);
-        return;
-    } else if (code_seen("SERIAL HIGH")) {
-        MYSERIAL.println("SERIAL HIGH");
-        MYSERIAL.begin(1152000);
-        return;
-    } else if(code_seen("Beat")) {
-        // Kick farm link timer
-        kicktime = millis();
+	enable_endstops(false);
+	current_position[X_AXIS] += 5;
+	current_position[Y_AXIS] += 5;
+	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS] / 40, active_extruder);
+	st_synchronize();
 
-    } else if(code_seen("FR")) {
-        // Factory full reset
-        factory_reset(0,true);        
-    }
-    //else if (code_seen('Cal')) {
-		//  lcd_calibration();
-	  // }
+	// Let the user move the Z axes up to the end stoppers.
+#ifdef TMC2130
+	if (calibrate_z_auto())
+	{
+#else //TMC2130
+	if (lcd_calibrate_z_end_stop_manual(onlyZ))
+	{
+#endif //TMC2130
+		refresh_cmd_timeout();
+		#ifndef STEEL_SHEET
+		if (((degHotend(0) > MAX_HOTEND_TEMP_CALIBRATION) || (degBed() > MAX_BED_TEMP_CALIBRATION)) && (!onlyZ))
+		{
+			lcd_wait_for_cool_down();
+		}
+		#endif //STEEL_SHEET
+		if(!onlyZ)
+		{
+			KEEPALIVE_STATE(PAUSED_FOR_USER);
+			#ifdef STEEL_SHEET
+			bool result = lcd_show_fullscreen_message_yes_no_and_wait_P(_T(MSG_STEEL_SHEET_CHECK), false, false);
+			if(result) lcd_show_fullscreen_message_and_wait_P(_T(MSG_REMOVE_STEEL_SHEET));
+			#endif //STEEL_SHEET
+			lcd_show_fullscreen_message_and_wait_P(_T(MSG_CONFIRM_NOZZLE_CLEAN));
+		    lcd_show_fullscreen_message_and_wait_P(_T(MSG_PAPER));
+			KEEPALIVE_STATE(IN_HANDLER);
+			lcd_display_message_fullscreen_P(_T(MSG_FIND_BED_OFFSET_AND_SKEW_LINE1));
+			lcd_implementation_print_at(0, 2, 1);
+			lcd_printPGM(_T(MSG_FIND_BED_OFFSET_AND_SKEW_LINE2));
+		}
+		// Move the print head close to the bed.
+		current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
 
-  }  
-  else if (code_seen('^')) {
-    // nothing, this is a version line
-  } else if(code_seen('G'))
-  {
-    switch((int)code_value())
-    {
-    case 0: // G0 -> G1
-    case 1: // G1
-      if(Stopped == false) {
+		bool endstops_enabled  = enable_endstops(true);
+#ifdef TMC2130
+		tmc2130_home_enter(Z_AXIS_MASK);
+#endif //TMC2130
 
-        #ifdef FILAMENT_RUNOUT_SUPPORT
-            
-            if(READ(FR_SENS)){
+		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS] / 40, active_extruder);
 
-                        feedmultiplyBckp=feedmultiply;
-                        float target[4];
-                        float lastpos[4];
-                        target[X_AXIS]=current_position[X_AXIS];
-                        target[Y_AXIS]=current_position[Y_AXIS];
-                        target[Z_AXIS]=current_position[Z_AXIS];
-                        target[E_AXIS]=current_position[E_AXIS];
-                        lastpos[X_AXIS]=current_position[X_AXIS];
-                        lastpos[Y_AXIS]=current_position[Y_AXIS];
-                        lastpos[Z_AXIS]=current_position[Z_AXIS];
-                        lastpos[E_AXIS]=current_position[E_AXIS];
-                        //retract by E
-                        
-                        target[E_AXIS]+= FILAMENTCHANGE_FIRSTRETRACT ;
-                        
-                        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 400, active_extruder);
+		st_synchronize();
+#ifdef TMC2130
+		tmc2130_home_exit();
+#endif //TMC2130
+		enable_endstops(endstops_enabled);
 
+		if (st_get_position_mm(Z_AXIS) == MESH_HOME_Z_SEARCH)
+		{
 
-                        target[Z_AXIS]+= FILAMENTCHANGE_ZADD ;
+			int8_t verbosity_level = 0;
+			if (code_seen('V'))
+			{
+				// Just 'V' without a number counts as V1.
+				char c = strchr_pointer[1];
+				verbosity_level = (c == ' ' || c == '\t' || c == 0) ? 1 : code_value_short();
+			}
 
-                        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 300, active_extruder);
-
-                        target[X_AXIS]= FILAMENTCHANGE_XPOS ;
-                        
-                        target[Y_AXIS]= FILAMENTCHANGE_YPOS ;
-                         
-                 
-                        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 70, active_extruder);
-
-                        target[E_AXIS]+= FILAMENTCHANGE_FINALRETRACT ;
-                          
+			if (onlyZ)
+			{
+				clean_up_after_endstop_move();
+				// Z only calibration.
+				// Load the machine correction matrix
+				world2machine_initialize();
+				// and correct the current_position to match the transformed coordinate system.
+				world2machine_update_current();
+				//FIXME
+				bool result = sample_mesh_and_store_reference();
+				if (result)
+				{
+					if (calibration_status() == CALIBRATION_STATUS_Z_CALIBRATION)
+						// Shipped, the nozzle height has been set already. The user can start printing now.
+						calibration_status_store(CALIBRATION_STATUS_CALIBRATED);
+						final_result = true;
+					// babystep_apply();
+				}
+			}
+			else
+			{
+				// Reset the baby step value and the baby step applied flag.
+				calibration_status_store(CALIBRATION_STATUS_XYZ_CALIBRATION);
+				eeprom_update_word((uint16_t*)EEPROM_BABYSTEP_Z, 0);
+				// Complete XYZ calibration.
+				uint8_t point_too_far_mask = 0;
+				BedSkewOffsetDetectionResultType result = find_bed_offset_and_skew(verbosity_level, point_too_far_mask);
+				clean_up_after_endstop_move();
+				// Print head up.
+				current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS] / 40, active_extruder);
+				st_synchronize();
+//#ifndef NEW_XYZCAL
+				if (result >= 0)
+				{
+					#ifdef HEATBED_V2
+					sample_z();
+					#else //HEATBED_V2
+					point_too_far_mask = 0;
+					// Second half: The fine adjustment.
+					// Let the planner use the uncorrected coordinates.
+					mbl.reset();
+					world2machine_reset();
+					// Home in the XY plane.
+					setup_for_endstop_move();
+					home_xy();
+					result = improve_bed_offset_and_skew(1, verbosity_level, point_too_far_mask);
+					clean_up_after_endstop_move();
+					// Print head up.
+					current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+					plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS] / 40, active_extruder);
+					st_synchronize();
+					// if (result >= 0) babystep_apply();					
+					#endif //HEATBED_V2
+				}
+//#endif //NEW_XYZCAL
+				lcd_update_enable(true);
+				lcd_update(2);
 
-                        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 20, active_extruder);
+				lcd_bed_calibration_show_result(result, point_too_far_mask);
+				if (result >= 0)
+				{
+					// Calibration valid, the machine should be able to print. Advise the user to run the V2Calibration.gcode.
+					calibration_status_store(CALIBRATION_STATUS_LIVE_ADJUST);
+					if (eeprom_read_byte((uint8_t*)EEPROM_WIZARD_ACTIVE) != 1) lcd_show_fullscreen_message_and_wait_P(_T(MSG_BABYSTEP_Z_NOT_SET));
+					final_result = true;
+				}
+			}
+#ifdef TMC2130
+			tmc2130_home_exit();
+#endif
+		}
+		else
+		{
+			lcd_show_fullscreen_message_and_wait_P(PSTR("Calibration failed! Check the axes and run again."));
+			final_result = false;
+		}
+	}
+	else
+	{
+		// Timeouted.
+	}
+	lcd_update_enable(true);
+#ifdef TMC2130
+	FORCE_HIGH_POWER_END;
+#endif // TMC2130
+	return final_result;
+}
 
-                        //finish moves
-                        st_synchronize();
-                        //disable extruder steppers so filament can be removed
-                        disable_e0();
-                        disable_e1();
-                        disable_e2();
-                        delay(100);
-                        
-                        //LCD_ALERTMESSAGEPGM(_T(MSG_FILAMENTCHANGE));
-                        uint8_t cnt=0;
-                        int counterBeep = 0;
-                        lcd_wait_interact();
-                        while(!lcd_clicked()){
-                          cnt++;
-                          manage_heater();
-                          manage_inactivity(true);
-                          //lcd_update();
-                          if(cnt==0)
-                          {
-                          #if BEEPER > 0
-                          
-                            if (counterBeep== 500){
-                              counterBeep = 0;
-                              
-                            }
-                          
-                            
-                            SET_OUTPUT(BEEPER);
-                            if (counterBeep== 0){
-                              WRITE(BEEPER,HIGH);
-                            }
-                            
-                            if (counterBeep== 20){
-                              WRITE(BEEPER,LOW);
-                            }
-                            
-                            
-                            
-                          
-                            counterBeep++;
-                          #else
-                      #if !defined(LCD_FEEDBACK_FREQUENCY_HZ) || !defined(LCD_FEEDBACK_FREQUENCY_DURATION_MS)
-                              lcd_buzz(1000/6,100);
-                      #else
-                        lcd_buzz(LCD_FEEDBACK_FREQUENCY_DURATION_MS,LCD_FEEDBACK_FREQUENCY_HZ);
-                      #endif
-                          #endif
-                          }
-                        }
-                        
-                        WRITE(BEEPER,LOW);
-                        
-                        target[E_AXIS]+= FILAMENTCHANGE_FIRSTFEED ;
-                        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 20, active_extruder); 
-                        
-                        
-                        target[E_AXIS]+= FILAMENTCHANGE_FINALFEED ;
-                        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 2, active_extruder); 
-                        
-                 
-                        
-                        
-                        
-                        lcd_change_fil_state = 0;
-                        lcd_loading_filament();
-                        while ((lcd_change_fil_state == 0)||(lcd_change_fil_state != 1)){
-                        
-                          lcd_change_fil_state = 0;
-                          lcd_alright();
-                          switch(lcd_change_fil_state){
-                          
-                             case 2:
-                                     target[E_AXIS]+= FILAMENTCHANGE_FIRSTFEED ;
-                                     plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 20, active_extruder); 
-                        
-                        
-                                     target[E_AXIS]+= FILAMENTCHANGE_FINALFEED ;
-                                     plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 2, active_extruder); 
-                                      
-                                     
-                                     lcd_loading_filament();
-                                     break;
-                             case 3:
-                                     target[E_AXIS]+= FILAMENTCHANGE_FINALFEED ;
-                                     plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 2, active_extruder); 
-                                     lcd_loading_color();
-                                     break;
-                                          
-                             default:
-                                     lcd_change_success();
-                                     break;
-                          }
-                          
-                        }
-                        
+void gcode_M114()
+{
+	SERIAL_PROTOCOLPGM("X:");
+	SERIAL_PROTOCOL(current_position[X_AXIS]);
+	SERIAL_PROTOCOLPGM(" Y:");
+	SERIAL_PROTOCOL(current_position[Y_AXIS]);
+	SERIAL_PROTOCOLPGM(" Z:");
+	SERIAL_PROTOCOL(current_position[Z_AXIS]);
+	SERIAL_PROTOCOLPGM(" E:");
+	SERIAL_PROTOCOL(current_position[E_AXIS]);
 
-                        
-                      target[E_AXIS]+= 5;
-                      plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 2, active_extruder);
-                        
-                      target[E_AXIS]+= FILAMENTCHANGE_FIRSTRETRACT;
-                      plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 400, active_extruder);
-                        
+	SERIAL_PROTOCOLRPGM(_n(" Count X: "));////MSG_COUNT_X c=0 r=0
+	SERIAL_PROTOCOL(float(st_get_position(X_AXIS)) / axis_steps_per_unit[X_AXIS]);
+	SERIAL_PROTOCOLPGM(" Y:");
+	SERIAL_PROTOCOL(float(st_get_position(Y_AXIS)) / axis_steps_per_unit[Y_AXIS]);
+	SERIAL_PROTOCOLPGM(" Z:");
+	SERIAL_PROTOCOL(float(st_get_position(Z_AXIS)) / axis_steps_per_unit[Z_AXIS]);
+	SERIAL_PROTOCOLPGM(" E:");
+	SERIAL_PROTOCOL(float(st_get_position(E_AXIS)) / axis_steps_per_unit[E_AXIS]);
 
-                        //current_position[E_AXIS]=target[E_AXIS]; //the long retract of L is compensated by manual filament feeding
-                        //plan_set_e_position(current_position[E_AXIS]);
-                        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 70, active_extruder); //should do nothing
-                        plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], target[Z_AXIS], target[E_AXIS], 70, active_extruder); //move xy back
-                        plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], target[E_AXIS], 200, active_extruder); //move z back
-                        
-                        
-                        target[E_AXIS]= target[E_AXIS] - FILAMENTCHANGE_FIRSTRETRACT;
-                        
-                      
-                             
-                        plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], target[E_AXIS], 5, active_extruder); //final untretract
-                        
-                        
-                        plan_set_e_position(lastpos[E_AXIS]);
-                        
-                        feedmultiply=feedmultiplyBckp;
-                        
-                     
-                        
-                        char cmd[9];
+	SERIAL_PROTOCOLLN("");
+}
 
-                        sprintf_P(cmd, PSTR("M220 S%i"), feedmultiplyBckp);
-                        enquecommand(cmd);
+void gcode_M701()
+{
+#ifdef SNMM
+	extr_adj(snmm_extruder);//loads current extruder
+#else
+	enable_z();
+	custom_message = true;
+	custom_message_type = 2;
 
-            }
+	lcd_setstatuspgm(_T(MSG_LOADING_FILAMENT));
+	current_position[E_AXIS] += 70;
+	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 400 / 60, active_extruder); //fast sequence
 
+	current_position[E_AXIS] += 25;
+	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 100 / 60, active_extruder); //slow sequence
+	st_synchronize();
 
+	tone(BEEPER, 500);
+	delay_keep_alive(50);
+	noTone(BEEPER);
 
-        #endif
+	if (!farm_mode && loading_flag) {
+		bool clean = lcd_show_fullscreen_message_yes_no_and_wait_P(_T(MSG_FILAMENT_CLEAN), false, true);
 
+		while (!clean) {
+			lcd_update_enable(true);
+			lcd_update(2);
+			current_position[E_AXIS] += 25;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 100 / 60, active_extruder); //slow sequence
+			st_synchronize();
+			clean = lcd_show_fullscreen_message_yes_no_and_wait_P(_T(MSG_FILAMENT_CLEAN), false, true);
 
-        get_coordinates(); // For X Y Z E F
-		if (total_filament_used > ((current_position[E_AXIS] - destination[E_AXIS]) * 100)) { //protection against total_filament_used overflow
-			total_filament_used = total_filament_used + ((destination[E_AXIS] - current_position[E_AXIS]) * 100);
 		}
-          #ifdef FWRETRACT
-            if(autoretract_enabled)
-            if( !(code_seen('X') || code_seen('Y') || code_seen('Z')) && code_seen('E')) {
-              float echange=destination[E_AXIS]-current_position[E_AXIS];
 
-              if((echange<-MIN_RETRACT && !retracted) || (echange>MIN_RETRACT && retracted)) { //move appears to be an attempt to retract or recover
-                  current_position[E_AXIS] = destination[E_AXIS]; //hide the slicer-generated retract/recover from calculations
-                  plan_set_e_position(current_position[E_AXIS]); //AND from the planner
-                  retract(!retracted);
-                  return;
-              }
+	}
+	lcd_update_enable(true);
+	lcd_update(2);
+	lcd_setstatuspgm(_T(WELCOME_MSG));
+	disable_z();
+	loading_flag = false;
+	custom_message = false;
+	custom_message_type = 0;
+#endif
 
+}
+/**
+ * @brief Get serial number from 32U2 processor
+ *
+ * Typical format of S/N is:CZPX0917X003XC13518
+ *
+ * Command operates only in farm mode, if not in farm mode, "Not in farm mode." is written to MYSERIAL.
+ *
+ * Send command ;S to serial port 0 to retrieve serial number stored in 32U2 processor,
+ * reply is transmitted to serial port 1 character by character.
+ * Operation takes typically 23 ms. If the retransmit is not finished until 100 ms,
+ * it is interrupted, so less, or no characters are retransmitted, only newline character is send
+ * in any case.
+ */
+static void gcode_PRUSA_SN()
+{
+    if (farm_mode) {
+        selectedSerialPort = 0;
+        MSerial.write(";S");
+        int numbersRead = 0;
+        Timer timeout;
+        timeout.start();
 
+        while (numbersRead < 19) {
+            while (MSerial.available() > 0) {
+                uint8_t serial_char = MSerial.read();
+                selectedSerialPort = 1;
+                MSerial.write(serial_char);
+                numbersRead++;
+                selectedSerialPort = 0;
             }
-          #endif //FWRETRACT
-        prepare_move();
-        //ClearToSend();
-      }
-      break;
-    case 2: // G2  - CW ARC
-      if(Stopped == false) {
-        get_arc_coordinates();
-        prepare_arc_move(true);
-      }
-      break;
-    case 3: // G3  - CCW ARC
-      if(Stopped == false) {
-        get_arc_coordinates();
-        prepare_arc_move(false);
-      }
-      break;
-    case 4: // G4 dwell      
-      codenum = 0;
-      if(code_seen('P')) codenum = code_value(); // milliseconds to wait
-      if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
-	  if(codenum != 0) LCD_MESSAGERPGM(_i("Sleep..."));////MSG_DWELL c=0 r=0
-      st_synchronize();
-      codenum += millis();  // keep track of when we started waiting
-      previous_millis_cmd = millis();
-      while(millis() < codenum) {
-        manage_heater();
-        manage_inactivity();
-        lcd_update();
-      }
-      break;
-      #ifdef FWRETRACT
-      case 10: // G10 retract
-       #if EXTRUDERS > 1
-        retracted_swap[active_extruder]=(code_seen('S') && code_value_long() == 1); // checks for swap retract argument
-        retract(true,retracted_swap[active_extruder]);
-       #else
-        retract(true);
-       #endif
-      break;
-      case 11: // G11 retract_recover
-       #if EXTRUDERS > 1
-        retract(false,retracted_swap[active_extruder]);
-       #else
-        retract(false);
-       #endif 
-      break;
-      #endif //FWRETRACT
-    case 28: //G28 Home all Axis one at a time
-    {
-      st_synchronize();
-
+            if (timeout.expired(100)) break;
+        }
+        selectedSerialPort = 1;
+        MSerial.write('\n');
 #if 0
-      SERIAL_ECHOPGM("G28, initial ");  print_world_coordinates();
-      SERIAL_ECHOPGM("G28, initial ");  print_physical_coordinates();
+        for (int b = 0; b < 3; b++) {
+            tone(BEEPER, 110);
+            delay(50);
+            noTone(BEEPER);
+            delay(50);
+        }
 #endif
+    } else {
+        MYSERIAL.println("Not in farm mode.");
+    }
+}
 
-      // Flag for the display update routine and to disable the print cancelation during homing.
-		  homing_flag = true;
-      
-      // Which axes should be homed?
-      bool home_x = code_seen(axis_codes[X_AXIS]);
-      bool home_y = code_seen(axis_codes[Y_AXIS]);
-      bool home_z = code_seen(axis_codes[Z_AXIS]);
-      // calibrate?
-      bool calib = code_seen('C');
-      // Either all X,Y,Z codes are present, or none of them.
-      bool home_all_axes = home_x == home_y && home_x == home_z;
-      if (home_all_axes)
-        // No X/Y/Z code provided means to home all axes.
-        home_x = home_y = home_z = true;
+void process_commands()
+{
+	if (!buflen) return; //empty command
+  #ifdef FILAMENT_RUNOUT_SUPPORT
+    SET_INPUT(FR_SENS);
+  #endif
 
+#ifdef CMDBUFFER_DEBUG
+  SERIAL_ECHOPGM("Processing a GCODE command: ");
+  SERIAL_ECHO(cmdbuffer+bufindr+CMDHDRSIZE);
+  SERIAL_ECHOLNPGM("");
+  SERIAL_ECHOPGM("In cmdqueue: ");
+  SERIAL_ECHO(buflen);
+  SERIAL_ECHOLNPGM("");
+#endif /* CMDBUFFER_DEBUG */
+  
+  unsigned long codenum; //throw away variable
+  char *starpos = NULL;
 #ifdef ENABLE_AUTO_BED_LEVELING
-      plan_bed_level_matrix.set_to_identity();  //Reset the plane ("erase" all leveling data)
-#endif //ENABLE_AUTO_BED_LEVELING
+  float x_tmp, y_tmp, z_tmp, real_z;
+#endif
+
+  // PRUSA GCODES
+  KEEPALIVE_STATE(IN_HANDLER);
+
+#ifdef SNMM
+  float tmp_motor[3] = DEFAULT_PWM_MOTOR_CURRENT;
+  float tmp_motor_loud[3] = DEFAULT_PWM_MOTOR_CURRENT_LOUD;
+  int8_t SilentMode;
+#endif
+  
+  if (code_seen("M117")) { //moved to highest priority place to be able to to print strings which includes "G", "PRUSA" and "^"
+	  starpos = (strchr(strchr_pointer + 5, '*'));
+	  if (starpos != NULL)
+		  *(starpos) = '\0';
+	  lcd_setstatus(strchr_pointer + 5);
+  }
+
+#ifdef TMC2130
+	else if (strncmp_P(CMDBUFFER_CURRENT_STRING, PSTR("CRASH_"), 6) == 0)
+	{
+	  if(code_seen("CRASH_DETECTED"))
+	  {
+		  uint8_t mask = 0;
+		  if (code_seen("X")) mask |= X_AXIS_MASK;
+		  if (code_seen("Y")) mask |= Y_AXIS_MASK;
+		  crashdet_detected(mask);
+	  }
+	  else if(code_seen("CRASH_RECOVER"))
+		  crashdet_recover();
+	  else if(code_seen("CRASH_CANCEL"))
+		  crashdet_cancel();
+	}
+	else if (strncmp_P(CMDBUFFER_CURRENT_STRING, PSTR("TMC_"), 4) == 0)
+	{
+		if (strncmp_P(CMDBUFFER_CURRENT_STRING + 4, PSTR("SET_WAVE_E"), 10) == 0)
+		{
+			uint8_t fac = (uint8_t)strtol(CMDBUFFER_CURRENT_STRING + 14, NULL, 10);
+			tmc2130_set_wave(E_AXIS, 247, fac);
+		}
+		else if (strncmp_P(CMDBUFFER_CURRENT_STRING + 4, PSTR("SET_STEP_E"), 10) == 0)
+		{
+			uint8_t step = (uint8_t)strtol(CMDBUFFER_CURRENT_STRING + 14, NULL, 10);
+			uint16_t res = tmc2130_get_res(E_AXIS);
+			tmc2130_goto_step(E_AXIS, step & (4*res - 1), 2, 1000, res);
+		}
+	}
+#endif //TMC2130
+
+  else if(code_seen("PRUSA")){
+		if (code_seen("Ping")) {  //PRUSA Ping
+			if (farm_mode) {
+				PingTime = millis();
+				//MYSERIAL.print(farm_no); MYSERIAL.println(": OK");
+			}	  
+		}
+		else if (code_seen("PRN")) {
+		  MYSERIAL.println(status_number);
+
+        }else if (code_seen("FAN")) {
+            MYSERIAL.print("E0:");
+            MYSERIAL.print(60*fan_speed[0]);
+            MYSERIAL.println(" RPM");
+            MYSERIAL.print("PRN0:");
+            MYSERIAL.print(60*fan_speed[1]);
+            MYSERIAL.println(" RPM");
             
-      // Reset world2machine_rotation_and_skew and world2machine_shift, therefore
-      // the planner will not perform any adjustments in the XY plane. 
-      // Wait for the motors to stop and update the current position with the absolute values.
-      world2machine_revert_to_uncorrected();
+        }else if (code_seen("fn")) {
+		  if (farm_mode) {
+			  MYSERIAL.println(farm_no);
+		  }
+		  else {
+			  MYSERIAL.println("Not in farm mode.");
+		  }
+		  
+		}
+		else if (code_seen("thx")) {
+			no_response = false;
+		}else if (code_seen("fv")) {
+        // get file version
+        #ifdef SDSUPPORT
+        card.openFile(strchr_pointer + 3,true);
+        while (true) {
+            uint16_t readByte = card.get();
+            MYSERIAL.write(readByte);
+            if (readByte=='\n') {
+                break;
+            }
+        }
+        card.closefile();
 
-      // For mesh bed leveling deactivate the matrix temporarily.
-      // It is necessary to disable the bed leveling for the X and Y homing moves, so that the move is performed
-      // in a single axis only.
-      // In case of re-homing the X or Y axes only, the mesh bed leveling is restored after G28.
-#ifdef MESH_BED_LEVELING
-      uint8_t mbl_was_active = mbl.active;
-      mbl.active = 0;
-      current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
-#endif
+        #endif // SDSUPPORT
 
-      // Reset baby stepping to zero, if the babystepping has already been loaded before. The babystepsTodo value will be
-      // consumed during the first movements following this statement.
-      if (home_z)
-        babystep_undo();
+    } else if (code_seen("M28")) {
+        trace();
+        prusa_sd_card_upload = true;
+        card.openFile(strchr_pointer+4,false);
+	} else if (code_seen("SN")) { 
+        gcode_PRUSA_SN();
+	} else if(code_seen("Fir")){
 
-      saved_feedrate = feedrate;
-      saved_feedmultiply = feedmultiply;
-      feedmultiply = 100;
-      previous_millis_cmd = millis();
+      SERIAL_PROTOCOLLN(FW_VERSION);
 
-      enable_endstops(true);
+    } else if(code_seen("Rev")){
 
-      memcpy(destination, current_position, sizeof(destination));
-      feedrate = 0.0;
+      SERIAL_PROTOCOLLN(FILAMENT_SIZE "-" ELECTRONICS "-" NOZZLE_TYPE );
 
-      #if Z_HOME_DIR > 0                      // If homing away from BED do Z first
-      if(home_z)
-        homeaxis(Z_AXIS);
-      #endif
+    } else if(code_seen("Lang")) {
+      lcd_force_language_selection();
+    } else if(code_seen("Lz")) {
+      EEPROM_save_B(EEPROM_BABYSTEP_Z,0);
+      
+    } else if (code_seen("SERIAL LOW")) {
+        MYSERIAL.println("SERIAL LOW");
+        MYSERIAL.begin(BAUDRATE);
+        return;
+    } else if (code_seen("SERIAL HIGH")) {
+        MYSERIAL.println("SERIAL HIGH");
+        MYSERIAL.begin(1152000);
+        return;
+    } else if(code_seen("Beat")) {
+        // Kick farm link timer
+        kicktime = millis();
 
-      #ifdef QUICK_HOME
-      // In the quick mode, if both x and y are to be homed, a diagonal move will be performed initially.
-      if(home_x && home_y)  //first diagonal move
-      {
-        current_position[X_AXIS] = 0;current_position[Y_AXIS] = 0;
+    } else if(code_seen("FR")) {
+        // Factory full reset
+        factory_reset(0,true);        
+    }
+    //else if (code_seen('Cal')) {
+		//  lcd_calibration();
+	  // }
 
-        int x_axis_home_dir = home_dir(X_AXIS);
+  }  
+  else if (code_seen('^')) {
+    // nothing, this is a version line
+  } else if(code_seen('G'))
+  {
+    switch((int)code_value())
+    {
+    case 0: // G0 -> G1
+    case 1: // G1
+      if(Stopped == false) {
 
-        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-        destination[X_AXIS] = 1.5 * max_length(X_AXIS) * x_axis_home_dir;destination[Y_AXIS] = 1.5 * max_length(Y_AXIS) * home_dir(Y_AXIS);
-        feedrate = homing_feedrate[X_AXIS];
-        if(homing_feedrate[Y_AXIS]<feedrate)
-          feedrate = homing_feedrate[Y_AXIS];
-        if (max_length(X_AXIS) > max_length(Y_AXIS)) {
-          feedrate *= sqrt(pow(max_length(Y_AXIS) / max_length(X_AXIS), 2) + 1);
-        } else {
-          feedrate *= sqrt(pow(max_length(X_AXIS) / max_length(Y_AXIS), 2) + 1);
-        }
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-        st_synchronize();
+        #ifdef FILAMENT_RUNOUT_SUPPORT
+            
+            if(READ(FR_SENS)){
 
-        axis_is_at_home(X_AXIS);
-        axis_is_at_home(Y_AXIS);
-        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-        destination[X_AXIS] = current_position[X_AXIS];
-        destination[Y_AXIS] = current_position[Y_AXIS];
-        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-        feedrate = 0.0;
-        st_synchronize();
-        endstops_hit_on_purpose();
+                        feedmultiplyBckp=feedmultiply;
+                        float target[4];
+                        float lastpos[4];
+                        target[X_AXIS]=current_position[X_AXIS];
+                        target[Y_AXIS]=current_position[Y_AXIS];
+                        target[Z_AXIS]=current_position[Z_AXIS];
+                        target[E_AXIS]=current_position[E_AXIS];
+                        lastpos[X_AXIS]=current_position[X_AXIS];
+                        lastpos[Y_AXIS]=current_position[Y_AXIS];
+                        lastpos[Z_AXIS]=current_position[Z_AXIS];
+                        lastpos[E_AXIS]=current_position[E_AXIS];
+                        //retract by E
+                        
+                        target[E_AXIS]+= FILAMENTCHANGE_FIRSTRETRACT ;
+                        
+                        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 400, active_extruder);
 
-        current_position[X_AXIS] = destination[X_AXIS];
-        current_position[Y_AXIS] = destination[Y_AXIS];
-        current_position[Z_AXIS] = destination[Z_AXIS];
-      }
-      #endif /* QUICK_HOME */
 
-#ifdef TMC2130	 
-      if(home_x)
-	  {
-		if (!calib)
-			homeaxis(X_AXIS);
-		else
-			tmc2130_home_calibrate(X_AXIS);
-	  }
+                        target[Z_AXIS]+= FILAMENTCHANGE_ZADD ;
 
-      if(home_y)
-	  {
-		if (!calib)
-	        homeaxis(Y_AXIS);
-		else
-			tmc2130_home_calibrate(Y_AXIS);
-	  }
-#endif //TMC2130
+                        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 300, active_extruder);
 
+                        target[X_AXIS]= FILAMENTCHANGE_XPOS ;
+                        
+                        target[Y_AXIS]= FILAMENTCHANGE_YPOS ;
+                         
+                 
+                        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 70, active_extruder);
 
-      if(code_seen(axis_codes[X_AXIS]) && code_value_long() != 0)
-        current_position[X_AXIS]=code_value()+add_homing[X_AXIS];
+                        target[E_AXIS]+= FILAMENTCHANGE_FINALRETRACT ;
+                          
 
-      if(code_seen(axis_codes[Y_AXIS]) && code_value_long() != 0)
-		    current_position[Y_AXIS]=code_value()+add_homing[Y_AXIS];
+                        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 20, active_extruder);
 
-      #if Z_HOME_DIR < 0                      // If homing towards BED do Z last
-        #ifndef Z_SAFE_HOMING
-          if(home_z) {
-            #if defined (Z_RAISE_BEFORE_HOMING) && (Z_RAISE_BEFORE_HOMING > 0)
-              destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1);    // Set destination away from bed
-              feedrate = max_feedrate[Z_AXIS];
-              plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
-              st_synchronize();
-            #endif // defined (Z_RAISE_BEFORE_HOMING) && (Z_RAISE_BEFORE_HOMING > 0)
-            #if (defined(MESH_BED_LEVELING) && !defined(MK1BP))  // If Mesh bed leveling, moxve X&Y to safe position for home
-      			  if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] )) 
-      			  {
-                homeaxis(X_AXIS);
-                homeaxis(Y_AXIS);
-      			  } 
-              // 1st mesh bed leveling measurement point, corrected.
-              world2machine_initialize();
-              world2machine(pgm_read_float(bed_ref_points_4), pgm_read_float(bed_ref_points_4+1), destination[X_AXIS], destination[Y_AXIS]);
-              world2machine_reset();
-              if (destination[Y_AXIS] < Y_MIN_POS)
-                  destination[Y_AXIS] = Y_MIN_POS;
-              destination[Z_AXIS] = MESH_HOME_Z_SEARCH;    // Set destination away from bed
-              feedrate = homing_feedrate[Z_AXIS]/10;
-              current_position[Z_AXIS] = 0;
-              enable_endstops(false);
-#ifdef DEBUG_BUILD
-              SERIAL_ECHOLNPGM("plan_set_position()");
-              MYSERIAL.println(current_position[X_AXIS]);MYSERIAL.println(current_position[Y_AXIS]);
-              MYSERIAL.println(current_position[Z_AXIS]);MYSERIAL.println(current_position[E_AXIS]);
-#endif
-              plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-#ifdef DEBUG_BUILD
-              SERIAL_ECHOLNPGM("plan_buffer_line()");
-              MYSERIAL.println(destination[X_AXIS]);MYSERIAL.println(destination[Y_AXIS]);
-              MYSERIAL.println(destination[Z_AXIS]);MYSERIAL.println(destination[E_AXIS]);
-              MYSERIAL.println(feedrate);MYSERIAL.println(active_extruder);
-#endif
-              plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
-              st_synchronize();
-              current_position[X_AXIS] = destination[X_AXIS];
-              current_position[Y_AXIS] = destination[Y_AXIS];
-              enable_endstops(true);
-              endstops_hit_on_purpose();
-              homeaxis(Z_AXIS);
-            #else // MESH_BED_LEVELING
-              homeaxis(Z_AXIS);
-            #endif // MESH_BED_LEVELING
-          }
-        #else // defined(Z_SAFE_HOMING): Z Safe mode activated.
-          if(home_all_axes) {
-            destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - X_PROBE_OFFSET_FROM_EXTRUDER);
-            destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - Y_PROBE_OFFSET_FROM_EXTRUDER);
-            destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1);    // Set destination away from bed
-            feedrate = XY_TRAVEL_SPEED/60;
-            current_position[Z_AXIS] = 0;
+                        //finish moves
+                        st_synchronize();
+                        //disable extruder steppers so filament can be removed
+                        disable_e0();
+                        disable_e1();
+                        disable_e2();
+                        delay(100);
+                        
+                        //LCD_ALERTMESSAGEPGM(_T(MSG_FILAMENTCHANGE));
+                        uint8_t cnt=0;
+                        int counterBeep = 0;
+                        lcd_wait_interact();
+                        while(!lcd_clicked()){
+                          cnt++;
+                          manage_heater();
+                          manage_inactivity(true);
+                          //lcd_update();
+                          if(cnt==0)
+                          {
+                          #if BEEPER > 0
+                          
+                            if (counterBeep== 500){
+                              counterBeep = 0;
+                              
+                            }
+                          
+                            
+                            SET_OUTPUT(BEEPER);
+                            if (counterBeep== 0){
+                              WRITE(BEEPER,HIGH);
+                            }
+                            
+                            if (counterBeep== 20){
+                              WRITE(BEEPER,LOW);
+                            }
+                            
+                            
+                            
+                          
+                            counterBeep++;
+                          #else
+                      #if !defined(LCD_FEEDBACK_FREQUENCY_HZ) || !defined(LCD_FEEDBACK_FREQUENCY_DURATION_MS)
+                              lcd_buzz(1000/6,100);
+                      #else
+                        lcd_buzz(LCD_FEEDBACK_FREQUENCY_DURATION_MS,LCD_FEEDBACK_FREQUENCY_HZ);
+                      #endif
+                          #endif
+                          }
+                        }
+                        
+                        WRITE(BEEPER,LOW);
+                        
+                        target[E_AXIS]+= FILAMENTCHANGE_FIRSTFEED ;
+                        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 20, active_extruder); 
+                        
+                        
+                        target[E_AXIS]+= FILAMENTCHANGE_FINALFEED ;
+                        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 2, active_extruder); 
+                        
+                 
+                        
+                        
+                        
+                        lcd_change_fil_state = 0;
+                        lcd_loading_filament();
+                        while ((lcd_change_fil_state == 0)||(lcd_change_fil_state != 1)){
+                        
+                          lcd_change_fil_state = 0;
+                          lcd_alright();
+                          switch(lcd_change_fil_state){
+                          
+                             case 2:
+                                     target[E_AXIS]+= FILAMENTCHANGE_FIRSTFEED ;
+                                     plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 20, active_extruder); 
+                        
+                        
+                                     target[E_AXIS]+= FILAMENTCHANGE_FINALFEED ;
+                                     plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 2, active_extruder); 
+                                      
+                                     
+                                     lcd_loading_filament();
+                                     break;
+                             case 3:
+                                     target[E_AXIS]+= FILAMENTCHANGE_FINALFEED ;
+                                     plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 2, active_extruder); 
+                                     lcd_loading_color();
+                                     break;
+                                          
+                             default:
+                                     lcd_change_success();
+                                     break;
+                          }
+                          
+                        }
+                        
 
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-            plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
-            st_synchronize();
-            current_position[X_AXIS] = destination[X_AXIS];
-            current_position[Y_AXIS] = destination[Y_AXIS];
+                        
+                      target[E_AXIS]+= 5;
+                      plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 2, active_extruder);
+                        
+                      target[E_AXIS]+= FILAMENTCHANGE_FIRSTRETRACT;
+                      plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 400, active_extruder);
+                        
 
-            homeaxis(Z_AXIS);
-          }
-                                                // Let's see if X and Y are homed and probe is inside bed area.
-          if(home_z) {
-            if ( (axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]) \
-              && (current_position[X_AXIS]+X_PROBE_OFFSET_FROM_EXTRUDER >= X_MIN_POS) \
-              && (current_position[X_AXIS]+X_PROBE_OFFSET_FROM_EXTRUDER <= X_MAX_POS) \
-              && (current_position[Y_AXIS]+Y_PROBE_OFFSET_FROM_EXTRUDER >= Y_MIN_POS) \
-              && (current_position[Y_AXIS]+Y_PROBE_OFFSET_FROM_EXTRUDER <= Y_MAX_POS)) {
+                        //current_position[E_AXIS]=target[E_AXIS]; //the long retract of L is compensated by manual filament feeding
+                        //plan_set_e_position(current_position[E_AXIS]);
+                        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 70, active_extruder); //should do nothing
+                        plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], target[Z_AXIS], target[E_AXIS], 70, active_extruder); //move xy back
+                        plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], target[E_AXIS], 200, active_extruder); //move z back
+                        
+                        
+                        target[E_AXIS]= target[E_AXIS] - FILAMENTCHANGE_FIRSTRETRACT;
+                        
+                      
+                             
+                        plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], target[E_AXIS], 5, active_extruder); //final untretract
+                        
+                        
+                        plan_set_e_position(lastpos[E_AXIS]);
+                        
+                        feedmultiply=feedmultiplyBckp;
+                        
+                     
+                        
+                        char cmd[9];
 
-              current_position[Z_AXIS] = 0;
-              plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-              destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1);    // Set destination away from bed
-              feedrate = max_feedrate[Z_AXIS];
-              plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
-              st_synchronize();
+                        sprintf_P(cmd, PSTR("M220 S%i"), feedmultiplyBckp);
+                        enquecommand(cmd);
 
-              homeaxis(Z_AXIS);
-            } else if (!((axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]))) {
-                LCD_MESSAGERPGM(MSG_POSITION_UNKNOWN);
-                SERIAL_ECHO_START;
-                SERIAL_ECHOLNRPGM(MSG_POSITION_UNKNOWN);
-            } else {
-                LCD_MESSAGERPGM(MSG_ZPROBE_OUT);
-                SERIAL_ECHO_START;
-                SERIAL_ECHOLNRPGM(MSG_ZPROBE_OUT);
             }
-          }
-        #endif // Z_SAFE_HOMING
-      #endif // Z_HOME_DIR < 0
 
-      if(code_seen(axis_codes[Z_AXIS]) && code_value_long() != 0)
-        current_position[Z_AXIS]=code_value()+add_homing[Z_AXIS];
-      #ifdef ENABLE_AUTO_BED_LEVELING
-        if(home_z)
-          current_position[Z_AXIS] += zprobe_zoffset;  //Add Z_Probe offset (the distance is negative)
-      #endif
-      
-      // Set the planner and stepper routine positions.
-      // At this point the mesh bed leveling and world2machine corrections are disabled and current_position
-      // contains the machine coordinates.
-      plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 
-      #ifdef ENDSTOPS_ONLY_FOR_HOMING
-        enable_endstops(false);
-      #endif
 
-      feedrate = saved_feedrate;
-      feedmultiply = saved_feedmultiply;
-      previous_millis_cmd = millis();
-      endstops_hit_on_purpose();
-#ifndef MESH_BED_LEVELING
-      // If MESH_BED_LEVELING is not active, then it is the original Prusa i3.
-      // Offer the user to load the baby step value, which has been adjusted at the previous print session.
-      if(card.sdprinting && eeprom_read_word((uint16_t *)EEPROM_BABYSTEP_Z))
-          lcd_adjust_z();
-#endif
+        #endif
 
-    // Load the machine correction matrix
-    world2machine_initialize();
-    // and correct the current_position XY axes to match the transformed coordinate system.
-    world2machine_update_current();
 
-#if (defined(MESH_BED_LEVELING) && !defined(MK1BP))
-	if (code_seen(axis_codes[X_AXIS]) || code_seen(axis_codes[Y_AXIS]) || code_seen('W') || code_seen(axis_codes[Z_AXIS]))
-		{
-      if (! home_z && mbl_was_active) {
-        // Re-enable the mesh bed leveling if only the X and Y axes were re-homed.
-        mbl.active = true;
-        // and re-adjust the current logical Z axis with the bed leveling offset applicable at the current XY position.
-        current_position[Z_AXIS] -= mbl.get_z(st_get_position_mm(X_AXIS), st_get_position_mm(Y_AXIS));
-      }
+        get_coordinates(); // For X Y Z E F
+		if (total_filament_used > ((current_position[E_AXIS] - destination[E_AXIS]) * 100)) { //protection against total_filament_used overflow
+			total_filament_used = total_filament_used + ((destination[E_AXIS] - current_position[E_AXIS]) * 100);
 		}
-	else
-		{
-			st_synchronize();
-			homing_flag = false;
-			// Push the commands to the front of the message queue in the reverse order!
-			// There shall be always enough space reserved for these commands.
-			// enquecommand_front_P((PSTR("G80")));
-			goto case_G80;
-	  }
-#endif
+          #ifdef FWRETRACT
+            if(autoretract_enabled)
+            if( !(code_seen('X') || code_seen('Y') || code_seen('Z')) && code_seen('E')) {
+              float echange=destination[E_AXIS]-current_position[E_AXIS];
 
-	  if (farm_mode) { prusa_statistics(20); };
+              if((echange<-MIN_RETRACT && !retracted) || (echange>MIN_RETRACT && retracted)) { //move appears to be an attempt to retract or recover
+                  current_position[E_AXIS] = destination[E_AXIS]; //hide the slicer-generated retract/recover from calculations
+                  plan_set_e_position(current_position[E_AXIS]); //AND from the planner
+                  retract(!retracted);
+                  return;
+              }
 
-	  homing_flag = false;
-#if 0
-      SERIAL_ECHOPGM("G28, final ");  print_world_coordinates();
-      SERIAL_ECHOPGM("G28, final ");  print_physical_coordinates();
-      SERIAL_ECHOPGM("G28, final ");  print_mesh_bed_leveling_table();
-#endif
+
+            }
+          #endif //FWRETRACT
+        prepare_move();
+        //ClearToSend();
+      }
+      break;
+    case 2: // G2  - CW ARC
+      if(Stopped == false) {
+        get_arc_coordinates();
+        prepare_arc_move(true);
+      }
+      break;
+    case 3: // G3  - CCW ARC
+      if(Stopped == false) {
+        get_arc_coordinates();
+        prepare_arc_move(false);
+      }
+      break;
+    case 4: // G4 dwell      
+      codenum = 0;
+      if(code_seen('P')) codenum = code_value(); // milliseconds to wait
+      if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
+	  if(codenum != 0) LCD_MESSAGERPGM(_i("Sleep..."));////MSG_DWELL c=0 r=0
+      st_synchronize();
+      codenum += millis();  // keep track of when we started waiting
+      previous_millis_cmd = millis();
+      while(millis() < codenum) {
+        manage_heater();
+        manage_inactivity();
+        lcd_update();
+      }
+      break;
+      #ifdef FWRETRACT
+      case 10: // G10 retract
+       #if EXTRUDERS > 1
+        retracted_swap[active_extruder]=(code_seen('S') && code_value_long() == 1); // checks for swap retract argument
+        retract(true,retracted_swap[active_extruder]);
+       #else
+        retract(true);
+       #endif
+      break;
+      case 11: // G11 retract_recover
+       #if EXTRUDERS > 1
+        retract(false,retracted_swap[active_extruder]);
+       #else
+        retract(false);
+       #endif 
       break;
+      #endif //FWRETRACT
+    case 28: //G28 Home all Axis one at a time
+    {
+      // Which axes should be homed?
+      bool home_x = code_seen(axis_codes[X_AXIS]);
+      bool home_y = code_seen(axis_codes[Y_AXIS]);
+      bool home_z = code_seen(axis_codes[Z_AXIS]);
+      // calibrate?
+      bool calib = code_seen('C');
+
+	  gcode_G28(home_x, home_y, home_z, calib);
+	  
+	  break;
     }
 #ifdef ENABLE_AUTO_BED_LEVELING
     case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
@@ -3524,6 +3545,14 @@ void process_commands()
 #ifdef PINDA_THERMISTOR
 		if (true)
 		{
+
+			if (calibration_status() >= CALIBRATION_STATUS_XYZ_CALIBRATION) {
+				//we need to know accurate position of first calibration point
+				//if xyz calibration was not performed yet, interrupt temperature calibration and inform user that xyz cal. is needed
+				lcd_show_fullscreen_message_and_wait_P(_i("Please run XYZ calibration first."));
+				break;
+			}
+			
 			if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS]))
 			{
 				// We don't know where we are! HOME!
@@ -3538,20 +3567,19 @@ void process_commands()
 			
 			if (result)
 			{
+				current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
 				current_position[Z_AXIS] = 50;
-				current_position[Y_AXIS] += 180;
+				current_position[Y_AXIS] = 180;
 				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
 				st_synchronize();
 				lcd_show_fullscreen_message_and_wait_P(_T(MSG_REMOVE_STEEL_SHEET));
-				current_position[Y_AXIS] -= 180;
+				current_position[Y_AXIS] = pgm_read_float(bed_ref_points_4 + 1);
+				current_position[X_AXIS] = pgm_read_float(bed_ref_points_4);
 				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
 				st_synchronize();
-				feedrate = homing_feedrate[Z_AXIS] / 10;
-				enable_endstops(true);
-				endstops_hit_on_purpose();
-				homeaxis(Z_AXIS);
-				plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-				enable_endstops(false);
+				gcode_G28(false, false, true, false);
+
 			}
 			if ((current_temperature_pinda > 35) && (farm_mode == false)) {
 				//waiting for PIDNA probe to cool down in case that we are not in farm mode
@@ -3581,8 +3609,11 @@ void process_commands()
 			custom_message_type = 4;
 			custom_message_state = 1;
 			custom_message = _T(MSG_TEMP_CALIBRATION);
+			current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
 			current_position[X_AXIS] = PINDA_PREHEAT_X;
 			current_position[Y_AXIS] = PINDA_PREHEAT_Y;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
 			current_position[Z_AXIS] = PINDA_PREHEAT_Z;
 			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
 			st_synchronize();
@@ -3595,11 +3626,10 @@ void process_commands()
 
 			eeprom_update_byte((uint8_t*)EEPROM_CALIBRATION_STATUS_PINDA, 0); //invalidate temp. calibration in case that in will be aborted during the calibration process 
 
-			current_position[Z_AXIS] = 5;
+			current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
 			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
-
-			current_position[X_AXIS] = pgm_read_float(bed_ref_points);
-			current_position[Y_AXIS] = pgm_read_float(bed_ref_points + 1);
+			current_position[X_AXIS] = pgm_read_float(bed_ref_points_4);
+			current_position[Y_AXIS] = pgm_read_float(bed_ref_points_4 + 1);
 			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
 			st_synchronize();
 
@@ -3640,8 +3670,11 @@ void process_commands()
 				custom_message_state = i + 2;
 				setTargetBed(50 + 10 * (temp - 30) / 5);
 //				setTargetHotend(255, 0);
+				current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
 				current_position[X_AXIS] = PINDA_PREHEAT_X;
 				current_position[Y_AXIS] = PINDA_PREHEAT_Y;
+				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
 				current_position[Z_AXIS] = PINDA_PREHEAT_Z;
 				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
 				st_synchronize();
@@ -3650,10 +3683,10 @@ void process_commands()
 					delay_keep_alive(1000);
 					serialecho_temperatures();
 				}
-				current_position[Z_AXIS] = 5;
+				current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
 				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
-				current_position[X_AXIS] = pgm_read_float(bed_ref_points);
-				current_position[Y_AXIS] = pgm_read_float(bed_ref_points + 1);
+				current_position[X_AXIS] = pgm_read_float(bed_ref_points_4);
+				current_position[Y_AXIS] = pgm_read_float(bed_ref_points_4 + 1);
 				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
 				st_synchronize();
 				find_z_result = find_bed_induction_sensor_point_z(-1.f);
@@ -3858,15 +3891,6 @@ void process_commands()
 #endif //MK1BP
 	case_G80:
 	{
-#ifdef TMC2130
-		//previously enqueued "G28 W0" failed (crash Z)
-		if (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && !axis_known_position[Z_AXIS] && (READ(Z_TMC2130_DIAG) != 0))
-		{
-			kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
-			break;
-		}
-#endif //TMC2130
-
 		mesh_bed_leveling_flag = true;
 		int8_t verbosity_level = 0;
 		static bool run = false;

Firmware/mesh_bed_calibration.cpp

@@ -962,6 +962,10 @@ static inline void update_current_position_z()
 // At the current position, find the Z stop.
 inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter, int verbosity_level)
 {
+#ifdef TMC2130
+	FORCE_HIGH_POWER_START;
+#endif
+
 	#ifdef SUPPORT_VERBOSITY
     if(verbosity_level >= 10) SERIAL_ECHOLNPGM("find bed induction sensor point z");
 	#endif // SUPPORT_VERBOSITY
@@ -978,7 +982,7 @@ inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter, i
     if (! endstop_z_hit_on_purpose())
         goto error;
 #ifdef TMC2130
-	if ((tmc2130_mode == TMC2130_MODE_NORMAL) && (READ(Z_TMC2130_DIAG) != 0)) goto error; //crash Z detected
+	if (READ(Z_TMC2130_DIAG) != 0) goto error; //crash Z detected
 #endif //TMC2130
     for (uint8_t i = 0; i < n_iter; ++ i)
 	{
@@ -993,7 +997,7 @@ inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter, i
         if (! endstop_z_hit_on_purpose())
             goto error;
 #ifdef TMC2130
-		if ((tmc2130_mode == TMC2130_MODE_NORMAL) && (READ(Z_TMC2130_DIAG) != 0)) goto error; //crash Z detected
+		if (READ(Z_TMC2130_DIAG) != 0) goto error; //crash Z detected
 #endif //TMC2130
 //        SERIAL_ECHOPGM("Bed find_bed_induction_sensor_point_z low, height: ");
 //        MYSERIAL.print(current_position[Z_AXIS], 5);
@@ -1007,16 +1011,23 @@ inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter, i
     if (n_iter > 1)
         current_position[Z_AXIS] /= float(n_iter);
 
+
     enable_endstops(endstops_enabled);
     enable_z_endstop(endstop_z_enabled);
 //    SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 3");
-    return true;
+#ifdef TMC2130
+	FORCE_HIGH_POWER_END;
+#endif
+	return true;
 
 error:
 //    SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 4");
     enable_endstops(endstops_enabled);
     enable_z_endstop(endstop_z_enabled);
-    return false;
+#ifdef TMC2130
+	FORCE_HIGH_POWER_END;
+#endif
+	return false;
 }
 
 #ifdef NEW_XYZCAL

lang_backup/language_all.cpp

@@ -2226,10 +2226,8 @@ const char * const MSG_TEMP_CALIBRATION_ON_LANG_TABLE[LANG_NUM] PROGMEM = {
 };
 
 const char MSG_TEMP_CAL_FAILED_EN[] PROGMEM = "Temperature calibration failed";
-const char MSG_TEMP_CAL_FAILED_CZ[] PROGMEM = "Teplotni kalibrace selhala";
-const char * const MSG_TEMP_CAL_FAILED_LANG_TABLE[LANG_NUM] PROGMEM = {
-	MSG_TEMP_CAL_FAILED_EN,
-	MSG_TEMP_CAL_FAILED_CZ
+const char * const MSG_TEMP_CAL_FAILED_LANG_TABLE[1] PROGMEM = {
+	MSG_TEMP_CAL_FAILED_EN
 };
 
 const char MSG_TEMP_CAL_WARNING_EN[] PROGMEM = "Stable ambient temperature 21-26C is needed a rigid stand is required.";

lang_backup/language_all.h

@@ -728,8 +728,8 @@ extern const char* const MSG_TEMP_CALIBRATION_OFF_LANG_TABLE[LANG_NUM];
 #define MSG_TEMP_CALIBRATION_OFF LANG_TABLE_SELECT(MSG_TEMP_CALIBRATION_OFF_LANG_TABLE)
 extern const char* const MSG_TEMP_CALIBRATION_ON_LANG_TABLE[LANG_NUM];
 #define MSG_TEMP_CALIBRATION_ON LANG_TABLE_SELECT(MSG_TEMP_CALIBRATION_ON_LANG_TABLE)
-extern const char* const MSG_TEMP_CAL_FAILED_LANG_TABLE[LANG_NUM];
-#define MSG_TEMP_CAL_FAILED LANG_TABLE_SELECT(MSG_TEMP_CAL_FAILED_LANG_TABLE)
+extern const char* const MSG_TEMP_CAL_FAILED_LANG_TABLE[1];
+#define MSG_TEMP_CAL_FAILED LANG_TABLE_SELECT_EXPLICIT(MSG_TEMP_CAL_FAILED_LANG_TABLE, 0)
 extern const char* const MSG_TEMP_CAL_WARNING_LANG_TABLE[1];
 #define MSG_TEMP_CAL_WARNING LANG_TABLE_SELECT_EXPLICIT(MSG_TEMP_CAL_WARNING_LANG_TABLE, 0)
 extern const char* const MSG_TOSHIBA_FLASH_AIR_COMPATIBILITY_OFF_LANG_TABLE[1];

lang_backup/language_cz.h

@@ -415,4 +415,3 @@
 #define MSG_CHANGED_PRINTER			 "Varovani: doslo ke zmene typu tiskarny."
 #define MSG_CHANGED_BOTH			 "Varovani: doslo ke zmene typu tiskarny a motherboardu."
 #define MSG_WAITING_TEMP_PINDA		 "Cekani na zchladnuti PINDA"
-#define MSG_TEMP_CAL_FAILED			 "Teplotni kalibrace selhala"

lang_backup/language_en.h

@@ -424,4 +424,4 @@
 #define(length=20, lines=4) MSG_CHANGED_PRINTER			 "Warning: printer type changed."
 #define(length=20, lines=4) MSG_CHANGED_BOTH			 "Warning: both printer type and motherboard type changed."
 #define(length=20, lines=3) MSG_WAITING_TEMP_PINDA		 "Waiting for PINDA probe cooling"
-#define(length=20, lines=8) MSG_TEMP_CAL_FAILED			 "Temperature calibration failed"
+#define(length=20, lines=8) MSG_TEMP_CAL_FAILED			 "Temperature calibration failed"