Bläddra i källkod

Asynchronous M114 and (R)ealtime position option (#17032)

Scott Lahteine 4 år sedan
förälder
incheckning
3a07b4412d
Inget konto är kopplat till bidragsgivarens mejladress

+ 4
- 2
Marlin/Configuration_adv.h Visa fil

@@ -276,8 +276,10 @@
276 276
   #define AUTOTEMP_OLDWEIGHT 0.98
277 277
 #endif
278 278
 
279
-// Show extra position information with 'M114 D'
280
-//#define M114_DETAIL
279
+// Extra options for the M114 "Current Position" report
280
+//#define M114_DETAIL         // Use 'M114` for details to check planner calculations
281
+//#define M114_REALTIME       // Real current position based on forward kinematics
282
+//#define M114_LEGACY         // M114 used to synchronize on every call. Enable if needed.
281 283
 
282 284
 // Show Temperature ADC value
283 285
 // Enable for M105 to include ADC values read from temperature sensors.

+ 25
- 1
Marlin/src/core/types.h Visa fil

@@ -187,6 +187,12 @@ struct XYval {
187 187
   };
188 188
   FI void set(const T px)                               { x = px; }
189 189
   FI void set(const T px, const T py)                   { x = px; y = py; }
190
+  FI void set(const T (&arr)[XY])                       { x = arr[0]; y = arr[1]; }
191
+  FI void set(const T (&arr)[XYZ])                      { x = arr[0]; y = arr[1]; }
192
+  FI void set(const T (&arr)[XYZE])                     { x = arr[0]; y = arr[1]; }
193
+  #if XYZE_N > XYZE
194
+    FI void set(const T (&arr)[XYZE_N])                 { x = arr[0]; y = arr[1]; }
195
+  #endif
190 196
   FI void reset()                                       { x = y = 0; }
191 197
   FI T magnitude()                                const { return (T)sqrtf(x*x + y*y); }
192 198
   FI operator T* ()                                     { return pos; }
@@ -197,6 +203,8 @@ struct XYval {
197 203
   FI XYval<int16_t>    asInt()                    const { return { int16_t(x), int16_t(y) }; }
198 204
   FI XYval<int32_t>   asLong()                          { return { int32_t(x), int32_t(y) }; }
199 205
   FI XYval<int32_t>   asLong()                    const { return { int32_t(x), int32_t(y) }; }
206
+  FI XYval<int32_t>   ROUNDL()                          { return { int32_t(LROUND(x)), int32_t(LROUND(y)) }; }
207
+  FI XYval<int32_t>   ROUNDL()                    const { return { int32_t(LROUND(x)), int32_t(LROUND(y)) }; }
200 208
   FI XYval<float>    asFloat()                          { return {   float(x),   float(y) }; }
201 209
   FI XYval<float>    asFloat()                    const { return {   float(x),   float(y) }; }
202 210
   FI XYval<float> reciprocal()                    const { return {  _RECIP(x),  _RECIP(y) }; }
@@ -290,6 +298,12 @@ struct XYZval {
290 298
   FI void set(const T px, const T py)                  { x = px; y = py; }
291 299
   FI void set(const T px, const T py, const T pz)      { x = px; y = py; z = pz; }
292 300
   FI void set(const XYval<T> pxy, const T pz)          { x = pxy.x; y = pxy.y; z = pz; }
301
+  FI void set(const T (&arr)[XY])                      { x = arr[0]; y = arr[1]; }
302
+  FI void set(const T (&arr)[XYZ])                     { x = arr[0]; y = arr[1]; z = arr[2]; }
303
+  FI void set(const T (&arr)[XYZE])                    { x = arr[0]; y = arr[1]; z = arr[2]; }
304
+  #if XYZE_N > XYZE
305
+    FI void set(const T (&arr)[XYZE_N])                { x = arr[0]; y = arr[1]; z = arr[2]; }
306
+  #endif
293 307
   FI void reset()                                      { x = y = z = 0; }
294 308
   FI T magnitude()                               const { return (T)sqrtf(x*x + y*y + z*z); }
295 309
   FI operator T* ()                                    { return pos; }
@@ -300,6 +314,8 @@ struct XYZval {
300 314
   FI XYZval<int16_t>   asInt()                   const { return { int16_t(x), int16_t(y), int16_t(z) }; }
301 315
   FI XYZval<int32_t>  asLong()                         { return { int32_t(x), int32_t(y), int32_t(z) }; }
302 316
   FI XYZval<int32_t>  asLong()                   const { return { int32_t(x), int32_t(y), int32_t(z) }; }
317
+  FI XYZval<int32_t>  ROUNDL()                         { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)) }; }
318
+  FI XYZval<int32_t>  ROUNDL()                   const { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)) }; }
303 319
   FI XYZval<float>   asFloat()                         { return {   float(x),   float(y),   float(z) }; }
304 320
   FI XYZval<float>   asFloat()                   const { return {   float(x),   float(y),   float(z) }; }
305 321
   FI XYZval<float> reciprocal()                  const { return {  _RECIP(x),  _RECIP(y),  _RECIP(z) }; }
@@ -397,12 +413,20 @@ struct XYZEval {
397 413
   FI void set(const XYval<T> pxy, const T pz, const T pe)     { x = pxy.x;  y = pxy.y;  z = pz;     e = pe;    }
398 414
   FI void set(const XYval<T> pxy, const XYval<T> pze)         { x = pxy.x;  y = pxy.y;  z = pze.z;  e = pze.e; }
399 415
   FI void set(const XYZval<T> pxyz, const T pe)               { x = pxyz.x; y = pxyz.y; z = pxyz.z; e = pe;    }
416
+  FI void set(const T (&arr)[XY])                             { x = arr[0]; y = arr[1]; }
417
+  FI void set(const T (&arr)[XYZ])                            { x = arr[0]; y = arr[1]; z = arr[2]; }
418
+  FI void set(const T (&arr)[XYZE])                           { x = arr[0]; y = arr[1]; z = arr[2]; e = arr[3]; }
419
+  #if XYZE_N > XYZE
420
+    FI void set(const T (&arr)[XYZE_N])                       { x = arr[0]; y = arr[1]; z = arr[2]; e = arr[3]; }
421
+  #endif
400 422
   FI XYZEval<T>          copy()                         const { return *this; }
401 423
   FI XYZEval<T>           ABS()                         const { return { T(_ABS(x)), T(_ABS(y)), T(_ABS(z)), T(_ABS(e)) }; }
402 424
   FI XYZEval<int16_t>   asInt()                               { return { int16_t(x), int16_t(y), int16_t(z), int16_t(e) }; }
403 425
   FI XYZEval<int16_t>   asInt()                         const { return { int16_t(x), int16_t(y), int16_t(z), int16_t(e) }; }
404
-  FI XYZEval<int32_t>  asLong()                         const { return { int32_t(x), int32_t(y), int32_t(z), int32_t(e) }; }
405 426
   FI XYZEval<int32_t>  asLong()                               { return { int32_t(x), int32_t(y), int32_t(z), int32_t(e) }; }
427
+  FI XYZEval<int32_t>  asLong()                         const { return { int32_t(x), int32_t(y), int32_t(z), int32_t(e) }; }
428
+  FI XYZEval<int32_t>  ROUNDL()                               { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)), int32_t(LROUND(e)) }; }
429
+  FI XYZEval<int32_t>  ROUNDL()                         const { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)), int32_t(LROUND(e)) }; }
406 430
   FI XYZEval<float>   asFloat()                               { return {   float(x),   float(y),   float(z),   float(e) }; }
407 431
   FI XYZEval<float>   asFloat()                         const { return {   float(x),   float(y),   float(z),   float(e) }; }
408 432
   FI XYZEval<float> reciprocal()                        const { return {  _RECIP(x),  _RECIP(y),  _RECIP(z),  _RECIP(e) }; }

+ 25
- 5
Marlin/src/gcode/host/M114.cpp Visa fil

@@ -34,7 +34,7 @@
34 34
     #include "../../core/debug_out.h"
35 35
   #endif
36 36
 
37
-  void report_xyze(const xyze_pos_t &pos, const uint8_t n=4, const uint8_t precision=3) {
37
+  void report_xyze(const xyze_pos_t &pos, const uint8_t n=XYZE, const uint8_t precision=3) {
38 38
     char str[12];
39 39
     for (uint8_t a = 0; a < n; a++) {
40 40
       SERIAL_CHAR(' ', axis_codes[a], ':');
@@ -42,6 +42,7 @@
42 42
     }
43 43
     SERIAL_EOL();
44 44
   }
45
+  inline void report_xyz(const xyze_pos_t &pos) { report_xyze(pos, 3); }
45 46
 
46 47
   void report_xyz(const xyz_pos_t &pos, const uint8_t precision=3) {
47 48
     char str[12];
@@ -51,23 +52,26 @@
51 52
     }
52 53
     SERIAL_EOL();
53 54
   }
54
-  inline void report_xyz(const xyze_pos_t &pos) { report_xyze(pos, 3); }
55 55
 
56 56
   void report_current_position_detail() {
57 57
 
58
+    // Position as sent by G-code
58 59
     SERIAL_ECHOPGM("\nLogical:");
59 60
     report_xyz(current_position.asLogical());
60 61
 
62
+    // Cartesian position in native machine space
61 63
     SERIAL_ECHOPGM("Raw:    ");
62 64
     report_xyz(current_position);
63 65
 
64 66
     xyze_pos_t leveled = current_position;
65 67
 
66 68
     #if HAS_LEVELING
69
+      // Current position with leveling applied
67 70
       SERIAL_ECHOPGM("Leveled:");
68 71
       planner.apply_leveling(leveled);
69 72
       report_xyz(leveled);
70 73
 
74
+      // Test planner un-leveling. This should match the Raw result.
71 75
       SERIAL_ECHOPGM("UnLevel:");
72 76
       xyze_pos_t unleveled = leveled;
73 77
       planner.unapply_leveling(unleveled);
@@ -75,6 +79,7 @@
75 79
     #endif
76 80
 
77 81
     #if IS_KINEMATIC
82
+      // Kinematics applied to the leveled position
78 83
       #if IS_SCARA
79 84
         SERIAL_ECHOPGM("ScaraK: ");
80 85
       #else
@@ -180,12 +185,21 @@
180 185
 #endif // M114_DETAIL
181 186
 
182 187
 /**
183
- * M114: Report current position to host
188
+ * M114: Report the current position to host.
189
+ *       Since steppers are moving, the count positions are
190
+ *       projected by using planner calculations.
191
+ *   D - Report more detail. This syncs the planner. (Requires M114_DETAIL)
192
+ *   E - Report E stepper position (Requires M114_DETAIL)
193
+ *   R - Report the realtime position instead of projected.
184 194
  */
185 195
 void GcodeSuite::M114() {
186 196
 
187 197
   #if ENABLED(M114_DETAIL)
188 198
     if (parser.seen('D')) {
199
+      #if DISABLED(M114_LEGACY)
200
+        planner.synchronize();
201
+      #endif
202
+      report_current_position();
189 203
       report_current_position_detail();
190 204
       return;
191 205
     }
@@ -195,6 +209,12 @@ void GcodeSuite::M114() {
195 209
     }
196 210
   #endif
197 211
 
198
-  planner.synchronize();
199
-  report_current_position();
212
+  #if ENABLED(M114_REALTIME)
213
+    if (parser.seen('R')) { report_real_position(); return; }
214
+  #endif
215
+
216
+  #if ENABLED(M114_LEGACY)
217
+    planner.synchronize();
218
+  #endif
219
+  report_current_position_projected();
200 220
 }

+ 44
- 12
Marlin/src/module/motion.cpp Visa fil

@@ -206,17 +206,53 @@ xyz_pos_t cartes;
206 206
 /**
207 207
  * Output the current position to serial
208 208
  */
209
-void report_current_position() {
210
-  const xyz_pos_t lpos = current_position.asLogical();
211
-  SERIAL_ECHOPAIR("X:", lpos.x, " Y:", lpos.y, " Z:", lpos.z, " E:", current_position.e);
212 209
 
210
+inline void report_more_positions() {
213 211
   stepper.report_positions();
214
-
215 212
   #if IS_SCARA
216 213
     scara_report_positions();
217 214
   #endif
218 215
 }
219 216
 
217
+// Report the logical position for a given machine position
218
+inline void report_logical_position(const xyze_pos_t &rpos) {
219
+  const xyze_pos_t lpos = rpos.asLogical();
220
+  SERIAL_ECHOPAIR_P(X_LBL, lpos.x, SP_Y_LBL, lpos.y, SP_Z_LBL, lpos.z, SP_E_LBL, lpos.e);
221
+  report_more_positions();
222
+}
223
+
224
+// Report the real current position according to the steppers.
225
+// Forward kinematics and un-leveling are applied.
226
+void report_real_position() {
227
+  get_cartesian_from_steppers();
228
+  xyze_pos_t npos = cartes;
229
+  npos.e = planner.get_axis_position_mm(E_AXIS);
230
+
231
+  #if HAS_POSITION_MODIFIERS
232
+    planner.unapply_modifiers(npos
233
+      #if HAS_LEVELING
234
+        , true
235
+      #endif
236
+    );
237
+  #endif
238
+
239
+  report_logical_position(npos);
240
+}
241
+
242
+// Report the logical current position according to the most recent G-code command
243
+void report_current_position() { report_logical_position(current_position); }
244
+
245
+/**
246
+ * Report the logical current position according to the most recent G-code command.
247
+ * The planner.position always corresponds to the last G-code too. This makes M114
248
+ * suitable for debugging kinematics and leveling while avoiding planner sync that
249
+ * definitively interrupts the printing flow.
250
+ */
251
+void report_current_position_projected() {
252
+  report_logical_position(current_position);
253
+  stepper.report_a_position(planner.position);
254
+}
255
+
220 256
 /**
221 257
  * sync_plan_position
222 258
  *
@@ -241,11 +277,7 @@ void sync_plan_position_e() { planner.set_e_position_mm(current_position.e); }
241 277
  */
242 278
 void get_cartesian_from_steppers() {
243 279
   #if ENABLED(DELTA)
244
-    forward_kinematics_DELTA(
245
-      planner.get_axis_position_mm(A_AXIS),
246
-      planner.get_axis_position_mm(B_AXIS),
247
-      planner.get_axis_position_mm(C_AXIS)
248
-    );
280
+    forward_kinematics_DELTA(planner.get_axis_positions_mm());
249 281
   #else
250 282
     #if IS_SCARA
251 283
       forward_kinematics_SCARA(
@@ -663,11 +695,11 @@ void restore_feedrate_and_scaling() {
663 695
 
664 696
 FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
665 697
   const millis_t ms = millis();
666
-  thermalManager.manage_heater();  // This returns immediately if not really needed.
667 698
   if (ELAPSED(ms, next_idle_ms)) {
668 699
     next_idle_ms = ms + 200UL;
669
-    idle();
700
+    return idle();
670 701
   }
702
+  thermalManager.manage_heater();  // Returns immediately on most calls
671 703
 }
672 704
 
673 705
 #if IS_KINEMATIC
@@ -1324,7 +1356,7 @@ void do_homing_move(const AxisEnum axis, const float distance, const feedRate_t
1324 1356
     current_position[axis] = distance;
1325 1357
     line_to_current_position(real_fr_mm_s);
1326 1358
   #else
1327
-    abce_pos_t target = { planner.get_axis_position_mm(A_AXIS), planner.get_axis_position_mm(B_AXIS), planner.get_axis_position_mm(C_AXIS), planner.get_axis_position_mm(E_AXIS) };
1359
+    abce_pos_t target = planner.get_axis_positions_mm();
1328 1360
     target[axis] = 0;
1329 1361
     planner.set_machine_position_mm(target);
1330 1362
     target[axis] = distance;

+ 2
- 0
Marlin/src/module/motion.h Visa fil

@@ -162,7 +162,9 @@ typedef struct { xyz_pos_t min, max; } axis_limits_t;
162 162
   #define update_software_endstops(...) NOOP
163 163
 #endif
164 164
 
165
+void report_real_position();
165 166
 void report_current_position();
167
+void report_current_position_projected();
166 168
 
167 169
 void get_cartesian_from_steppers();
168 170
 void set_current_from_steppers_for_axis(const AxisEnum axis);

+ 16
- 6
Marlin/src/module/planner.h Visa fil

@@ -289,6 +289,12 @@ class Planner {
289 289
       static float extruder_advance_K[EXTRUDERS];
290 290
     #endif
291 291
 
292
+    /**
293
+     * The current position of the tool in absolute steps
294
+     * Recalculated if any axis_steps_per_mm are changed by gcode
295
+     */
296
+    static xyze_long_t position;
297
+
292 298
     #if HAS_POSITION_FLOAT
293 299
       static xyze_pos_t position_float;
294 300
     #endif
@@ -306,12 +312,6 @@ class Planner {
306 312
   private:
307 313
 
308 314
     /**
309
-     * The current position of the tool in absolute steps
310
-     * Recalculated if any axis_steps_per_mm are changed by gcode
311
-     */
312
-    static xyze_long_t position;
313
-
314
-    /**
315 315
      * Speed of previous path line segment
316 316
      */
317 317
     static xyze_float_t previous_speed;
@@ -725,6 +725,16 @@ class Planner {
725 725
      */
726 726
     static float get_axis_position_mm(const AxisEnum axis);
727 727
 
728
+    static inline abce_pos_t get_axis_positions_mm() {
729
+      const abce_pos_t out = {
730
+        get_axis_position_mm(A_AXIS),
731
+        get_axis_position_mm(B_AXIS),
732
+        get_axis_position_mm(C_AXIS),
733
+        get_axis_position_mm(E_AXIS)
734
+      };
735
+      return out;
736
+    }
737
+
728 738
     // SCARA AB axes are in degrees, not mm
729 739
     #if IS_SCARA
730 740
       FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }

+ 14
- 10
Marlin/src/module/stepper.cpp Visa fil

@@ -2448,6 +2448,19 @@ int32_t Stepper::triggered_position(const AxisEnum axis) {
2448 2448
   return v;
2449 2449
 }
2450 2450
 
2451
+void Stepper::report_a_position(const xyz_long_t &pos) {
2452
+  #if CORE_IS_XY || CORE_IS_XZ || ENABLED(DELTA) || IS_SCARA
2453
+    SERIAL_ECHOPAIR(STR_COUNT_A, pos.x, " B:", pos.y);
2454
+  #else
2455
+    SERIAL_ECHOPAIR_P(PSTR(STR_COUNT_X), pos.x, SP_Y_LBL, pos.y);
2456
+  #endif
2457
+  #if CORE_IS_XZ || CORE_IS_YZ || ENABLED(DELTA)
2458
+    SERIAL_ECHOLNPAIR(" C:", pos.z);
2459
+  #else
2460
+    SERIAL_ECHOLNPAIR_P(SP_Z_LBL, pos.z);
2461
+  #endif
2462
+}
2463
+
2451 2464
 void Stepper::report_positions() {
2452 2465
 
2453 2466
   #ifdef __AVR__
@@ -2461,16 +2474,7 @@ void Stepper::report_positions() {
2461 2474
     if (was_enabled) wake_up();
2462 2475
   #endif
2463 2476
 
2464
-  #if CORE_IS_XY || CORE_IS_XZ || ENABLED(DELTA) || IS_SCARA
2465
-    SERIAL_ECHOPAIR(STR_COUNT_A, pos.x, " B:", pos.y);
2466
-  #else
2467
-    SERIAL_ECHOPAIR_P(PSTR(STR_COUNT_X), pos.x, SP_Y_LBL, pos.y);
2468
-  #endif
2469
-  #if CORE_IS_XZ || CORE_IS_YZ || ENABLED(DELTA)
2470
-    SERIAL_ECHOLNPAIR(" C:", pos.z);
2471
-  #else
2472
-    SERIAL_ECHOLNPAIR_P(SP_Z_LBL, pos.z);
2473
-  #endif
2477
+  report_a_position(pos);
2474 2478
 }
2475 2479
 
2476 2480
 #if ENABLED(BABYSTEPPING)

+ 1
- 0
Marlin/src/module/stepper.h Visa fil

@@ -411,6 +411,7 @@ class Stepper {
411 411
     static void set_axis_position(const AxisEnum a, const int32_t &v);
412 412
 
413 413
     // Report the positions of the steppers, in steps
414
+    static void report_a_position(const xyz_long_t &pos);
414 415
     static void report_positions();
415 416
 
416 417
     // Quickly stop all steppers

Laddar…
Avbryt
Spara