|
@@ -113,7 +113,7 @@
|
113
|
113
|
|
114
|
114
|
Planner planner;
|
115
|
115
|
|
116
|
|
- // public:
|
|
116
|
+// public:
|
117
|
117
|
|
118
|
118
|
/**
|
119
|
119
|
* A ring buffer of moves described in steps
|
|
@@ -200,10 +200,9 @@ float Planner::previous_nominal_speed_sqr;
|
200
|
200
|
#endif
|
201
|
201
|
|
202
|
202
|
#ifdef XY_FREQUENCY_LIMIT
|
203
|
|
- // Old direction bits. Used for speed calculations
|
204
|
|
- unsigned char Planner::old_direction_bits = 0;
|
205
|
|
- // Segment times (in µs). Used for speed calculations
|
206
|
|
- xy_ulong_t Planner::axis_segment_time_us[3] = { { MAX_FREQ_TIME_US + 1, MAX_FREQ_TIME_US + 1 } };
|
|
203
|
+ int8_t Planner::xy_freq_limit_hz = XY_FREQUENCY_LIMIT;
|
|
204
|
+ float Planner::xy_freq_min_speed_factor = (XY_FREQUENCY_MIN_PERCENT) * 0.01f;
|
|
205
|
+ int32_t Planner::xy_freq_min_interval_us = LROUND(1000000.0 / (XY_FREQUENCY_LIMIT));
|
207
|
206
|
#endif
|
208
|
207
|
|
209
|
208
|
#if ENABLED(LIN_ADVANCE)
|
|
@@ -2006,7 +2005,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
2006
|
2005
|
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
|
2007
|
2006
|
#if EITHER(SLOWDOWN, ULTRA_LCD) || defined(XY_FREQUENCY_LIMIT)
|
2008
|
2007
|
// Segment time im micro seconds
|
2009
|
|
- uint32_t segment_time_us = LROUND(1000000.0f / inverse_secs);
|
|
2008
|
+ int32_t segment_time_us = LROUND(1000000.0f / inverse_secs);
|
2010
|
2009
|
#endif
|
2011
|
2010
|
|
2012
|
2011
|
#if ENABLED(SLOWDOWN)
|
|
@@ -2014,9 +2013,10 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
2014
|
2013
|
#define SLOWDOWN_DIVISOR 2
|
2015
|
2014
|
#endif
|
2016
|
2015
|
if (WITHIN(moves_queued, 2, (BLOCK_BUFFER_SIZE) / (SLOWDOWN_DIVISOR) - 1)) {
|
2017
|
|
- if (segment_time_us < settings.min_segment_time_us) {
|
2018
|
|
- // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
|
2019
|
|
- const uint32_t nst = segment_time_us + LROUND(2 * (settings.min_segment_time_us - segment_time_us) / moves_queued);
|
|
2016
|
+ const int32_t time_diff = settings.min_segment_time_us - segment_time_us;
|
|
2017
|
+ if (time_diff > 0) {
|
|
2018
|
+ // Buffer is draining so add extra time. The amount of time added increases if the buffer is still emptied more.
|
|
2019
|
+ const int32_t nst = segment_time_us + LROUND(2 * time_diff / moves_queued);
|
2020
|
2020
|
inverse_secs = 1000000.0f / nst;
|
2021
|
2021
|
#if defined(XY_FREQUENCY_LIMIT) || HAS_SPI_LCD
|
2022
|
2022
|
segment_time_us = nst;
|
|
@@ -2072,42 +2072,36 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
2072
|
2072
|
}
|
2073
|
2073
|
#endif
|
2074
|
2074
|
|
2075
|
|
- // Max segment time in µs.
|
2076
|
2075
|
#ifdef XY_FREQUENCY_LIMIT
|
2077
|
2076
|
|
2078
|
|
- // Check and limit the xy direction change frequency
|
2079
|
|
- const unsigned char direction_change = block->direction_bits ^ old_direction_bits;
|
2080
|
|
- old_direction_bits = block->direction_bits;
|
2081
|
|
- segment_time_us = LROUND((float)segment_time_us / speed_factor);
|
2082
|
|
-
|
2083
|
|
- uint32_t xs0 = axis_segment_time_us[0].x,
|
2084
|
|
- xs1 = axis_segment_time_us[1].x,
|
2085
|
|
- xs2 = axis_segment_time_us[2].x,
|
2086
|
|
- ys0 = axis_segment_time_us[0].y,
|
2087
|
|
- ys1 = axis_segment_time_us[1].y,
|
2088
|
|
- ys2 = axis_segment_time_us[2].y;
|
2089
|
|
-
|
2090
|
|
- if (TEST(direction_change, X_AXIS)) {
|
2091
|
|
- xs2 = axis_segment_time_us[2].x = xs1;
|
2092
|
|
- xs1 = axis_segment_time_us[1].x = xs0;
|
2093
|
|
- xs0 = 0;
|
2094
|
|
- }
|
2095
|
|
- xs0 = axis_segment_time_us[0].x = xs0 + segment_time_us;
|
|
2077
|
+ static uint8_t old_direction_bits; // = 0
|
2096
|
2078
|
|
2097
|
|
- if (TEST(direction_change, Y_AXIS)) {
|
2098
|
|
- ys2 = axis_segment_time_us[2].y = axis_segment_time_us[1].y;
|
2099
|
|
- ys1 = axis_segment_time_us[1].y = axis_segment_time_us[0].y;
|
2100
|
|
- ys0 = 0;
|
2101
|
|
- }
|
2102
|
|
- ys0 = axis_segment_time_us[0].y = ys0 + segment_time_us;
|
2103
|
|
-
|
2104
|
|
- const uint32_t max_x_segment_time = _MAX(xs0, xs1, xs2),
|
2105
|
|
- max_y_segment_time = _MAX(ys0, ys1, ys2),
|
2106
|
|
- min_xy_segment_time = _MIN(max_x_segment_time, max_y_segment_time);
|
2107
|
|
- if (min_xy_segment_time < MAX_FREQ_TIME_US) {
|
2108
|
|
- const float low_sf = speed_factor * min_xy_segment_time / (MAX_FREQ_TIME_US);
|
2109
|
|
- NOMORE(speed_factor, low_sf);
|
|
2079
|
+ if (xy_freq_limit_hz) {
|
|
2080
|
+ // Check and limit the xy direction change frequency
|
|
2081
|
+ const uint8_t direction_change = block->direction_bits ^ old_direction_bits;
|
|
2082
|
+ old_direction_bits = block->direction_bits;
|
|
2083
|
+ segment_time_us = LROUND(float(segment_time_us) / speed_factor);
|
|
2084
|
+
|
|
2085
|
+ static int32_t xs0, xs1, xs2, ys0, ys1, ys2;
|
|
2086
|
+ if (segment_time_us > xy_freq_min_interval_us)
|
|
2087
|
+ xs2 = xs1 = ys2 = ys1 = xy_freq_min_interval_us;
|
|
2088
|
+ else {
|
|
2089
|
+ xs2 = xs1; xs1 = xs0;
|
|
2090
|
+ ys2 = ys1; ys1 = ys0;
|
|
2091
|
+ }
|
|
2092
|
+ xs0 = TEST(direction_change, X_AXIS) ? segment_time_us : xy_freq_min_interval_us;
|
|
2093
|
+ ys0 = TEST(direction_change, Y_AXIS) ? segment_time_us : xy_freq_min_interval_us;
|
|
2094
|
+
|
|
2095
|
+ if (segment_time_us < xy_freq_min_interval_us) {
|
|
2096
|
+ const int32_t least_xy_segment_time = _MIN(_MAX(xs0, xs1, xs2), _MAX(ys0, ys1, ys2));
|
|
2097
|
+ if (least_xy_segment_time < xy_freq_min_interval_us) {
|
|
2098
|
+ float freq_xy_feedrate = (speed_factor * least_xy_segment_time) / xy_freq_min_interval_us;
|
|
2099
|
+ NOLESS(freq_xy_feedrate, xy_freq_min_speed_factor);
|
|
2100
|
+ NOMORE(speed_factor, freq_xy_feedrate);
|
|
2101
|
+ }
|
|
2102
|
+ }
|
2110
|
2103
|
}
|
|
2104
|
+
|
2111
|
2105
|
#endif // XY_FREQUENCY_LIMIT
|
2112
|
2106
|
|
2113
|
2107
|
// Correct the speed
|
|
@@ -2832,7 +2826,7 @@ void Planner::set_max_jerk(const AxisEnum axis, float targetValue) {
|
2832
|
2826
|
const bool was_enabled = stepper.suspend();
|
2833
|
2827
|
#endif
|
2834
|
2828
|
|
2835
|
|
- millis_t bbru = block_buffer_runtime_us;
|
|
2829
|
+ uint32_t bbru = block_buffer_runtime_us;
|
2836
|
2830
|
|
2837
|
2831
|
#ifdef __AVR__
|
2838
|
2832
|
// Reenable Stepper ISR
|
|
@@ -2844,7 +2838,7 @@ void Planner::set_max_jerk(const AxisEnum axis, float targetValue) {
|
2844
|
2838
|
// Doesn't matter because block_buffer_runtime_us is already too small an estimation.
|
2845
|
2839
|
bbru >>= 10;
|
2846
|
2840
|
// limit to about a minute.
|
2847
|
|
- NOMORE(bbru, 0xFFFFul);
|
|
2841
|
+ NOMORE(bbru, 0x0000FFFFUL);
|
2848
|
2842
|
return bbru;
|
2849
|
2843
|
}
|
2850
|
2844
|
|