|
@@ -1204,22 +1204,17 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
|
1204
|
1204
|
static float previous_safe_speed;
|
1205
|
1205
|
|
1206
|
1206
|
float safe_speed = block->nominal_speed;
|
1207
|
|
- bool limited = false;
|
|
1207
|
+ uint8_t limited = 0;
|
1208
|
1208
|
LOOP_XYZE(i) {
|
1209
|
|
- float jerk = fabs(current_speed[i]);
|
1210
|
|
- if (jerk > max_jerk[i]) {
|
1211
|
|
- // The actual jerk is lower if it has been limited by the XY jerk.
|
|
1209
|
+ const float jerk = fabs(current_speed[i]), maxj = max_jerk[i];
|
|
1210
|
+ if (jerk > maxj) {
|
1212
|
1211
|
if (limited) {
|
1213
|
|
- // Spare one division by a following gymnastics:
|
1214
|
|
- // Instead of jerk *= safe_speed / block->nominal_speed,
|
1215
|
|
- // multiply max_jerk[i] by the divisor.
|
1216
|
|
- jerk *= safe_speed;
|
1217
|
|
- float mjerk = max_jerk[i] * block->nominal_speed;
|
1218
|
|
- if (jerk > mjerk) safe_speed *= mjerk / jerk;
|
|
1212
|
+ const float mjerk = maxj * block->nominal_speed;
|
|
1213
|
+ if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk;
|
1219
|
1214
|
}
|
1220
|
1215
|
else {
|
1221
|
|
- safe_speed = max_jerk[i];
|
1222
|
|
- limited = true;
|
|
1216
|
+ ++limited;
|
|
1217
|
+ safe_speed = maxj;
|
1223
|
1218
|
}
|
1224
|
1219
|
}
|
1225
|
1220
|
}
|
|
@@ -1236,7 +1231,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
|
1236
|
1231
|
vmax_junction = prev_speed_larger ? block->nominal_speed : previous_nominal_speed;
|
1237
|
1232
|
// Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
|
1238
|
1233
|
float v_factor = 1.f;
|
1239
|
|
- limited = false;
|
|
1234
|
+ limited = 0;
|
1240
|
1235
|
// Now limit the jerk in all axes.
|
1241
|
1236
|
LOOP_XYZE(axis) {
|
1242
|
1237
|
// Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
|