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

Improve the distribution of the advance steps

This commit is contained in:
Yuri D'Elia 2019-05-21 21:32:38 +02:00
parent 294bf4068d
commit 282b502393
1 changed files with 71 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
Firmware/stepper.cpp
View File

@ -124,8 +124,9 @@ volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1};
static uint16_t main_Rate; static uint16_t main_Rate;
static uint16_t eISR_Rate; static uint16_t eISR_Rate;
static uint16_t eISR_Err;
static volatile uint16_t current_adv_steps; static uint16_t current_adv_steps;
static uint16_t final_adv_steps; static uint16_t final_adv_steps;
static uint16_t max_adv_steps; static uint16_t max_adv_steps;
static uint32_t LA_decelerate_after; static uint32_t LA_decelerate_after;
@ -355,14 +356,13 @@ FORCE_INLINE void stepper_next_block()
final_adv_steps = current_block->final_adv_steps; final_adv_steps = current_block->final_adv_steps;
max_adv_steps = current_block->max_adv_steps; max_adv_steps = current_block->max_adv_steps;
e_step_loops = current_block->advance_step_loops; e_step_loops = current_block->advance_step_loops;
LA_phase = -1;
} else { } else {
nextAdvanceISR = ADV_NEVER; e_steps = 0;
eISR_Rate = ADV_NEVER;
e_step_loops = 1; e_step_loops = 1;
LA_phase = -1;
current_adv_steps = 0; current_adv_steps = 0;
} }
nextAdvanceISR = ADV_NEVER;
LA_phase = -1;
#endif #endif
if (current_block->flag & BLOCK_FLAG_DDA_LOWRES) { if (current_block->flag & BLOCK_FLAG_DDA_LOWRES) {
@ -707,6 +707,39 @@ FORCE_INLINE void stepper_tick_highres()
} }
} }
#ifdef LIN_ADVANCE
FORCE_INLINE void advance_spread(uint16_t timer)
{
if(eISR_Err > timer)
{
// advance-step skipped
eISR_Err -= timer;
eISR_Rate = timer;
nextAdvanceISR = timer;
return;
}
// at least one step
uint8_t ticks = 1;
uint32_t block = current_block->advance_rate;
uint16_t max_t = timer - eISR_Err;
while (block < max_t)
{
++ticks;
block += current_block->advance_rate;
}
if (block > timer)
eISR_Err += block - timer;
else
eISR_Err -= timer - block;
eISR_Rate = timer / ticks;
nextAdvanceISR = eISR_Rate / 2;
}
#endif
FORCE_INLINE void isr() { FORCE_INLINE void isr() {
//WRITE_NC(LOGIC_ANALYZER_CH0, true); //WRITE_NC(LOGIC_ANALYZER_CH0, true);
@ -741,11 +774,10 @@ FORCE_INLINE void isr() {
acceleration_time += timer; acceleration_time += timer;
#ifdef LIN_ADVANCE #ifdef LIN_ADVANCE
if (current_block->use_advance_lead) { if (current_block->use_advance_lead) {
if (step_events_completed.wide <= (unsigned long int)step_loops) { bool first = (step_events_completed.wide <= (unsigned long int)step_loops);
// First acceleration loop if (first) eISR_Err = current_block->advance_rate / 2;
eISR_Rate = current_block->advance_rate; if (first || nextAdvanceISR != ADV_NEVER)
nextAdvanceISR = 0; advance_spread(timer);
}
} }
#endif #endif
} }
@ -763,10 +795,20 @@ FORCE_INLINE void isr() {
deceleration_time += timer; deceleration_time += timer;
#ifdef LIN_ADVANCE #ifdef LIN_ADVANCE
if (current_block->use_advance_lead) { if (current_block->use_advance_lead) {
if (step_events_completed.wide <= (unsigned long int)current_block->decelerate_after + step_loops) { bool first = (step_events_completed.wide <= (unsigned long int)current_block->decelerate_after + step_loops);
// First deceleration loop if (first) eISR_Err = current_block->advance_rate / 2;
eISR_Rate = current_block->advance_rate; if (first || nextAdvanceISR != ADV_NEVER)
nextAdvanceISR = 0; {
advance_spread(timer);
if (step_loops == e_step_loops)
LA_phase = (eISR_Rate > main_Rate);
else
{
// avoid overflow through division (TODO: this can be
// improved as both step_loops and e_step_loops are
// currently guaranteed to be powers of two)
LA_phase = (eISR_Rate / step_loops > main_Rate / e_step_loops);
}
} }
} }
#endif #endif
@ -779,6 +821,11 @@ FORCE_INLINE void isr() {
step_loops_nominal = step_loops; step_loops_nominal = step_loops;
} }
_NEXT_ISR(OCR1A_nominal); _NEXT_ISR(OCR1A_nominal);
#ifdef LIN_ADVANCE
if (current_block->use_advance_lead && nextAdvanceISR != ADV_NEVER) {
advance_spread(OCR1A_nominal);
}
#endif
} }
//WRITE_NC(LOGIC_ANALYZER_CH1, false); //WRITE_NC(LOGIC_ANALYZER_CH1, false);
} }
@ -816,48 +863,26 @@ FORCE_INLINE void advance_isr() {
if (step_events_completed.wide > LA_decelerate_after && current_adv_steps > final_adv_steps) { if (step_events_completed.wide > LA_decelerate_after && current_adv_steps > final_adv_steps) {
// decompression // decompression
e_steps -= e_step_loops; e_steps -= e_step_loops;
current_adv_steps -= e_step_loops; if(current_adv_steps > e_step_loops)
nextAdvanceISR = eISR_Rate; current_adv_steps -= e_step_loops;
if(nextAdvanceISR == ADV_NEVER)
{
LA_phase = 1;
e_step_loops = 1;
}
else else
{ current_adv_steps = 0;
if (step_loops == e_step_loops) nextAdvanceISR = eISR_Rate;
LA_phase = (eISR_Rate > main_Rate);
else
{
// avoid overflow through division (TODO: this can be
// improved as both step_loops and e_step_loops are
// guaranteed to be powers of two)
LA_phase = (eISR_Rate / step_loops > main_Rate / e_step_loops);
}
}
} }
else if (step_events_completed.wide < LA_decelerate_after && current_adv_steps < max_adv_steps) { else if (step_events_completed.wide < LA_decelerate_after && current_adv_steps < max_adv_steps) {
// compression // compression
e_steps += e_step_loops; e_steps += e_step_loops;
current_adv_steps += e_step_loops; current_adv_steps += e_step_loops;
nextAdvanceISR = eISR_Rate; nextAdvanceISR = eISR_Rate;
LA_phase = -1;
if(nextAdvanceISR == ADV_NEVER)
e_step_loops = 1;
} }
else { else {
// advance steps completed // advance steps completed
nextAdvanceISR = ADV_NEVER; nextAdvanceISR = ADV_NEVER;
eISR_Rate = ADV_NEVER;
LA_phase = -1; LA_phase = -1;
e_step_loops = 1; e_step_loops = 1;
} }
} }
#define LA_FREQ_MDIV 8 // divider for the advance frequency for maximum
// time allotted to merge regular and advance
// ticks (stick to a power-of-two)
FORCE_INLINE void advance_isr_scheduler() { FORCE_INLINE void advance_isr_scheduler() {
// Integrate the final timer value, accounting for scheduling adjustments // Integrate the final timer value, accounting for scheduling adjustments
if(nextAdvanceISR && nextAdvanceISR != ADV_NEVER) if(nextAdvanceISR && nextAdvanceISR != ADV_NEVER)
@ -884,8 +909,8 @@ FORCE_INLINE void advance_isr_scheduler() {
#endif #endif
} }
// Run the next advance isr if triggered now or soon enough // Run the next advance isr if triggered
bool eisr = nextAdvanceISR < (TCNT1 + eISR_Rate / LA_FREQ_MDIV); bool eisr = !nextAdvanceISR;
if (eisr) if (eisr)
{ {
#ifdef LA_DEBUG_LOGIC #ifdef LA_DEBUG_LOGIC
@ -899,23 +924,24 @@ FORCE_INLINE void advance_isr_scheduler() {
// Tick E steps if any // Tick E steps if any
if (e_steps && (LA_phase < 0 || LA_phase == eisr)) { if (e_steps && (LA_phase < 0 || LA_phase == eisr)) {
uint8_t max_ticks = max(e_step_loops, step_loops); uint8_t max_ticks = (eisr? e_step_loops: step_loops);
max_ticks = min(abs(e_steps), max_ticks); max_ticks = min(abs(e_steps), max_ticks);
#ifdef FILAMENT_SENSOR #ifdef FILAMENT_SENSOR
fsensor_counter += max_ticks; fsensor_counter += max_ticks;
#endif #endif
WRITE(E0_DIR_PIN, e_steps < 0? INVERT_E0_DIR: !INVERT_E0_DIR); WRITE(E0_DIR_PIN, e_steps < 0? INVERT_E0_DIR: !INVERT_E0_DIR);
while(max_ticks--) do
{ {
WRITE_NC(E0_STEP_PIN, !INVERT_E_STEP_PIN); WRITE_NC(E0_STEP_PIN, !INVERT_E_STEP_PIN);
e_steps += (e_steps < 0)? 1: -1; e_steps += (e_steps < 0)? 1: -1;
WRITE_NC(E0_STEP_PIN, INVERT_E_STEP_PIN); WRITE_NC(E0_STEP_PIN, INVERT_E_STEP_PIN);
} }
while(--max_ticks);
} }
// Schedule the next closest tick, ignoring advance if scheduled too // Schedule the next closest tick, ignoring advance if scheduled too
// soon in order to avoid skewing the regular stepper acceleration // soon in order to avoid skewing the regular stepper acceleration
if (nextAdvanceISR != ADV_NEVER && (nextAdvanceISR + TCNT1 + eISR_Rate / LA_FREQ_MDIV) < nextMainISR) if (nextAdvanceISR != ADV_NEVER && (nextAdvanceISR + TCNT1 + 40) < nextMainISR)
OCR1A = nextAdvanceISR; OCR1A = nextAdvanceISR;
else else
OCR1A = nextMainISR; OCR1A = nextMainISR;
@ -1162,7 +1188,6 @@ void st_init()
nextMainISR = 0; nextMainISR = 0;
nextAdvanceISR = ADV_NEVER; nextAdvanceISR = ADV_NEVER;
main_Rate = ADV_NEVER; main_Rate = ADV_NEVER;
eISR_Rate = ADV_NEVER;
e_steps = 0; e_steps = 0;
e_step_loops = 1; e_step_loops = 1;
LA_phase = -1; LA_phase = -1;