Skip to content

Commit

Permalink
Asynchronous M114 and (R)ealtime position option (MarlinFirmware#17032)
Browse files Browse the repository at this point in the history
  • Loading branch information
@thinkyhead @oechslein
thinkyhead authored and oechslein committed Mar 21, 2020
1 parent 8c89b29 commit 502e19e
Show file tree
Hide file tree
Showing 8 changed files with 131 additions and 36 deletions.
6 changes: 4 additions & 2 deletions Marlin/Configuration_adv.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -276,8 +276,10 @@
#define AUTOTEMP_OLDWEIGHT 0.98
#endif

// Show extra position information with 'M114 D'
#define M114_DETAIL // CO Enabled
// Extra options for the M114 "Current Position" report
#define M114_DETAIL // CO Enabled // Use 'M114` for details to check planner calculations
//#define M114_REALTIME // Real current position based on forward kinematics
//#define M114_LEGACY // M114 used to synchronize on every call. Enable if needed.

// Show Temperature ADC value
// Enable for M105 to include ADC values read from temperature sensors.
Expand Down
26 changes: 25 additions & 1 deletion Marlin/src/core/types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -187,6 +187,12 @@ struct XYval {
};
FI void set(const T px) { x = px; }
FI void set(const T px, const T py) { x = px; y = py; }
FI void set(const T (&arr)[XY]) { x = arr[0]; y = arr[1]; }
FI void set(const T (&arr)[XYZ]) { x = arr[0]; y = arr[1]; }
FI void set(const T (&arr)[XYZE]) { x = arr[0]; y = arr[1]; }
#if XYZE_N > XYZE
FI void set(const T (&arr)[XYZE_N]) { x = arr[0]; y = arr[1]; }
#endif
FI void reset() { x = y = 0; }
FI T magnitude() const { return (T)sqrtf(x*x + y*y); }
FI operator T* () { return pos; }
Expand All @@ -197,6 +203,8 @@ struct XYval {
FI XYval<int16_t> asInt() const { return { int16_t(x), int16_t(y) }; }
FI XYval<int32_t> asLong() { return { int32_t(x), int32_t(y) }; }
FI XYval<int32_t> asLong() const { return { int32_t(x), int32_t(y) }; }
FI XYval<int32_t> ROUNDL() { return { int32_t(LROUND(x)), int32_t(LROUND(y)) }; }
FI XYval<int32_t> ROUNDL() const { return { int32_t(LROUND(x)), int32_t(LROUND(y)) }; }
FI XYval<float> asFloat() { return { float(x), float(y) }; }
FI XYval<float> asFloat() const { return { float(x), float(y) }; }
FI XYval<float> reciprocal() const { return { _RECIP(x), _RECIP(y) }; }
Expand Down Expand Up @@ -290,6 +298,12 @@ struct XYZval {
FI void set(const T px, const T py) { x = px; y = py; }
FI void set(const T px, const T py, const T pz) { x = px; y = py; z = pz; }
FI void set(const XYval<T> pxy, const T pz) { x = pxy.x; y = pxy.y; z = pz; }
FI void set(const T (&arr)[XY]) { x = arr[0]; y = arr[1]; }
FI void set(const T (&arr)[XYZ]) { x = arr[0]; y = arr[1]; z = arr[2]; }
FI void set(const T (&arr)[XYZE]) { x = arr[0]; y = arr[1]; z = arr[2]; }
#if XYZE_N > XYZE
FI void set(const T (&arr)[XYZE_N]) { x = arr[0]; y = arr[1]; z = arr[2]; }
#endif
FI void reset() { x = y = z = 0; }
FI T magnitude() const { return (T)sqrtf(x*x + y*y + z*z); }
FI operator T* () { return pos; }
Expand All @@ -300,6 +314,8 @@ struct XYZval {
FI XYZval<int16_t> asInt() const { return { int16_t(x), int16_t(y), int16_t(z) }; }
FI XYZval<int32_t> asLong() { return { int32_t(x), int32_t(y), int32_t(z) }; }
FI XYZval<int32_t> asLong() const { return { int32_t(x), int32_t(y), int32_t(z) }; }
FI XYZval<int32_t> ROUNDL() { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)) }; }
FI XYZval<int32_t> ROUNDL() const { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)) }; }
FI XYZval<float> asFloat() { return { float(x), float(y), float(z) }; }
FI XYZval<float> asFloat() const { return { float(x), float(y), float(z) }; }
FI XYZval<float> reciprocal() const { return { _RECIP(x), _RECIP(y), _RECIP(z) }; }
Expand Down Expand Up @@ -397,12 +413,20 @@ struct XYZEval {
FI void set(const XYval<T> pxy, const T pz, const T pe) { x = pxy.x; y = pxy.y; z = pz; e = pe; }
FI void set(const XYval<T> pxy, const XYval<T> pze) { x = pxy.x; y = pxy.y; z = pze.z; e = pze.e; }
FI void set(const XYZval<T> pxyz, const T pe) { x = pxyz.x; y = pxyz.y; z = pxyz.z; e = pe; }
FI void set(const T (&arr)[XY]) { x = arr[0]; y = arr[1]; }
FI void set(const T (&arr)[XYZ]) { x = arr[0]; y = arr[1]; z = arr[2]; }
FI void set(const T (&arr)[XYZE]) { x = arr[0]; y = arr[1]; z = arr[2]; e = arr[3]; }
#if XYZE_N > XYZE
FI void set(const T (&arr)[XYZE_N]) { x = arr[0]; y = arr[1]; z = arr[2]; e = arr[3]; }
#endif
FI XYZEval<T> copy() const { return *this; }
FI XYZEval<T> ABS() const { return { T(_ABS(x)), T(_ABS(y)), T(_ABS(z)), T(_ABS(e)) }; }
FI XYZEval<int16_t> asInt() { return { int16_t(x), int16_t(y), int16_t(z), int16_t(e) }; }
FI XYZEval<int16_t> asInt() const { return { int16_t(x), int16_t(y), int16_t(z), int16_t(e) }; }
FI XYZEval<int32_t> asLong() const { return { int32_t(x), int32_t(y), int32_t(z), int32_t(e) }; }
FI XYZEval<int32_t> asLong() { return { int32_t(x), int32_t(y), int32_t(z), int32_t(e) }; }
FI XYZEval<int32_t> asLong() const { return { int32_t(x), int32_t(y), int32_t(z), int32_t(e) }; }
FI XYZEval<int32_t> ROUNDL() { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)), int32_t(LROUND(e)) }; }
FI XYZEval<int32_t> ROUNDL() const { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)), int32_t(LROUND(e)) }; }
FI XYZEval<float> asFloat() { return { float(x), float(y), float(z), float(e) }; }
FI XYZEval<float> asFloat() const { return { float(x), float(y), float(z), float(e) }; }
FI XYZEval<float> reciprocal() const { return { _RECIP(x), _RECIP(y), _RECIP(z), _RECIP(e) }; }
Expand Down
30 changes: 25 additions & 5 deletions Marlin/src/gcode/host/M114.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,14 +34,15 @@
#include "../../core/debug_out.h"
#endif

void report_xyze(const xyze_pos_t &pos, const uint8_t n=4, const uint8_t precision=3) {
void report_xyze(const xyze_pos_t &pos, const uint8_t n=XYZE, const uint8_t precision=3) {
char str[12];
for (uint8_t a = 0; a < n; a++) {
SERIAL_CHAR(' ', axis_codes[a], ':');
SERIAL_ECHO(dtostrf(pos[a], 1, precision, str));
}
SERIAL_EOL();
}
inline void report_xyz(const xyze_pos_t &pos) { report_xyze(pos, 3); }

void report_xyz(const xyz_pos_t &pos, const uint8_t precision=3) {
char str[12];
Expand All @@ -51,30 +52,34 @@
}
SERIAL_EOL();
}
inline void report_xyz(const xyze_pos_t &pos) { report_xyze(pos, 3); }

void report_current_position_detail() {

// Position as sent by G-code
SERIAL_ECHOPGM("\nLogical:");
report_xyz(current_position.asLogical());

// Cartesian position in native machine space
SERIAL_ECHOPGM("Raw: ");
report_xyz(current_position);

xyze_pos_t leveled = current_position;

#if HAS_LEVELING
// Current position with leveling applied
SERIAL_ECHOPGM("Leveled:");
planner.apply_leveling(leveled);
report_xyz(leveled);

// Test planner un-leveling. This should match the Raw result.
SERIAL_ECHOPGM("UnLevel:");
xyze_pos_t unleveled = leveled;
planner.unapply_leveling(unleveled);
report_xyz(unleveled);
#endif

#if IS_KINEMATIC
// Kinematics applied to the leveled position
#if IS_SCARA
SERIAL_ECHOPGM("ScaraK: ");
#else
Expand Down Expand Up @@ -180,12 +185,21 @@
#endif // M114_DETAIL

/**
* M114: Report current position to host
* M114: Report the current position to host.
* Since steppers are moving, the count positions are
* projected by using planner calculations.
* D - Report more detail. This syncs the planner. (Requires M114_DETAIL)
* E - Report E stepper position (Requires M114_DETAIL)
* R - Report the realtime position instead of projected.
*/
void GcodeSuite::M114() {

#if ENABLED(M114_DETAIL)
if (parser.seen('D')) {
#if DISABLED(M114_LEGACY)
planner.synchronize();
#endif
report_current_position();
report_current_position_detail();
return;
}
Expand All @@ -195,6 +209,12 @@ void GcodeSuite::M114() {
}
#endif

planner.synchronize();
report_current_position();
#if ENABLED(M114_REALTIME)
if (parser.seen('R')) { report_real_position(); return; }
#endif

#if ENABLED(M114_LEGACY)
planner.synchronize();
#endif
report_current_position_projected();
}
56 changes: 44 additions & 12 deletions Marlin/src/module/motion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -206,17 +206,53 @@ xyz_pos_t cartes;
/**
* Output the current position to serial
*/
void report_current_position() {
const xyz_pos_t lpos = current_position.asLogical();
SERIAL_ECHOPAIR("X:", lpos.x, " Y:", lpos.y, " Z:", lpos.z, " E:", current_position.e);

inline void report_more_positions() {
stepper.report_positions();

#if IS_SCARA
scara_report_positions();
#endif
}

// Report the logical position for a given machine position
inline void report_logical_position(const xyze_pos_t &rpos) {
const xyze_pos_t lpos = rpos.asLogical();
SERIAL_ECHOPAIR_P(X_LBL, lpos.x, SP_Y_LBL, lpos.y, SP_Z_LBL, lpos.z, SP_E_LBL, lpos.e);
report_more_positions();
}

// Report the real current position according to the steppers.
// Forward kinematics and un-leveling are applied.
void report_real_position() {
get_cartesian_from_steppers();
xyze_pos_t npos = cartes;
npos.e = planner.get_axis_position_mm(E_AXIS);

#if HAS_POSITION_MODIFIERS
planner.unapply_modifiers(npos
#if HAS_LEVELING
, true
#endif
);
#endif

report_logical_position(npos);
}

// Report the logical current position according to the most recent G-code command
void report_current_position() { report_logical_position(current_position); }

/**
* Report the logical current position according to the most recent G-code command.
* The planner.position always corresponds to the last G-code too. This makes M114
* suitable for debugging kinematics and leveling while avoiding planner sync that
* definitively interrupts the printing flow.
*/
void report_current_position_projected() {
report_logical_position(current_position);
stepper.report_a_position(planner.position);
}

/**
* sync_plan_position
*
Expand All @@ -241,11 +277,7 @@ void sync_plan_position_e() { planner.set_e_position_mm(current_position.e); }
*/
void get_cartesian_from_steppers() {
#if ENABLED(DELTA)
forward_kinematics_DELTA(
planner.get_axis_position_mm(A_AXIS),
planner.get_axis_position_mm(B_AXIS),
planner.get_axis_position_mm(C_AXIS)
);
forward_kinematics_DELTA(planner.get_axis_positions_mm());
#else
#if IS_SCARA
forward_kinematics_SCARA(
Expand Down Expand Up @@ -663,11 +695,11 @@ void restore_feedrate_and_scaling() {

FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
const millis_t ms = millis();
thermalManager.manage_heater(); // This returns immediately if not really needed.
if (ELAPSED(ms, next_idle_ms)) {
next_idle_ms = ms + 200UL;
idle();
return idle();
}
thermalManager.manage_heater(); // Returns immediately on most calls
}

#if IS_KINEMATIC
Expand Down Expand Up @@ -1324,7 +1356,7 @@ void do_homing_move(const AxisEnum axis, const float distance, const feedRate_t
current_position[axis] = distance;
line_to_current_position(real_fr_mm_s);
#else
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) };
abce_pos_t target = planner.get_axis_positions_mm();
target[axis] = 0;
planner.set_machine_position_mm(target);
target[axis] = distance;
Expand Down
2 changes: 2 additions & 0 deletions Marlin/src/module/motion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -162,7 +162,9 @@ typedef struct { xyz_pos_t min, max; } axis_limits_t;
#define update_software_endstops(...) NOOP
#endif

void report_real_position();
void report_current_position();
void report_current_position_projected();

void get_cartesian_from_steppers();
void set_current_from_steppers_for_axis(const AxisEnum axis);
Expand Down
22 changes: 16 additions & 6 deletions Marlin/src/module/planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -289,6 +289,12 @@ class Planner {
static float extruder_advance_K[EXTRUDERS];
#endif

/**
* The current position of the tool in absolute steps
* Recalculated if any axis_steps_per_mm are changed by gcode
*/
static xyze_long_t position;

#if HAS_POSITION_FLOAT
static xyze_pos_t position_float;
#endif
Expand All @@ -305,12 +311,6 @@ class Planner {

private:

/**
* The current position of the tool in absolute steps
* Recalculated if any axis_steps_per_mm are changed by gcode
*/
static xyze_long_t position;

/**
* Speed of previous path line segment
*/
Expand Down Expand Up @@ -725,6 +725,16 @@ class Planner {
*/
static float get_axis_position_mm(const AxisEnum axis);

static inline abce_pos_t get_axis_positions_mm() {
const abce_pos_t out = {
get_axis_position_mm(A_AXIS),
get_axis_position_mm(B_AXIS),
get_axis_position_mm(C_AXIS),
get_axis_position_mm(E_AXIS)
};
return out;
}

// SCARA AB axes are in degrees, not mm
#if IS_SCARA
FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }
Expand Down
24 changes: 14 additions & 10 deletions Marlin/src/module/stepper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2448,6 +2448,19 @@ int32_t Stepper::triggered_position(const AxisEnum axis) {
return v;
}

void Stepper::report_a_position(const xyz_long_t &pos) {
#if CORE_IS_XY || CORE_IS_XZ || ENABLED(DELTA) || IS_SCARA
SERIAL_ECHOPAIR(STR_COUNT_A, pos.x, " B:", pos.y);
#else
SERIAL_ECHOPAIR_P(PSTR(STR_COUNT_X), pos.x, SP_Y_LBL, pos.y);
#endif
#if CORE_IS_XZ || CORE_IS_YZ || ENABLED(DELTA)
SERIAL_ECHOLNPAIR(" C:", pos.z);
#else
SERIAL_ECHOLNPAIR_P(SP_Z_LBL, pos.z);
#endif
}

void Stepper::report_positions() {

#ifdef __AVR__
Expand All @@ -2461,16 +2474,7 @@ void Stepper::report_positions() {
if (was_enabled) wake_up();
#endif

#if CORE_IS_XY || CORE_IS_XZ || ENABLED(DELTA) || IS_SCARA
SERIAL_ECHOPAIR(STR_COUNT_A, pos.x, " B:", pos.y);
#else
SERIAL_ECHOPAIR_P(PSTR(STR_COUNT_X), pos.x, SP_Y_LBL, pos.y);
#endif
#if CORE_IS_XZ || CORE_IS_YZ || ENABLED(DELTA)
SERIAL_ECHOLNPAIR(" C:", pos.z);
#else
SERIAL_ECHOLNPAIR_P(SP_Z_LBL, pos.z);
#endif
report_a_position(pos);
}

#if ENABLED(BABYSTEPPING)
Expand Down
1 change: 1 addition & 0 deletions Marlin/src/module/stepper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -411,6 +411,7 @@ class Stepper {
static void set_axis_position(const AxisEnum a, const int32_t &v);

// Report the positions of the steppers, in steps
static void report_a_position(const xyz_long_t &pos);
static void report_positions();

// Quickly stop all steppers
Expand Down

0 comments on commit 502e19e

Please sign in to comment.