Bug Fix: Switching extruder tool change leaves Z in wrong position

The tool_change function saves the current_position to the destination
array soon after starting.  Later in the switching extruder section, the
destination array is modified when moving the Z axis up & down.  A later
section of tool_change moves the head back to the “original location”
using the destination array.  This later section assumes that the
destination array hasn’t been modified.

The fix is to save the destination Z position and then restore it after
the Z movements have completed.

Going back to using the current_position array for the switching
extruder Z axis moves (and  leaving the destination array untouched)
doesn’t fix the problem.

This bug was introduced by the “Make tool_change kinematic compatible”
commit # 847429eff4 which was merged on 10
Oct 2016 as part of PR 4982.

This bug was discovered in Issue 5966.

Marlin/Marlin_main.cpp

       if (nozzle_timed_out)
         lcd_filament_change_show_message(FILAMENT_CHANGE_MESSAGE_CLICK_TO_HEAT_NOZZLE);
 
-      #if HAS_BUZZER 
+      #if HAS_BUZZER
         filament_change_beep();
       #endif
 
     stepper.synchronize();
 
     #if defined(FILAMENT_CHANGE_EXTRUDE_LENGTH) && FILAMENT_CHANGE_EXTRUDE_LENGTH > 0
-  
+
       do {
         // "Wait for filament extrude"
         lcd_filament_change_show_message(FILAMENT_CHANGE_MESSAGE_EXTRUDE);
                   z_raise = 0.3 + (z_diff > 0.0 ? z_diff : 0.0);
 
             // Always raise by some amount (destination copied from current_position earlier)
+            float save_Z = destination[Z_AXIS];  // save Z for later on
             destination[Z_AXIS] += z_raise;
             planner.buffer_line_kinematic(destination, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
             stepper.synchronize();
               planner.buffer_line_kinematic(destination, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
               stepper.synchronize();
             }
+            destination[Z_AXIS] = save_Z;  // restore original Z position so the 'Move to the "old position"' is correct
           #endif
 
           /**
   #else
     #define M600_TEST true
   #endif
-             
+
   if (M600_TEST && stepper_inactive_time && ELAPSED(ms, previous_cmd_ms + stepper_inactive_time)
       && !ignore_stepper_queue && !planner.blocks_queued()) {
     #if ENABLED(DISABLE_INACTIVE_X)