josh
/
Prusa-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Remove cumulative error in segments without cruising (take 3) 

Remove another division by precomputing the division directly in
adv_comp.
This commit is contained in:
Yuri D'Elia 2020-06-21 16:19:46 +02:00
parent 753e651af3
commit 7c140bc497
1 changed files with 53 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
Firmware/planner.cpp
View File

@ -225,15 +225,15 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit
uint32_t accel_decel_steps = accelerate_steps + decelerate_steps; uint32_t accel_decel_steps = accelerate_steps + decelerate_steps;
// Size of Plateau of Nominal Rate. // Size of Plateau of Nominal Rate.
uint32_t plateau_steps = 0; uint32_t plateau_steps = 0;
// Maximum effective speed reached in the trapezoid (mm/s) // Maximum effective speed reached in the trapezoid (step/min)
float max_speed; float max_rate;
// Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
// have to use intersection_distance() to calculate when to abort acceleration and start braking // have to use intersection_distance() to calculate when to abort acceleration and start braking
// in order to reach the final_rate exactly at the end of this block. // in order to reach the final_rate exactly at the end of this block.
if (accel_decel_steps < block->step_event_count.wide) { if (accel_decel_steps < block->step_event_count.wide) {
plateau_steps = block->step_event_count.wide - accel_decel_steps; plateau_steps = block->step_event_count.wide - accel_decel_steps;
max_speed = block->nominal_speed; max_rate = block->nominal_rate;
} else { } else {
uint32_t acceleration_x4 = acceleration << 2; uint32_t acceleration_x4 = acceleration << 2;
// Avoid negative numbers // Avoid negative numbers
@ -267,15 +267,15 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit
accelerate_steps = block->step_event_count.wide - decelerate_steps; accelerate_steps = block->step_event_count.wide - decelerate_steps;
} }
max_speed = sqrt(acceleration_x2 * accelerate_steps + initial_rate_sqr) / block->speed_factor; max_rate = sqrt(acceleration_x2 * accelerate_steps + initial_rate_sqr);
} }
#ifdef LIN_ADVANCE #ifdef LIN_ADVANCE
uint16_t final_adv_steps = 0; uint16_t final_adv_steps = 0;
uint16_t max_adv_steps = 0; uint16_t max_adv_steps = 0;
if (block->use_advance_lead) { if (block->use_advance_lead) {
final_adv_steps = exit_speed * block->adv_comp; final_adv_steps = final_rate * block->adv_comp;
max_adv_steps = max_speed * block->adv_comp; max_adv_steps = max_rate * block->adv_comp;
} }
#endif #endif
@ -1142,51 +1142,6 @@ Having the real displacement of the head, we can calculate the total movement le
block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0))); block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0)));
#ifdef LIN_ADVANCE
if (block->use_advance_lead) {
// the nominal speed doesn't change past this point: calculate the compression ratio for the
// segment (the required advance steps are computed during trapezoid planning)
block->adv_comp = extruder_advance_K * e_D_ratio * cs.axis_steps_per_unit[E_AXIS];
float advance_speed;
if (e_D_ratio > 0)
advance_speed = (extruder_advance_K * e_D_ratio * block->acceleration * cs.axis_steps_per_unit[E_AXIS]);
else
advance_speed = cs.max_jerk[E_AXIS] * cs.axis_steps_per_unit[E_AXIS];
// to save more space we avoid another copy of calc_timer and go through slow division, but we
// still need to replicate the *exact* same step grouping policy (see below)
if (advance_speed > MAX_STEP_FREQUENCY) advance_speed = MAX_STEP_FREQUENCY;
float advance_rate = (F_CPU / 8.0) / advance_speed;
if (advance_speed > 20000) {
block->advance_rate = advance_rate * 4;
block->advance_step_loops = 4;
}
else if (advance_speed > 10000) {
block->advance_rate = advance_rate * 2;
block->advance_step_loops = 2;
}
else
{
// never overflow the internal accumulator with very low rates
if (advance_rate < UINT16_MAX)
block->advance_rate = advance_rate;
else
block->advance_rate = UINT16_MAX;
block->advance_step_loops = 1;
}
#ifdef LA_DEBUG
if (block->advance_step_loops > 2)
// @wavexx: we should really check for the difference between step_loops and
// advance_step_loops instead. A difference of more than 1 will lead
// to uneven speed and *should* be adjusted here by furthermore
// reducing the speed.
SERIAL_ECHOLNPGM("LA: More than 2 steps per eISR loop executed.");
#endif
}
#endif
// Start with a safe speed. // Start with a safe speed.
// Safe speed is the speed, from which the machine may halt to stop immediately. // Safe speed is the speed, from which the machine may halt to stop immediately.
float safe_speed = block->nominal_speed; float safe_speed = block->nominal_speed;
@ -1312,6 +1267,53 @@ Having the real displacement of the head, we can calculate the total movement le
// Precalculate the division, so when all the trapezoids in the planner queue get recalculated, the division is not repeated. // Precalculate the division, so when all the trapezoids in the planner queue get recalculated, the division is not repeated.
block->speed_factor = block->nominal_rate / block->nominal_speed; block->speed_factor = block->nominal_rate / block->nominal_speed;
#ifdef LIN_ADVANCE
if (block->use_advance_lead) {
// calculate the compression ratio for the segment (the required advance steps are computed
// during trapezoid planning)
float adv_comp = extruder_advance_K * e_D_ratio * cs.axis_steps_per_unit[E_AXIS]; // (step/(mm/s))
block->adv_comp = adv_comp / block->speed_factor; // step/(step/min)
float advance_speed;
if (e_D_ratio > 0)
advance_speed = (extruder_advance_K * e_D_ratio * block->acceleration * cs.axis_steps_per_unit[E_AXIS]);
else
advance_speed = cs.max_jerk[E_AXIS] * cs.axis_steps_per_unit[E_AXIS];
// to save more space we avoid another copy of calc_timer and go through slow division, but we
// still need to replicate the *exact* same step grouping policy (see below)
if (advance_speed > MAX_STEP_FREQUENCY) advance_speed = MAX_STEP_FREQUENCY;
float advance_rate = (F_CPU / 8.0) / advance_speed;
if (advance_speed > 20000) {
block->advance_rate = advance_rate * 4;
block->advance_step_loops = 4;
}
else if (advance_speed > 10000) {
block->advance_rate = advance_rate * 2;
block->advance_step_loops = 2;
}
else
{
// never overflow the internal accumulator with very low rates
if (advance_rate < UINT16_MAX)
block->advance_rate = advance_rate;
else
block->advance_rate = UINT16_MAX;
block->advance_step_loops = 1;
}
#ifdef LA_DEBUG
if (block->advance_step_loops > 2)
// @wavexx: we should really check for the difference between step_loops and
// advance_step_loops instead. A difference of more than 1 will lead
// to uneven speed and *should* be adjusted here by furthermore
// reducing the speed.
SERIAL_ECHOLNPGM("LA: More than 2 steps per eISR loop executed.");
#endif
}
#endif
calculate_trapezoid_for_block(block, block->entry_speed, safe_speed); calculate_trapezoid_for_block(block, block->entry_speed, safe_speed);
if (block->step_event_count.wide <= 32767) if (block->step_event_count.wide <= 32767)