|
@@ -258,7 +258,7 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
|
258
|
258
|
|
259
|
259
|
|
260
|
260
|
// The kernel called by recalculate() when scanning the plan from last to first entry.
|
261
|
|
-void Planner::reverse_pass_kernel(block_t* const current, const block_t *next) {
|
|
261
|
+void Planner::reverse_pass_kernel(block_t* const current, const block_t * const next) {
|
262
|
262
|
if (!current || !next) return;
|
263
|
263
|
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
|
264
|
264
|
// If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
|
|
@@ -279,31 +279,25 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t *next) {
|
279
|
279
|
* Once in reverse and once forward. This implements the reverse pass.
|
280
|
280
|
*/
|
281
|
281
|
void Planner::reverse_pass() {
|
282
|
|
-
|
283
|
282
|
if (movesplanned() > 3) {
|
284
|
|
-
|
285
|
|
- block_t* block[3] = { NULL, NULL, NULL };
|
286
|
|
-
|
287
|
|
- // Make a local copy of block_buffer_tail, because the interrupt can alter it
|
288
|
|
- // Is a critical section REALLY needed for a single byte change?
|
289
|
|
- //CRITICAL_SECTION_START;
|
290
|
|
- uint8_t tail = block_buffer_tail;
|
291
|
|
- //CRITICAL_SECTION_END
|
292
|
|
-
|
293
|
|
- uint8_t b = BLOCK_MOD(block_buffer_head - 3);
|
294
|
|
- while (b != tail) {
|
295
|
|
- if (block[0] && TEST(block[0]->flag, BLOCK_BIT_START_FROM_FULL_HALT)) break;
|
296
|
|
- b = prev_block_index(b);
|
297
|
|
- block[2] = block[1];
|
298
|
|
- block[1] = block[0];
|
299
|
|
- block[0] = &block_buffer[b];
|
300
|
|
- reverse_pass_kernel(block[1], block[2]);
|
301
|
|
- }
|
|
283
|
+ const uint8_t endnr = BLOCK_MOD(block_buffer_tail + 2); // tail is running. tail+1 shouldn't be altered because it's connected to the running block.
|
|
284
|
+ // tail+2 because the index is not yet advanced when checked
|
|
285
|
+ uint8_t blocknr = prev_block_index(block_buffer_head);
|
|
286
|
+ block_t* current = &block_buffer[blocknr];
|
|
287
|
+
|
|
288
|
+ do {
|
|
289
|
+ const block_t * const next = current;
|
|
290
|
+ blocknr = prev_block_index(blocknr);
|
|
291
|
+ current = &block_buffer[blocknr];
|
|
292
|
+ if (TEST(current->flag, BLOCK_BIT_START_FROM_FULL_HALT)) // Up to this every block is already optimized.
|
|
293
|
+ break;
|
|
294
|
+ reverse_pass_kernel(current, next);
|
|
295
|
+ } while (blocknr != endnr);
|
302
|
296
|
}
|
303
|
297
|
}
|
304
|
298
|
|
305
|
299
|
// The kernel called by recalculate() when scanning the plan from first to last entry.
|
306
|
|
-void Planner::forward_pass_kernel(const block_t* previous, block_t* const current) {
|
|
300
|
+void Planner::forward_pass_kernel(const block_t * const previous, block_t* const current) {
|
307
|
301
|
if (!previous) return;
|
308
|
302
|
|
309
|
303
|
// If the previous block is an acceleration block, but it is not long enough to complete the
|
|
@@ -355,8 +349,8 @@ void Planner::recalculate_trapezoids() {
|
355
|
349
|
// Recalculate if current block entry or exit junction speed has changed.
|
356
|
350
|
if (TEST(current->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE)) {
|
357
|
351
|
// NOTE: Entry and exit factors always > 0 by all previous logic operations.
|
358
|
|
- float nom = current->nominal_speed;
|
359
|
|
- calculate_trapezoid_for_block(current, current->entry_speed / nom, next->entry_speed / nom);
|
|
352
|
+ const float nomr = 1.0 / current->nominal_speed;
|
|
353
|
+ calculate_trapezoid_for_block(current, current->entry_speed * nomr, next->entry_speed * nomr);
|
360
|
354
|
CBI(current->flag, BLOCK_BIT_RECALCULATE); // Reset current only to ensure next trapezoid is computed
|
361
|
355
|
}
|
362
|
356
|
}
|
|
@@ -364,8 +358,8 @@ void Planner::recalculate_trapezoids() {
|
364
|
358
|
}
|
365
|
359
|
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
|
366
|
360
|
if (next) {
|
367
|
|
- float nom = next->nominal_speed;
|
368
|
|
- calculate_trapezoid_for_block(next, next->entry_speed / nom, (MINIMUM_PLANNER_SPEED) / nom);
|
|
361
|
+ const float nomr = 1.0 / next->nominal_speed;
|
|
362
|
+ calculate_trapezoid_for_block(next, next->entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
|
369
|
363
|
CBI(next->flag, BLOCK_BIT_RECALCULATE);
|
370
|
364
|
}
|
371
|
365
|
}
|
|
@@ -1020,7 +1014,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
1020
|
1014
|
#endif
|
1021
|
1015
|
);
|
1022
|
1016
|
}
|
1023
|
|
- float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
|
|
1017
|
+ const float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
|
1024
|
1018
|
|
1025
|
1019
|
// Calculate inverse time for this move. No divide by zero due to previous checks.
|
1026
|
1020
|
// Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
|
|
@@ -1059,7 +1053,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
1059
|
1053
|
//FMM update ring buffer used for delay with filament measurements
|
1060
|
1054
|
if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && filwidth_delay_index[1] >= 0) { //only for extruder with filament sensor and if ring buffer is initialized
|
1061
|
1055
|
|
1062
|
|
- const int MMD_CM = MAX_MEASUREMENT_DELAY + 1, MMD_MM = MMD_CM * 10;
|
|
1056
|
+ constexpr int MMD_CM = MAX_MEASUREMENT_DELAY + 1, MMD_MM = MMD_CM * 10;
|
1063
|
1057
|
|
1064
|
1058
|
// increment counters with next move in e axis
|
1065
|
1059
|
filwidth_e_count += delta_mm[E_AXIS];
|
|
@@ -1356,13 +1350,14 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
1356
|
1350
|
|
1357
|
1351
|
#endif // LIN_ADVANCE
|
1358
|
1352
|
|
1359
|
|
- calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed);
|
|
1353
|
+ const float bnsr = 1.0 / block->nominal_speed;
|
|
1354
|
+ calculate_trapezoid_for_block(block, block->entry_speed * bnsr, safe_speed * bnsr);
|
1360
|
1355
|
|
1361
|
1356
|
// Move buffer head
|
1362
|
1357
|
block_buffer_head = next_buffer_head;
|
1363
|
1358
|
|
1364
|
1359
|
// Update the position (only when a move was queued)
|
1365
|
|
- static_assert(COUNT(target) > 1, "array as function parameter should be declared as reference and with count");
|
|
1360
|
+ static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!");
|
1366
|
1361
|
COPY(position, target);
|
1367
|
1362
|
|
1368
|
1363
|
recalculate();
|