Browse Source

Fix stepper/planner block handling, race conditions (#11098)

- Allow planner to alter the deceleration phase of the currently executing block.
- Remove BUSY flag, as it is NON ATOMIC to set bits in the Stepper ISR and Planner at the same time.
Eduardo José Tagle 7 years ago
parent
commit
edb21f349a
4 changed files with 177 additions and 98 deletions
  1. 120
    74
      Marlin/src/module/planner.cpp
  2. 21
    20
      Marlin/src/module/planner.h
  3. 31
    2
      Marlin/src/module/stepper.cpp
  4. 5
    2
      Marlin/src/module/stepper.h

+ 120
- 74
Marlin/src/module/planner.cpp View File

104
  * A ring buffer of moves described in steps
104
  * A ring buffer of moves described in steps
105
  */
105
  */
106
 block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
106
 block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
107
-volatile uint8_t Planner::block_buffer_head,  // Index of the next block to be pushed
108
-                 Planner::block_buffer_tail;  // Index of the busy block, if any
109
-uint16_t Planner::cleaning_buffer_counter;    // A counter to disable queuing of blocks
110
-uint8_t Planner::delay_before_delivering,     // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
111
-        Planner::block_buffer_planned;        // Index of the optimally planned block
107
+volatile uint8_t Planner::block_buffer_head,    // Index of the next block to be pushed
108
+                 Planner::block_buffer_nonbusy, // Index of the first non-busy block
109
+                 Planner::block_buffer_planned, // Index of the optimally planned block
110
+                 Planner::block_buffer_tail;    // Index of the busy block, if any
111
+uint16_t Planner::cleaning_buffer_counter;      // A counter to disable queuing of blocks
112
+uint8_t Planner::delay_before_delivering;       // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
112
 
113
 
113
 uint32_t Planner::max_acceleration_mm_per_s2[XYZE_N],    // (mm/s^2) M201 XYZE
114
 uint32_t Planner::max_acceleration_mm_per_s2[XYZE_N],    // (mm/s^2) M201 XYZE
114
          Planner::max_acceleration_steps_per_s2[XYZE_N], // (steps/s^2) Derived from mm_per_s2
115
          Planner::max_acceleration_steps_per_s2[XYZE_N], // (steps/s^2) Derived from mm_per_s2
240
     bed_level_matrix.set_to_identity();
241
     bed_level_matrix.set_to_identity();
241
   #endif
242
   #endif
242
   clear_block_buffer();
243
   clear_block_buffer();
243
-  block_buffer_planned = 0;
244
   delay_before_delivering = 0;
244
   delay_before_delivering = 0;
245
 }
245
 }
246
 
246
 
703
 /**
703
 /**
704
  * Calculate trapezoid parameters, multiplying the entry- and exit-speeds
704
  * Calculate trapezoid parameters, multiplying the entry- and exit-speeds
705
  * by the provided factors.
705
  * by the provided factors.
706
+ **
707
+ * ############ VERY IMPORTANT ############
708
+ * NOTE that the PRECONDITION to call this function is that the block is
709
+ * NOT BUSY and it is marked as RECALCULATE. That WARRANTIES the Stepper ISR
710
+ * is not and will not use the block while we modify it, so it is safe to
711
+ * alter its values.
706
  */
712
  */
707
 void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {
713
 void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {
708
 
714
 
744
       cruise_rate = block->nominal_rate;
750
       cruise_rate = block->nominal_rate;
745
   #endif
751
   #endif
746
 
752
 
747
-  // block->accelerate_until = accelerate_steps;
748
-  // block->decelerate_after = accelerate_steps+plateau_steps;
749
-
750
   #if ENABLED(S_CURVE_ACCELERATION)
753
   #if ENABLED(S_CURVE_ACCELERATION)
751
     // Jerk controlled speed requires to express speed versus time, NOT steps
754
     // Jerk controlled speed requires to express speed versus time, NOT steps
752
     uint32_t acceleration_time = ((float)(cruise_rate - initial_rate) / accel) * (STEPPER_TIMER_RATE),
755
     uint32_t acceleration_time = ((float)(cruise_rate - initial_rate) / accel) * (STEPPER_TIMER_RATE),
755
     // And to offload calculations from the ISR, we also calculate the inverse of those times here
758
     // And to offload calculations from the ISR, we also calculate the inverse of those times here
756
     uint32_t acceleration_time_inverse = get_period_inverse(acceleration_time);
759
     uint32_t acceleration_time_inverse = get_period_inverse(acceleration_time);
757
     uint32_t deceleration_time_inverse = get_period_inverse(deceleration_time);
760
     uint32_t deceleration_time_inverse = get_period_inverse(deceleration_time);
758
-
759
   #endif
761
   #endif
760
 
762
 
761
-  // Fill variables used by the stepper in a critical section
762
-  const bool was_enabled = STEPPER_ISR_ENABLED();
763
-  if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
764
-
765
-  // Don't update variables if block is busy; it is being interpreted by the planner.
766
-  // If this happens, there's a problem... The block speed is inconsistent. Some values
767
-  // have already been updated, but the Stepper ISR is already using the block. Fortunately,
768
-  // the values being used by the Stepper ISR weren't touched, so just stop here...
769
-  // TODO: There may be a way to update a running block, depending on the stepper ISR position.
770
-  if (!TEST(block->flag, BLOCK_BIT_BUSY)) {
771
-    block->accelerate_until = accelerate_steps;
772
-    block->decelerate_after = accelerate_steps + plateau_steps;
773
-    block->initial_rate = initial_rate;
774
-    #if ENABLED(S_CURVE_ACCELERATION)
775
-      block->acceleration_time = acceleration_time;
776
-      block->deceleration_time = deceleration_time;
777
-      block->acceleration_time_inverse = acceleration_time_inverse;
778
-      block->deceleration_time_inverse = deceleration_time_inverse;
779
-      block->cruise_rate = cruise_rate;
780
-    #endif
781
-    block->final_rate = final_rate;
782
-  }
783
-  if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
763
+  // Store new block parameters
764
+  block->accelerate_until = accelerate_steps;
765
+  block->decelerate_after = accelerate_steps + plateau_steps;
766
+  block->initial_rate = initial_rate;
767
+  #if ENABLED(S_CURVE_ACCELERATION)
768
+    block->acceleration_time = acceleration_time;
769
+    block->deceleration_time = deceleration_time;
770
+    block->acceleration_time_inverse = acceleration_time_inverse;
771
+    block->deceleration_time_inverse = deceleration_time_inverse;
772
+    block->cruise_rate = cruise_rate;
773
+  #endif
774
+  block->final_rate = final_rate;
784
 }
775
 }
785
 
776
 
786
 /*                            PLANNER SPEED DEFINITION
777
 /*                            PLANNER SPEED DEFINITION
831
       streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the
822
       streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the
832
       planner buffer that don't change with the addition of a new block, as describe above. In addition,
823
       planner buffer that don't change with the addition of a new block, as describe above. In addition,
833
       this block can never be less than block_buffer_tail and will always be pushed forward and maintain
824
       this block can never be less than block_buffer_tail and will always be pushed forward and maintain
834
-      this requirement when encountered by the plan_discard_current_block() routine during a cycle.
825
+      this requirement when encountered by the Planner::discard_current_block() routine during a cycle.
835
 
826
 
836
   NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short
827
   NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short
837
   line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't
828
   line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't
875
         // ISR does not consume the block before being recalculated
866
         // ISR does not consume the block before being recalculated
876
         SBI(current->flag, BLOCK_BIT_RECALCULATE);
867
         SBI(current->flag, BLOCK_BIT_RECALCULATE);
877
 
868
 
878
-        // Set the new entry speed
879
-        current->entry_speed_sqr = new_entry_speed_sqr;
869
+        // But there is an inherent race condition here, as the block may have
870
+        // become BUSY just before being marked RECALCULATE, so check for that!
871
+        if (stepper.is_block_busy(current)) {
872
+          // Block became busy. Clear the RECALCULATE flag (no point in
873
+          // recalculating BUSY blocks). And don't set its speed, as it can't
874
+          // be updated at this time.
875
+          CBI(current->flag, BLOCK_BIT_RECALCULATE);
876
+        }
877
+        else {
878
+          // Block is not BUSY so this is ahead of the Stepper ISR:
879
+          // Just Set the new entry speed.
880
+          current->entry_speed_sqr = new_entry_speed_sqr;
881
+        }
880
       }
882
       }
881
     }
883
     }
882
   }
884
   }
902
   // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
904
   // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
903
   // block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
905
   // block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
904
   // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
906
   // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
905
-  block_t *current;
906
   const block_t *next = NULL;
907
   const block_t *next = NULL;
907
   while (block_index != planned_block_index) {
908
   while (block_index != planned_block_index) {
908
 
909
 
909
     // Perform the reverse pass
910
     // Perform the reverse pass
910
-    current = &block_buffer[block_index];
911
+    block_t *current = &block_buffer[block_index];
911
 
912
 
912
     // Only consider non sync blocks
913
     // Only consider non sync blocks
913
     if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
914
     if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
917
 
918
 
918
     // Advance to the next
919
     // Advance to the next
919
     block_index = prev_block_index(block_index);
920
     block_index = prev_block_index(block_index);
921
+
922
+    // The ISR could advance the block_buffer_planned while we were doing the reverse pass.
923
+    // We must try to avoid using an already consumed block as the last one - So follow
924
+    // changes to the pointer and make sure to limit the loop to the currently busy block
925
+    while (planned_block_index != block_buffer_planned) {
926
+
927
+      // If we reached the busy block or an already processed block, break the loop now
928
+      if (block_index == planned_block_index) return;
929
+
930
+      // Advance the pointer, following the busy block
931
+      planned_block_index = next_block_index(planned_block_index);
932
+    }
920
   }
933
   }
921
 }
934
 }
922
 
935
 
940
         // so the stepper ISR does not consume the block before being recalculated
953
         // so the stepper ISR does not consume the block before being recalculated
941
         SBI(current->flag, BLOCK_BIT_RECALCULATE);
954
         SBI(current->flag, BLOCK_BIT_RECALCULATE);
942
 
955
 
943
-        // Always <= max_entry_speed_sqr. Backward pass sets this.
944
-        current->entry_speed_sqr = new_entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
956
+        // But there is an inherent race condition here, as the block maybe
957
+        // became BUSY, just before it was marked as RECALCULATE, so check
958
+        // if that is the case!
959
+        if (stepper.is_block_busy(current)) {
960
+          // Block became busy. Clear the RECALCULATE flag (no point in
961
+          //  recalculating BUSY blocks and don't set its speed, as it can't
962
+          //  be updated at this time.
963
+          CBI(current->flag, BLOCK_BIT_RECALCULATE);
964
+        }
965
+        else {
966
+          // Block is not BUSY, we won the race against the Stepper ISR:
967
+
968
+          // Always <= max_entry_speed_sqr. Backward pass sets this.
969
+          current->entry_speed_sqr = new_entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
945
 
970
 
946
-        // Set optimal plan pointer.
947
-        block_buffer_planned = block_index;
971
+          // Set optimal plan pointer.
972
+          block_buffer_planned = block_index;
973
+        }
948
       }
974
       }
949
     }
975
     }
950
 
976
 
981
 
1007
 
982
     // Skip SYNC blocks
1008
     // Skip SYNC blocks
983
     if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
1009
     if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
984
-      forward_pass_kernel(previous, current, block_index);
1010
+      // If there's no previous block or the previous block is not
1011
+      // BUSY (thus, modifiable) run the forward_pass_kernel. Otherwise,
1012
+      // the previous block became BUSY, so assume the current block's
1013
+      // entry speed can't be altered (since that would also require
1014
+      // updating the exit speed of the previous block).
1015
+      if (!previous || !stepper.is_block_busy(previous))
1016
+        forward_pass_kernel(previous, current, block_index);
985
       previous = current;
1017
       previous = current;
986
     }
1018
     }
987
     // Advance to the previous
1019
     // Advance to the previous
996
  */
1028
  */
997
 void Planner::recalculate_trapezoids() {
1029
 void Planner::recalculate_trapezoids() {
998
   // The tail may be changed by the ISR so get a local copy.
1030
   // The tail may be changed by the ISR so get a local copy.
999
-  uint8_t block_index = block_buffer_tail;
1000
-
1001
-  // As there could be a sync block in the head of the queue, and the next loop must not
1002
-  // recalculate the head block (as it needs to be specially handled), scan backwards until
1003
-  // we find the first non SYNC block
1004
-  uint8_t head_block_index = block_buffer_head;
1031
+  uint8_t block_index = block_buffer_tail,
1032
+          head_block_index = block_buffer_head;
1033
+  // Since there could be a sync block in the head of the queue, and the
1034
+  // next loop must not recalculate the head block (as it needs to be
1035
+  // specially handled), scan backwards to the first non-SYNC block.
1005
   while (head_block_index != block_index) {
1036
   while (head_block_index != block_index) {
1006
 
1037
 
1007
     // Go back (head always point to the first free block)
1038
     // Go back (head always point to the first free block)
1008
-    uint8_t prev_index = prev_block_index(head_block_index);
1039
+    const uint8_t prev_index = prev_block_index(head_block_index);
1009
 
1040
 
1010
     // Get the pointer to the block
1041
     // Get the pointer to the block
1011
     block_t *prev = &block_buffer[prev_index];
1042
     block_t *prev = &block_buffer[prev_index];
1015
 
1046
 
1016
     // Examine the previous block. This and all following are SYNC blocks
1047
     // Examine the previous block. This and all following are SYNC blocks
1017
     head_block_index = prev_index;
1048
     head_block_index = prev_index;
1018
-  };
1049
+  }
1019
 
1050
 
1020
   // Go from the tail (currently executed block) to the first block, without including it)
1051
   // Go from the tail (currently executed block) to the first block, without including it)
1021
   block_t *current = NULL, *next = NULL;
1052
   block_t *current = NULL, *next = NULL;
1037
           // RECALCULATE yet, but the next one is. That's the reason for the following line.
1068
           // RECALCULATE yet, but the next one is. That's the reason for the following line.
1038
           SBI(current->flag, BLOCK_BIT_RECALCULATE);
1069
           SBI(current->flag, BLOCK_BIT_RECALCULATE);
1039
 
1070
 
1040
-          // NOTE: Entry and exit factors always > 0 by all previous logic operations.
1041
-          const float current_nominal_speed = SQRT(current->nominal_speed_sqr),
1042
-                      nomr = 1.0 / current_nominal_speed;
1043
-          calculate_trapezoid_for_block(current, current_entry_speed * nomr, next_entry_speed * nomr);
1044
-          #if ENABLED(LIN_ADVANCE)
1045
-            if (current->use_advance_lead) {
1046
-              const float comp = current->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
1047
-              current->max_adv_steps = current_nominal_speed * comp;
1048
-              current->final_adv_steps = next_entry_speed * comp;
1049
-            }
1050
-          #endif
1071
+          // But there is an inherent race condition here, as the block maybe
1072
+          // became BUSY, just before it was marked as RECALCULATE, so check
1073
+          // if that is the case!
1074
+          if (!stepper.is_block_busy(current)) {
1075
+            // Block is not BUSY, we won the race against the Stepper ISR:
1076
+
1077
+            // NOTE: Entry and exit factors always > 0 by all previous logic operations.
1078
+            const float current_nominal_speed = SQRT(current->nominal_speed_sqr),
1079
+                        nomr = 1.0 / current_nominal_speed;
1080
+            calculate_trapezoid_for_block(current, current_entry_speed * nomr, next_entry_speed * nomr);
1081
+            #if ENABLED(LIN_ADVANCE)
1082
+              if (current->use_advance_lead) {
1083
+                const float comp = current->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
1084
+                current->max_adv_steps = current_nominal_speed * comp;
1085
+                current->final_adv_steps = next_entry_speed * comp;
1086
+              }
1087
+            #endif
1088
+          }
1051
 
1089
 
1052
           // Reset current only to ensure next trapezoid is computed - The
1090
           // Reset current only to ensure next trapezoid is computed - The
1053
           // stepper is free to use the block from now on.
1091
           // stepper is free to use the block from now on.
1070
     // marked as RECALCULATE yet. That's the reason for the following line.
1108
     // marked as RECALCULATE yet. That's the reason for the following line.
1071
     SBI(next->flag, BLOCK_BIT_RECALCULATE);
1109
     SBI(next->flag, BLOCK_BIT_RECALCULATE);
1072
 
1110
 
1073
-    const float next_nominal_speed = SQRT(next->nominal_speed_sqr),
1074
-                nomr = 1.0 / next_nominal_speed;
1075
-    calculate_trapezoid_for_block(next, next_entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
1076
-    #if ENABLED(LIN_ADVANCE)
1077
-      if (next->use_advance_lead) {
1078
-        const float comp = next->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
1079
-        next->max_adv_steps = next_nominal_speed * comp;
1080
-        next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp;
1081
-      }
1082
-    #endif
1111
+    // But there is an inherent race condition here, as the block maybe
1112
+    // became BUSY, just before it was marked as RECALCULATE, so check
1113
+    // if that is the case!
1114
+    if (!stepper.is_block_busy(current)) {
1115
+      // Block is not BUSY, we won the race against the Stepper ISR:
1116
+
1117
+      const float next_nominal_speed = SQRT(next->nominal_speed_sqr),
1118
+                  nomr = 1.0 / next_nominal_speed;
1119
+      calculate_trapezoid_for_block(next, next_entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
1120
+      #if ENABLED(LIN_ADVANCE)
1121
+        if (next->use_advance_lead) {
1122
+          const float comp = next->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
1123
+          next->max_adv_steps = next_nominal_speed * comp;
1124
+          next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp;
1125
+        }
1126
+      #endif
1127
+    }
1083
 
1128
 
1084
     // Reset next only to ensure its trapezoid is computed - The stepper is free to use
1129
     // Reset next only to ensure its trapezoid is computed - The stepper is free to use
1085
     // the block from now on.
1130
     // the block from now on.
1423
   if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
1468
   if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
1424
 
1469
 
1425
   // Drop all queue entries
1470
   // Drop all queue entries
1426
-  block_buffer_planned = block_buffer_head = block_buffer_tail;
1471
+  block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail;
1427
 
1472
 
1428
   // Restart the block delay for the first movement - As the queue was
1473
   // Restart the block delay for the first movement - As the queue was
1429
   // forced to empty, there's no risk the ISR will touch this.
1474
   // forced to empty, there's no risk the ISR will touch this.
1906
   // Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
1951
   // Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
1907
   float inverse_secs = fr_mm_s * inverse_millimeters;
1952
   float inverse_secs = fr_mm_s * inverse_millimeters;
1908
 
1953
 
1909
-  const uint8_t moves_queued = movesplanned();
1954
+  // Get the number of non busy movements in queue (non busy means that they can be altered)
1955
+  const uint8_t moves_queued = nonbusy_movesplanned();
1910
 
1956
 
1911
   // Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
1957
   // Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
1912
   #if ENABLED(SLOWDOWN) || ENABLED(ULTRA_LCD) || defined(XY_FREQUENCY_LIMIT)
1958
   #if ENABLED(SLOWDOWN) || ENABLED(ULTRA_LCD) || defined(XY_FREQUENCY_LIMIT)

+ 21
- 20
Marlin/src/module/planner.h View File

54
   // from a safe speed (in consideration of jerking from zero speed).
54
   // from a safe speed (in consideration of jerking from zero speed).
55
   BLOCK_BIT_NOMINAL_LENGTH,
55
   BLOCK_BIT_NOMINAL_LENGTH,
56
 
56
 
57
-  // The block is busy, being interpreted by the stepper ISR
58
-  BLOCK_BIT_BUSY,
59
-
60
   // The block is segment 2+ of a longer move
57
   // The block is segment 2+ of a longer move
61
   BLOCK_BIT_CONTINUED,
58
   BLOCK_BIT_CONTINUED,
62
 
59
 
67
 enum BlockFlag : char {
64
 enum BlockFlag : char {
68
   BLOCK_FLAG_RECALCULATE          = _BV(BLOCK_BIT_RECALCULATE),
65
   BLOCK_FLAG_RECALCULATE          = _BV(BLOCK_BIT_RECALCULATE),
69
   BLOCK_FLAG_NOMINAL_LENGTH       = _BV(BLOCK_BIT_NOMINAL_LENGTH),
66
   BLOCK_FLAG_NOMINAL_LENGTH       = _BV(BLOCK_BIT_NOMINAL_LENGTH),
70
-  BLOCK_FLAG_BUSY                 = _BV(BLOCK_BIT_BUSY),
71
   BLOCK_FLAG_CONTINUED            = _BV(BLOCK_BIT_CONTINUED),
67
   BLOCK_FLAG_CONTINUED            = _BV(BLOCK_BIT_CONTINUED),
72
   BLOCK_FLAG_SYNC_POSITION        = _BV(BLOCK_BIT_SYNC_POSITION)
68
   BLOCK_FLAG_SYNC_POSITION        = _BV(BLOCK_BIT_SYNC_POSITION)
73
 };
69
 };
83
  */
79
  */
84
 typedef struct {
80
 typedef struct {
85
 
81
 
86
-  uint8_t flag;                             // Block flags (See BlockFlag enum above)
82
+  volatile uint8_t flag;                    // Block flags (See BlockFlag enum above) - Modified by ISR and main thread!
87
 
83
 
88
   // Fields used by the motion planner to manage acceleration
84
   // Fields used by the motion planner to manage acceleration
89
   float nominal_speed_sqr,                  // The nominal speed for this block in (mm/sec)^2
85
   float nominal_speed_sqr,                  // The nominal speed for this block in (mm/sec)^2
175
      */
171
      */
176
     static block_t block_buffer[BLOCK_BUFFER_SIZE];
172
     static block_t block_buffer[BLOCK_BUFFER_SIZE];
177
     static volatile uint8_t block_buffer_head,      // Index of the next block to be pushed
173
     static volatile uint8_t block_buffer_head,      // Index of the next block to be pushed
174
+                            block_buffer_nonbusy,   // Index of the first non busy block
175
+                            block_buffer_planned,   // Index of the optimally planned block
178
                             block_buffer_tail;      // Index of the busy block, if any
176
                             block_buffer_tail;      // Index of the busy block, if any
179
     static uint16_t cleaning_buffer_counter;        // A counter to disable queuing of blocks
177
     static uint16_t cleaning_buffer_counter;        // A counter to disable queuing of blocks
180
-    static uint8_t delay_before_delivering,         // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
181
-                   block_buffer_planned;            // Index of the optimally planned block
178
+    static uint8_t delay_before_delivering;         // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
179
+
182
 
180
 
183
     #if ENABLED(DISTINCT_E_FACTORS)
181
     #if ENABLED(DISTINCT_E_FACTORS)
184
       static uint8_t last_extruder;                 // Respond to extruder change
182
       static uint8_t last_extruder;                 // Respond to extruder change
443
       #define ARG_Z const float &rz
441
       #define ARG_Z const float &rz
444
     #endif
442
     #endif
445
 
443
 
446
-    // Number of moves currently in the planner
444
+    // Number of moves currently in the planner including the busy block, if any
447
     FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); }
445
     FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); }
448
 
446
 
447
+    // Number of nonbusy moves currently in the planner
448
+    FORCE_INLINE static uint8_t nonbusy_movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_nonbusy); }
449
+
449
     // Remove all blocks from the buffer
450
     // Remove all blocks from the buffer
450
-    FORCE_INLINE static void clear_block_buffer() { block_buffer_head = block_buffer_tail = 0; }
451
+    FORCE_INLINE static void clear_block_buffer() { block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail = 0; }
451
 
452
 
452
     // Check if movement queue is full
453
     // Check if movement queue is full
453
     FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }
454
     FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }
649
     static block_t* get_current_block() {
650
     static block_t* get_current_block() {
650
 
651
 
651
       // Get the number of moves in the planner queue so far
652
       // Get the number of moves in the planner queue so far
652
-      uint8_t nr_moves = movesplanned();
653
+      const uint8_t nr_moves = movesplanned();
653
 
654
 
654
       // If there are any moves queued ...
655
       // If there are any moves queued ...
655
       if (nr_moves) {
656
       if (nr_moves) {
673
           block_buffer_runtime_us -= block->segment_time_us; // We can't be sure how long an active block will take, so don't count it.
674
           block_buffer_runtime_us -= block->segment_time_us; // We can't be sure how long an active block will take, so don't count it.
674
         #endif
675
         #endif
675
 
676
 
676
-        // Mark the block as busy, so the planner does not attempt to replan it
677
-        SBI(block->flag, BLOCK_BIT_BUSY);
677
+        // As this block is busy, advance the nonbusy block pointer
678
+        block_buffer_nonbusy = next_block_index(block_buffer_tail);
679
+
680
+        // Push block_buffer_planned pointer, if encountered.
681
+        if (block_buffer_tail == block_buffer_planned)
682
+          block_buffer_planned = block_buffer_nonbusy;
683
+
684
+        // Return the block
678
         return block;
685
         return block;
679
       }
686
       }
680
 
687
 
692
      * NB: There MUST be a current block to call this function!!
699
      * NB: There MUST be a current block to call this function!!
693
      */
700
      */
694
     FORCE_INLINE static void discard_current_block() {
701
     FORCE_INLINE static void discard_current_block() {
695
-      if (has_blocks_queued()) { // Discard non-empty buffer.
696
-        uint8_t block_index = next_block_index( block_buffer_tail );
697
-
698
-        // Push block_buffer_planned pointer, if encountered.
699
-        if (!has_blocks_queued()) block_buffer_planned = block_index;
700
-
701
-        block_buffer_tail = block_index;
702
-      }
702
+      if (has_blocks_queued())
703
+        block_buffer_tail = next_block_index(block_buffer_tail);
703
     }
704
     }
704
 
705
 
705
     #if ENABLED(ULTRA_LCD)
706
     #if ENABLED(ULTRA_LCD)

+ 31
- 2
Marlin/src/module/stepper.cpp View File

107
 
107
 
108
 // public:
108
 // public:
109
 
109
 
110
-block_t* Stepper::current_block = NULL;  // A pointer to the block currently being traced
111
-
112
 #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
110
 #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
113
   bool Stepper::homing_dual_axis = false;
111
   bool Stepper::homing_dual_axis = false;
114
 #endif
112
 #endif
119
 
117
 
120
 // private:
118
 // private:
121
 
119
 
120
+block_t* Stepper::current_block = NULL; // A pointer to the block currently being traced
121
+
122
 uint8_t Stepper::last_direction_bits = 0,
122
 uint8_t Stepper::last_direction_bits = 0,
123
         Stepper::axis_did_move;
123
         Stepper::axis_did_move;
124
 
124
 
1665
       acceleration_time = deceleration_time = 0;
1665
       acceleration_time = deceleration_time = 0;
1666
 
1666
 
1667
       uint8_t oversampling = 0;                         // Assume we won't use it
1667
       uint8_t oversampling = 0;                         // Assume we won't use it
1668
+
1668
       #if ENABLED(ADAPTIVE_STEP_SMOOTHING)
1669
       #if ENABLED(ADAPTIVE_STEP_SMOOTHING)
1669
         // At this point, we must decide if we can use Stepper movement axis smoothing.
1670
         // At this point, we must decide if we can use Stepper movement axis smoothing.
1670
         uint32_t max_rate = current_block->nominal_rate;  // Get the maximum rate (maximum event speed)
1671
         uint32_t max_rate = current_block->nominal_rate;  // Get the maximum rate (maximum event speed)
1874
   }
1875
   }
1875
 #endif // LIN_ADVANCE
1876
 #endif // LIN_ADVANCE
1876
 
1877
 
1878
+// Check if the given block is busy or not - Must not be called from ISR contexts
1879
+// The current_block could change in the middle of the read by an Stepper ISR, so
1880
+// we must explicitly prevent that!
1881
+bool Stepper::is_block_busy(const block_t* const block) {
1882
+  #ifdef __AVR__
1883
+    // A SW memory barrier, to ensure GCC does not overoptimize loops
1884
+    #define sw_barrier() asm volatile("": : :"memory");
1885
+
1886
+    // Keep reading until 2 consecutive reads return the same value,
1887
+    // meaning there was no update in-between caused by an interrupt.
1888
+    // This works because stepper ISRs happen at a slower rate than
1889
+    // successive reads of a variable, so 2 consecutive reads with
1890
+    // the same value means no interrupt updated it.
1891
+    block_t* vold, *vnew = current_block;
1892
+    sw_barrier();
1893
+    do {
1894
+      vold = vnew;
1895
+      vnew = current_block;
1896
+      sw_barrier();
1897
+    } while (vold != vnew);
1898
+  #else
1899
+    block_t *vnew = current_block;
1900
+  #endif
1901
+
1902
+  // Return if the block is busy or not
1903
+  return block == vnew;
1904
+}
1905
+
1877
 void Stepper::init() {
1906
 void Stepper::init() {
1878
 
1907
 
1879
   // Init Digipot Motor Current
1908
   // Init Digipot Motor Current

+ 5
- 2
Marlin/src/module/stepper.h View File

234
 
234
 
235
   public:
235
   public:
236
 
236
 
237
-    static block_t* current_block;  // A pointer to the block currently being traced
238
-
239
     #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
237
     #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
240
       static bool homing_dual_axis;
238
       static bool homing_dual_axis;
241
     #endif
239
     #endif
249
 
247
 
250
   private:
248
   private:
251
 
249
 
250
+    static block_t* current_block;          // A pointer to the block currently being traced
251
+
252
     static uint8_t last_direction_bits,     // The next stepping-bits to be output
252
     static uint8_t last_direction_bits,     // The next stepping-bits to be output
253
                    axis_did_move;           // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner
253
                    axis_did_move;           // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner
254
 
254
 
360
       static uint32_t advance_isr();
360
       static uint32_t advance_isr();
361
     #endif
361
     #endif
362
 
362
 
363
+    // Check if the given block is busy or not - Must not be called from ISR contexts
364
+    static bool is_block_busy(const block_t* const block);
365
+
363
     // Get the position of a stepper, in steps
366
     // Get the position of a stepper, in steps
364
     static int32_t position(const AxisEnum axis);
367
     static int32_t position(const AxisEnum axis);
365
 
368
 

Loading…
Cancel
Save