Implement G60 / G61 to meet spec

Marlin/src/core/language.h

@@ -162,8 +162,8 @@
 #define STR_SOFT_MIN                        "  Min: "
 #define STR_SOFT_MAX                        "  Max: "
 
-#define STR_SAVED_POS                       "Position saved"
-#define STR_RESTORING_POS                   "Restoring position"
+#define STR_SAVED_POSITION                  "Saved position #"
+#define STR_RESTORING_POSITION              "Restoring position #"
 #define STR_INVALID_POS_SLOT                "Invalid slot. Total: "
 #define STR_DONE                            "Done."
 

Marlin/src/gcode/feature/pause/G60.cpp

@@ -30,41 +30,78 @@
 #define DEBUG_OUT ENABLED(SAVED_POSITIONS_DEBUG)
 #include "../../../core/debug_out.h"
 
+bool report_stored_position(const uint8_t slot) {
+  if (!did_save_position[slot]) return false;
+  const xyze_pos_t &pos = stored_position[slot];
+  SERIAL_ECHO(STR_SAVED_POSITION, slot, C(':'));
+  #if NUM_AXES
+    SERIAL_ECHOPGM_P(
+      LIST_N(DOUBLE(NUM_AXES),
+        SP_X_LBL, pos.x, SP_Y_LBL, pos.y, SP_Z_LBL, pos.z,
+        SP_I_LBL, pos.i, SP_J_LBL, pos.j, SP_K_LBL, pos.k,
+        SP_U_LBL, pos.u, SP_V_LBL, pos.v, SP_W_LBL, pos.w
+      )
+    );
+  #endif
+  #if HAS_EXTRUDERS
+    SERIAL_ECHOPGM_P(SP_E_LBL, pos.e);
+  #endif
+  SERIAL_EOL();
+  return true;
+}
+
 /**
- * G60: Save current position
+ * G60: Saved Positions
  *
- *   S<slot> - Memory slot # (0-based) to save into (default 0)
+ *   S<slot> - Save to a memory slot. (default 0)
+ *   Q<slot> - Restore from a memory slot. (default 0)
+ *   D<slot> - Delete a memory slot. With no number, delete all.
  */
 void GcodeSuite::G60() {
-  const uint8_t slot = parser.byteval('S');
+  // With no parameters report any saved positions
+  if (!parser.seen_any()) {
+    uint8_t count = 0;
+    for (uint8_t s = 0; s < SAVED_POSITIONS; ++s)
+      if (report_stored_position(s)) ++count;
+    if (!count) SERIAL_ECHOLNPGM("No Saved Positions");
+    return;
+  }
 
-  if (slot >= SAVED_POSITIONS) {
-    SERIAL_ERROR_MSG(STR_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS));
+  // Only one of these parameters is permitted
+  const uint8_t seenD = parser.seen_test('D'),
+                seenQ = parser.seen_test('Q'),
+                seenS = parser.seen_test('S');
+  if (seenD + seenQ + seenS > 1) return;
+
+  // G60 D : Delete all saved positions
+  if (seenD && !parser.seenval('D')) {
+    did_save_position.reset();
     return;
   }
 
-  stored_position[slot] = current_position;
-  SBI(saved_slots[slot >> 3], slot & 0x07);
+  // G60 Dn / Q / S : Get the slot value
+  const uint8_t slot = parser.byteval(seenD ? 'D' : seenQ ? 'Q' : 'S');
 
-  #if ENABLED(SAVED_POSITIONS_DEBUG)
-  {
-    const xyze_pos_t &pos = stored_position[slot];
-    DEBUG_ECHOPGM(STR_SAVED_POS " S", slot, " :");
-    #if NUM_AXES
-      DEBUG_ECHOPGM_P(
-        LIST_N(DOUBLE(NUM_AXES),
-          SP_X_LBL, pos.x, SP_Y_LBL, pos.y, SP_Z_LBL, pos.z,
-          SP_I_LBL, pos.i, SP_J_LBL, pos.j, SP_K_LBL, pos.k,
-          SP_U_LBL, pos.u, SP_V_LBL, pos.v, SP_W_LBL, pos.w
-        )
-      );
-    #endif
-    #if HAS_EXTRUDERS
-      DEBUG_ECHOPGM_P(SP_E_LBL, pos.e);
-    #endif
-    DEBUG_EOL();
+  // G60 Q : Redirect to G61(slot)
+  if (seenQ) return G61(slot);
+
+  // Valid slot number?
+  if (SAVED_POSITIONS < 256 && slot >= SAVED_POSITIONS) {
+    SERIAL_ERROR_MSG(STR_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS));
+    return;
   }
-  #endif
+
+  // G60 Dn
+  if (seenD) {
+    SERIAL_ECHOLNPGM(STR_SAVED_POSITION, slot, ": DELETED");
+    did_save_position.clear(slot);
+    return;
+  }
+
+  // G60 S
+  stored_position[slot] = current_position;
+  did_save_position.set(slot);
+  report_stored_position(slot);
 }
 
 #endif // SAVED_POSITIONS

Marlin/src/gcode/feature/pause/G61.cpp

@@ -54,52 +54,64 @@
  *
  *   If no axes are specified then all axes are restored.
  */
-void GcodeSuite::G61() {
+void GcodeSuite::G61(int8_t slot/*=-1*/) {
 
-  const uint8_t slot = parser.byteval('S');
+  if (slot < 0) slot = parser.byteval('S');
 
-  #define SYNC_E(POINT) TERN_(HAS_EXTRUDERS, planner.set_e_position_mm((destination.e = current_position.e = (POINT))))
+  #define SYNC_E(E) planner.set_e_position_mm(current_position.e = (E))
 
-  #if SAVED_POSITIONS < 256
-    if (slot >= SAVED_POSITIONS) {
-      SERIAL_ERROR_MSG(STR_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS));
-      return;
-    }
-  #endif
+  if (SAVED_POSITIONS < 256 && slot >= SAVED_POSITIONS) {
+    SERIAL_ERROR_MSG(STR_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS));
+    return;
+  }
 
   // No saved position? No axes being restored?
-  if (!TEST(saved_slots[slot >> 3], slot & 0x07)) return;
+  if (!did_save_position[slot]) return;
 
   // Apply any given feedrate over 0.0
   REMEMBER(saved, feedrate_mm_s);
   const float fr = parser.linearval('F');
   if (fr > 0.0) feedrate_mm_s = MMM_TO_MMS(fr);
 
+  // No XYZ...E parameters, move to stored position
+
+  float epos = stored_position[slot].e;
   if (!parser.seen_axis()) {
-    DEBUG_ECHOLNPGM("Default position restore");
+    DEBUG_ECHOLNPGM(STR_RESTORING_POSITION, slot, " (all axes)");
+    // Move to the saved position, all axes except E
     do_blocking_move_to(stored_position[slot], feedrate_mm_s);
-    SYNC_E(stored_position[slot].e);
+    // Just set E to the saved position without moving it
+    TERN_(HAS_EXTRUDERS, SYNC_E(stored_position[slot].e));
+    report_current_position();
+    return;
   }
-  else {
-    if (parser.seen(STR_AXES_MAIN)) {
-      DEBUG_ECHOPGM(STR_RESTORING_POS " S", slot);
-      LOOP_NUM_AXES(i) {
-        destination[i] = parser.seen(AXIS_CHAR(i))
-          ? stored_position[slot][i] + parser.value_axis_units((AxisEnum)i)
-          : current_position[i];
+
+  // With XYZ...E return specified axes + optional offset
+
+  DEBUG_ECHOPGM(STR_RESTORING_POSITION " S", slot);
+
+  if (parser.seen(STR_AXES_MAIN)) {
+    destination = current_position;
+    LOOP_NUM_AXES(i) {
+      if (parser.seen(AXIS_CHAR(i))) {
+        destination[i] = stored_position[slot][i] + parser.value_axis_units((AxisEnum)i);
         DEBUG_ECHO(C(' '), C(AXIS_CHAR(i)), p_float_t(destination[i], 2));
       }
-      DEBUG_EOL();
-      // Move to the saved position
-      prepare_line_to_destination();
     }
-    #if HAS_EXTRUDERS
-      if (parser.seen_test('E')) {
-        DEBUG_ECHOLNPGM(STR_RESTORING_POS " S", slot, " E", current_position.e, "=>", stored_position[slot].e);
-        SYNC_E(stored_position[slot].e);
-      }
-    #endif
+    prepare_line_to_destination();
   }
+
+  #if HAS_EXTRUDERS
+    if (parser.seen('E')) {
+      epos += parser.value_axis_units(E_AXIS);
+      DEBUG_ECHOPGM(" E", epos);
+      SYNC_E(epos);
+    }
+  #endif
+
+  DEBUG_EOL();
+
+  report_current_position();
 }
 
 #endif // SAVED_POSITIONS

Marlin/src/gcode/gcode.h

@@ -606,7 +606,7 @@ class GcodeSuite {
 
   #if SAVED_POSITIONS
     static void G60();
-    static void G61();
+    static void G61(int8_t slot=-1);
   #endif
 
   #if ENABLED(GCODE_MOTION_MODES)

Marlin/src/module/motion.cpp

@@ -104,7 +104,7 @@ xyze_pos_t destination; // {0}
 
 // G60/G61 Position Save and Return
 #if SAVED_POSITIONS
-  uint8_t saved_slots[(SAVED_POSITIONS + 7) >> 3];
+  Flags<SAVED_POSITIONS> did_save_position;
   xyze_pos_t stored_position[SAVED_POSITIONS];
 #endif
 

Marlin/src/module/motion.h

@@ -50,7 +50,7 @@ extern xyze_pos_t current_position,  // High-level current tool position
 
 // G60/G61 Position Save and Return
 #if SAVED_POSITIONS
-  extern uint8_t saved_slots[(SAVED_POSITIONS + 7) >> 3]; // TODO: Add support for HAS_I_AXIS
+  extern Flags<SAVED_POSITIONS> did_save_position;
   extern xyze_pos_t stored_position[SAVED_POSITIONS];
 #endif