Fix feed rate mode handling

diff --git a/src/command.c b/src/command.c
index 4a43ec357860ba0b59490f0f371d0de98a8dc80f..16397d2647ff425ae3fb37ff240c508b91059b1c 100644 (file)
--- a/src/command.c
+++ b/src/command.c
@@ -227,7 +227,7 @@ void command_callback() {
   default:
     if (estop_triggered()) {status = STAT_MACHINE_ALARMED; break;}
     else if (mp_is_flushing()) break; // Flush GCode command
-    else if (mp_get_planner_buffer_room() < 2 ||
+    else if (!mp_get_planner_buffer_room() ||
              mp_is_resuming() ||
              mach_arc_active() ||
              mach_is_homing() ||

diff --git a/src/machine.c b/src/machine.c
index aa6e59a9d0ced33a28bc4e8c76dec2b022a61d59..98b3fefa02994a9d10f170a4426b8fd496ac51ce 100644 (file)
--- a/src/machine.c
+++ b/src/machine.c
@@ -456,28 +456,6 @@ float mach_get_work_position(uint8_t axis) {
  * These functions are not part of the NIST defined functions
  */
 
-/* Perform final operations for a rapid or feed
- *
- * These routines set the point position in the gcode model.
- *
- * Note: As far as the machine is concerned the final
- * position of a Gcode block (move) is achieved as soon as the move is
- * planned and the move target becomes the new model position.  In
- * reality the planner will (in all likelihood) have only just queued
- * the move for later execution, and the real tool position is still
- * close to the starting point.
- */
-void mach_finalize_move() {
-  copy_vector(mach.position, mach.gm.target);        // update model position
-
-  // if in inverse time mode reset feed rate so next block requires an
-  // explicit feed rate setting
-  if (mach.gm.feed_rate_mode == INVERSE_TIME_MODE &&
-      mach.gm.motion_mode == MOTION_MODE_FEED)
-    mach.gm.feed_rate = 0;
-}
-
-
 /* Compute optimal and minimum move times into the gcode_state
  *
  * "Minimum time" is the fastest the move can be performed given the velocity
@@ -543,12 +521,11 @@ float mach_calc_move_time(const float axis_length[],
 
   // Compute times for feed motion
   if (mach.gm.motion_mode != MOTION_MODE_RAPID) {
-    if (mach.gm.feed_rate_mode == INVERSE_TIME_MODE) {
+    if (mach.gm.feed_rate_mode == INVERSE_TIME_MODE)
       // Feed rate was un-inverted to minutes by mach_set_feed_rate()
       max_time = mach.gm.feed_rate;
-      mach.gm.feed_rate_mode = UNITS_PER_MINUTE_MODE;
 
-    } else {
+    else {
       // Compute length of linear move in millimeters.  Feed rate in mm/min.
       max_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] +
                       axis_square[AXIS_Z]) / mach.gm.feed_rate;
@@ -881,7 +858,7 @@ stat_t mach_rapid(float target[], float flags[]) {
   // prep and plan the move
   mach_update_work_offsets();      // update fully resolved offsets to state
   status = mp_aline(mach.gm.target, mach.gm.line); // send the move to planner
-  mach_finalize_move();
+  copy_vector(mach.position, mach.gm.target);      // update model position
 
   return status;
 }
@@ -934,7 +911,7 @@ stat_t mach_goto_g30_position(float target[], float flags[]) {
 void mach_set_feed_rate(float feed_rate) {
   if (mach.gm.feed_rate_mode == INVERSE_TIME_MODE)
     // normalize to minutes (active for this gcode block only)
-    mach.gm.feed_rate = 1 / feed_rate;
+    mach.gm.feed_rate = feed_rate ? 1 / feed_rate : 0; // Avoid div by zero
 
   else mach.gm.feed_rate = TO_MILLIMETERS(feed_rate);
 }
@@ -942,6 +919,8 @@ void mach_set_feed_rate(float feed_rate) {
 
 /// G93, G94
 void mach_set_feed_rate_mode(feed_rate_mode_t mode) {
+  if (mach.gm.feed_rate_mode == mode) return;
+  mach.gm.feed_rate = 0; // Force setting feed rate after changing modes
   mach.gm.feed_rate_mode = mode;
 }
 
@@ -963,7 +942,8 @@ stat_t mach_dwell(float seconds) {
 /// G1
 stat_t mach_feed(float target[], float flags[]) {
   // trap zero feed rate condition
-  if (mach.gm.feed_rate_mode != INVERSE_TIME_MODE && fp_ZERO(mach.gm.feed_rate))
+  if (fp_ZERO(mach.gm.feed_rate) ||
+      (mach.gm.feed_rate_mode == INVERSE_TIME_MODE && !mach.gf.feed_rate))
     return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
 
   mach.gm.motion_mode = MOTION_MODE_FEED;
@@ -976,7 +956,7 @@ stat_t mach_feed(float target[], float flags[]) {
   // prep and plan the move
   mach_update_work_offsets();      // update fully resolved offsets to state
   status = mp_aline(mach.gm.target, mach.gm.line); // send the move to planner
-  mach_finalize_move();
+  copy_vector(mach.position, mach.gm.target);      // update model position
 
   return status;
 }

diff --git a/src/machine.h b/src/machine.h
index 6d2ba470d08e85bdc65590496ab8b3010bcae5dc..342d7bfb48901532ac0a270d94ba8c7d04c2f681 100644 (file)
--- a/src/machine.h
+++ b/src/machine.h
@@ -346,7 +346,6 @@ float mach_get_work_position(uint8_t axis);
 
 // Critical helpers
 float mach_calc_move_time(const float axis_length[], const float axis_square[]);
-void mach_finalize_move();
 stat_t mach_deferred_write_callback();
 void mach_set_model_target(float target[], float flag[]);
 stat_t mach_test_soft_limits(float target[]);

diff --git a/src/plan/arc.c b/src/plan/arc.c
index 105788bf984ae93c0deeba0c67ad42919248b4d7..b8a346dd4e781bf8ca40619521c5be4f19297fd2 100644 (file)
--- a/src/plan/arc.c
+++ b/src/plan/arc.c
@@ -77,27 +77,15 @@ arc_t arc = {0};
  */
 static float _estimate_arc_time(float length, float linear_travel,
                                 float planar_travel) {
-  float t;
-
   // Determine move time at requested feed rate
-  if (mach.gm.feed_rate_mode == INVERSE_TIME_MODE) {
-    // Inverse feed rate has been normalized to minutes
-    t = mach.gm.feed_rate;
-
-    // Reset feed rate so next block requires an explicit feed rate setting
-    mach.gm.feed_rate = 0;
-    mach.gm.feed_rate_mode = UNITS_PER_MINUTE_MODE;
-
-  } else t = length / mach.gm.feed_rate;
+  // Inverse feed rate is normalized to minutes
+  float time = mach.gm.feed_rate_mode == INVERSE_TIME_MODE ?
+    mach.gm.feed_rate : length / mach.gm.feed_rate;
 
   // Downgrade the time if there is a rate-limiting axis
-  t = max3(t, planar_travel / mach.a[arc.plane_axis_0].feedrate_max,
-           planar_travel / mach.a[arc.plane_axis_1].feedrate_max);
-
-  if (0 < fabs(linear_travel))
-    t = max(t, fabs(linear_travel / mach.a[arc.linear_axis].feedrate_max));
-
-  return t;
+  return max4(time, planar_travel / mach.a[arc.plane_axis_0].feedrate_max,
+              planar_travel / mach.a[arc.plane_axis_1].feedrate_max,
+              fabs(linear_travel) / mach.a[arc.linear_axis].feedrate_max);
 }
 
 
@@ -344,7 +332,7 @@ static stat_t _compute_arc(const float position[], float offset[],
 }
 
 
-/*** machine entry point for arc
+/*** Machine entry point for arc
  *
  * Generates an arc by queuing line segments to the move buffer. The arc is
  * approximated by generating a large number of tiny, linear segments.
@@ -354,7 +342,8 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints
                      float radius,  // non-zero radius implies radius mode
                      uint8_t motion_mode) { // defined motion mode
   // trap missing feed rate
-  if (mach.gm.feed_rate_mode != INVERSE_TIME_MODE && fp_ZERO(mach.gm.feed_rate))
+  if (fp_ZERO(mach.gm.feed_rate) ||
+      (mach.gm.feed_rate_mode == INVERSE_TIME_MODE && !mach.gf.feed_rate))
     return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
 
   // set radius mode flag and do simple test(s)
@@ -443,15 +432,15 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints
   // compute arc runtime values
   RITORNO(_compute_arc(mach.position, offset, rotations, full_circle));
 
-  arc.run_state = MOVE_RUN;                // enable arc run from the callback
-  mach_arc_callback();                     // Queue initial arc moves
-  mach_finalize_move();
+  arc.run_state = MOVE_RUN;                   // Enable arc run in callback
+  mach_arc_callback();                        // Queue initial arc moves
+  copy_vector(mach.position, mach.gm.target); // update model position
 
   return STAT_OK;
 }
 
 
-/* Generate an arc
+/*** Generate an arc
  *
  *  Called from the controller main loop.  Each time it's called it queues
  *  as many arc segments (lines) as it can before it blocks, then returns.