From 12be141188c3511e13978d366164c3afcb0c04b8 Mon Sep 17 00:00:00 2001 From: Alex Voinea Date: Sat, 6 Jun 2020 19:32:48 +0300 Subject: [PATCH 1/6] Fix high speed deceleration --- Firmware/planner.h | 9 ++++----- Firmware/stepper.cpp | 13 ++++++------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/Firmware/planner.h b/Firmware/planner.h index 2096111ea..5978793be 100644 --- a/Firmware/planner.h +++ b/Firmware/planner.h @@ -77,8 +77,8 @@ typedef struct { unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) unsigned char active_extruder; // Selects the active extruder // accelerate_until and decelerate_after are set by calculate_trapezoid_for_block() and they need to be synchronized with the stepper interrupt controller. - long accelerate_until; // The index of the step event on which to stop acceleration - long decelerate_after; // The index of the step event on which to start decelerating + uint32_t accelerate_until; // The index of the step event on which to stop acceleration + uint32_t decelerate_after; // The index of the step event on which to start decelerating // Fields used by the motion planner to manage acceleration // float speed_x, speed_y, speed_z, speed_e; // Nominal mm/sec for each axis @@ -100,13 +100,12 @@ typedef struct { // Settings for the trapezoid generator (runs inside an interrupt handler). // Changing the following values in the planner needs to be synchronized with the interrupt handler by disabling the interrupts. - //FIXME nominal_rate, initial_rate and final_rate are limited to uint16_t by MultiU24X24toH16 in the stepper interrupt anyway! unsigned long nominal_rate; // The nominal step rate for this block in step_events/sec unsigned long initial_rate; // The jerk-adjusted step rate at start of block unsigned long final_rate; // The minimal rate at exit unsigned long acceleration_st; // acceleration steps/sec^2 - //FIXME does it have to be unsigned long? Probably uint8_t would be just fine. - unsigned long fan_speed; + //FIXME does it have to be int? Probably uint8_t would be just fine. Need to change in other places as well + int fan_speed; volatile char busy; diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp index de250ec97..5f72b5bd2 100644 --- a/Firmware/stepper.cpp +++ b/Firmware/stepper.cpp @@ -71,8 +71,7 @@ static dda_isteps_t counter_z, counter_e; volatile dda_usteps_t step_events_completed; // The number of step events executed in the current block -static int32_t acceleration_time, deceleration_time; -//static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate; +static uint32_t acceleration_time, deceleration_time; static uint16_t acc_step_rate; // needed for deccelaration start point static uint8_t step_loops; static uint16_t OCR1A_nominal; @@ -799,7 +798,7 @@ FORCE_INLINE void isr() { // 25.12us for acceleration / deceleration. { //WRITE_NC(LOGIC_ANALYZER_CH1, true); - if (step_events_completed.wide <= (unsigned long int)current_block->accelerate_until) { + if (step_events_completed.wide <= current_block->accelerate_until) { // v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate); acc_step_rate += uint16_t(current_block->initial_rate); @@ -817,12 +816,12 @@ FORCE_INLINE void isr() { } #endif } - else if (step_events_completed.wide > (unsigned long int)current_block->decelerate_after) { + else if (step_events_completed.wide > current_block->decelerate_after) { uint16_t step_rate; MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate); step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point. - if ((step_rate & 0x8000) || step_rate < uint16_t(current_block->final_rate)) { - // Result is negative or too small. + if (step_rate < uint16_t(current_block->final_rate)) { + // Result is too small. step_rate = uint16_t(current_block->final_rate); } // Step_rate to timer interval. @@ -832,7 +831,7 @@ FORCE_INLINE void isr() { #ifdef LIN_ADVANCE if (current_block->use_advance_lead) { - if (step_events_completed.wide <= (unsigned long int)current_block->decelerate_after + step_loops) { + if (step_events_completed.wide <= current_block->decelerate_after + step_loops) { target_adv_steps = current_block->final_adv_steps; la_state = ADV_INIT | ADV_ACC_VARY; } From 4654283f542f661a872c9d6d4fd6612a4db0e9f8 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Mon, 3 Aug 2020 18:16:20 +0200 Subject: [PATCH 2/6] Restore indentation --- Firmware/planner.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/planner.h b/Firmware/planner.h index 5978793be..425ed9b54 100644 --- a/Firmware/planner.h +++ b/Firmware/planner.h @@ -77,8 +77,8 @@ typedef struct { unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) unsigned char active_extruder; // Selects the active extruder // accelerate_until and decelerate_after are set by calculate_trapezoid_for_block() and they need to be synchronized with the stepper interrupt controller. - uint32_t accelerate_until; // The index of the step event on which to stop acceleration - uint32_t decelerate_after; // The index of the step event on which to start decelerating + uint32_t accelerate_until; // The index of the step event on which to stop acceleration + uint32_t decelerate_after; // The index of the step event on which to start decelerating // Fields used by the motion planner to manage acceleration // float speed_x, speed_y, speed_z, speed_e; // Nominal mm/sec for each axis From 8108d50b59fc13c62b72cba065193928c5b67f33 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Mon, 3 Aug 2020 18:42:40 +0200 Subject: [PATCH 3/6] Reintroduce/fix check for step_rate underflow during deceleration Check for negative results and results under the final_rate --- Firmware/stepper.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp index 5f72b5bd2..848cd3c66 100644 --- a/Firmware/stepper.cpp +++ b/Firmware/stepper.cpp @@ -819,11 +819,18 @@ FORCE_INLINE void isr() { else if (step_events_completed.wide > current_block->decelerate_after) { uint16_t step_rate; MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate); - step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point. - if (step_rate < uint16_t(current_block->final_rate)) { - // Result is too small. - step_rate = uint16_t(current_block->final_rate); + + if (step_rate > acc_step_rate) { // Check step_rate stays positive + step_rate = uint16_t(current_block->final_rate); } + else { + step_rate = acc_step_rate - step_rate; // Decelerate from acceleration end point. + + // lower limit + if (step_rate < current_block->final_rate) + step_rate = uint16_t(current_block->final_rate); + } + // Step_rate to timer interval. uint16_t timer = calc_timer(step_rate, step_loops); _NEXT_ISR(timer); From aebaca5cdc35c129c9e41acb3a8f08ef483ba3f8 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Mon, 3 Aug 2020 18:48:53 +0200 Subject: [PATCH 4/6] Correct comments regarding acceleration ramp Backport fixes from upstream Marlin --- Firmware/stepper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp index 848cd3c66..ef57ec0db 100644 --- a/Firmware/stepper.cpp +++ b/Firmware/stepper.cpp @@ -233,7 +233,7 @@ void invert_z_endstop(bool endstop_invert) // The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates // first block->accelerate_until step_events_completed, then keeps going at constant speed until // step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset. -// The slope of acceleration is calculated with the leib ramp alghorithm. +// The slope of acceleration is calculated using v = u + at where t is the accumulated timer values of the steps so far. // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse. // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. From 30a806608fca111cab82f7ac07c2804abf8f8225 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Mon, 3 Aug 2020 19:01:38 +0200 Subject: [PATCH 5/6] Also convert acceleration_rate to uint32_t acceleration_rate is also unsigned --- Firmware/planner.cpp | 2 +- Firmware/planner.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp index c0f465c2a..9ff291a0c 100644 --- a/Firmware/planner.cpp +++ b/Firmware/planner.cpp @@ -1132,7 +1132,7 @@ Having the real displacement of the head, we can calculate the total movement le block->acceleration_st = (block->acceleration_st + (bresenham_oversample >> 1)) / bresenham_oversample; #endif - block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0))); + block->acceleration_rate = ((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0))); #ifdef LIN_ADVANCE if (block->use_advance_lead) { diff --git a/Firmware/planner.h b/Firmware/planner.h index 425ed9b54..34899cac4 100644 --- a/Firmware/planner.h +++ b/Firmware/planner.h @@ -73,7 +73,7 @@ typedef struct { // steps_x.y,z, step_event_count, acceleration_rate, direction_bits and active_extruder are set by plan_buffer_line(). dda_isteps_t steps_x, steps_y, steps_z, steps_e; // Step count along each axis dda_usteps_t step_event_count; // The number of step events required to complete this block - long acceleration_rate; // The acceleration rate used for acceleration calculation + uint32_t acceleration_rate; // The acceleration rate used for acceleration calculation unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) unsigned char active_extruder; // Selects the active extruder // accelerate_until and decelerate_after are set by calculate_trapezoid_for_block() and they need to be synchronized with the stepper interrupt controller. From b8e8f182ca6ffba4b565e6b600df212381c808c5 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Mon, 3 Aug 2020 19:03:13 +0200 Subject: [PATCH 6/6] Add reference C implementations for MultiU16X8toH16/MultiU24X24toH16 Higher step counts might still overflow the ASM MultiU24X24toH16. https://github.com/MarlinFirmware/Marlin/commit/e4595fa24a2051e95df8a4ed044d2c94cca18e63 --- Firmware/speed_lookuptable.h | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/Firmware/speed_lookuptable.h b/Firmware/speed_lookuptable.h index 2748dd71a..21c6c767f 100644 --- a/Firmware/speed_lookuptable.h +++ b/Firmware/speed_lookuptable.h @@ -80,15 +80,21 @@ asm volatile ( \ #else //_NO_ASM -// NOTE: currently not implemented -void MultiU16X8toH16(unsigned short& intRes, unsigned char& charIn1, unsigned short& intIn2); -void MultiU24X24toH16(uint16_t& intRes, int32_t& longIn1, long& longIn2); +static inline void MultiU16X8toH16(uint16_t& intRes, uint8_t& charIn1, uint16_t& intIn2) +{ + intRes = ((uint32_t)charIn1 * (uint32_t)intIn2) >> 16; +} + +static inline void MultiU24X24toH16(uint16_t& intRes, uint32_t& longIn1, uint32_t& longIn2) +{ + intRes = ((uint64_t)longIn1 * (uint64_t)longIn2) >> 24; +} #endif //_NO_ASM FORCE_INLINE unsigned short calc_timer(uint16_t step_rate, uint8_t& step_loops) { - unsigned short timer; + uint16_t timer; if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY; if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times @@ -108,7 +114,7 @@ FORCE_INLINE unsigned short calc_timer(uint16_t step_rate, uint8_t& step_loops) if(step_rate >= (8*256)){ // higher step rate unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0]; unsigned char tmp_step_rate = (step_rate & 0x00ff); - unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2); + uint16_t gain = (uint16_t)pgm_read_word_near(table_address+2); MultiU16X8toH16(timer, tmp_step_rate, gain); timer = (unsigned short)pgm_read_word_near(table_address) - timer; }