Merge pull request #2792 from wavexx/MK3_fix_high_speed_deceleration

Mk3 fix high speed deceleration

Firmware/planner.cpp

@@ -1159,7 +1159,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)));
 
   // Start with a safe speed.
   // Safe speed is the speed, from which the machine may halt to stop immediately.

Firmware/planner.h

@@ -73,12 +73,12 @@ 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.
-  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;
 
 

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;
   }

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;
@@ -234,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.
@@ -788,7 +787,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);
@@ -809,14 +808,21 @@ 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.
-          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);
@@ -824,7 +830,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;
                 if (e_extruding && current_adv_steps < target_adv_steps)