Fix go up

Firmware/Marlin_main.cpp

@@ -2316,14 +2316,16 @@ bool homeaxis(int axis, bool doError, uint8_t cnt)
 			FORCE_HIGH_POWER_END;
 			if (doError)
 			{
+				current_position[Z_AXIS] = 0;
+				plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 				current_position[Z_AXIS] += MESH_HOME_Z_SEARCH;
 				plan_buffer_line_curposXYZE(max_feedrate[Z_AXIS], active_extruder);
 				st_synchronize();
 				kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
 			}
-			current_position[axis] = -MESH_HOME_Z_SEARCH; //assume that nozzle crashed into bed
+			current_position[Z_AXIS] = -MESH_HOME_Z_SEARCH; //assume that nozzle crashed into bed
 			plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-			return 0; 
+			return 0;
 		}
 #endif //TMC2130
         current_position[axis] = 0;
@@ -2340,14 +2342,16 @@ bool homeaxis(int axis, bool doError, uint8_t cnt)
 			FORCE_HIGH_POWER_END;
 			if (doError)
 			{
+				current_position[Z_AXIS] = 0;
+				plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 				current_position[Z_AXIS] += MESH_HOME_Z_SEARCH;
 				plan_buffer_line_curposXYZE(max_feedrate[Z_AXIS], active_extruder);
 				st_synchronize();
 				kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
 			}
-			current_position[axis] = -MESH_HOME_Z_SEARCH; //assume that nozzle crashed into bed
+			current_position[Z_AXIS] = -MESH_HOME_Z_SEARCH; //assume that nozzle crashed into bed
 			plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-			return 0; 
+			return 0;
 		}
 #endif //TMC2130
         axis_is_at_home(axis);