speed_lookuptable.h 4.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145
  1. #ifndef SPEED_LOOKUPTABLE_H
  2. #define SPEED_LOOKUPTABLE_H
  3. #include "Marlin.h"
  4. extern const uint16_t speed_lookuptable_fast[256][2] PROGMEM;
  5. extern const uint16_t speed_lookuptable_slow[256][2] PROGMEM;
  6. #ifndef _NO_ASM
  7. // return ((x * y) >> 8) with rounding when shifting right
  8. FORCE_INLINE uint16_t MUL8x16R8(uint8_t x, uint16_t y) {
  9. uint16_t out;
  10. __asm__ (
  11. // %0 out
  12. // %1 x
  13. // %2 y
  14. // uint8_t: %An or %n
  15. // uint16_t: %Bn %An
  16. // __uint24: %Cn %Bn %An
  17. // uint32_t: %Dn %Cn %Bn %An
  18. //
  19. //
  20. // B2 A2 *
  21. // A1
  22. //---------
  23. // B0 A0 RR
  24. "mul %B2, %A1" "\n\t"
  25. "movw %0, r0" "\n\t"
  26. "mul %A2, %A1" "\n\t"
  27. "lsl r0" "\n\t" //push MSB to carry for rounding
  28. "adc %A0, r1" "\n\t" //add with carry (for rounding)
  29. "clr r1" "\n\t" //make r1 __zero_reg__ again
  30. "adc %B0, r1" "\n\t" //propagate carry of addition (add 0 with carry)
  31. : "=&r" (out)
  32. : "r" (x), "r" (y)
  33. : "r0", "r1" //clobbers: Technically these are either scratch registers or always 0 registers, but I'm making sure the compiler knows just in case.
  34. );
  35. return out;
  36. }
  37. // return ((x * y) >> 24) with rounding when shifting right
  38. FORCE_INLINE uint16_t MUL24x24R24(__uint24 x, __uint24 y) {
  39. uint16_t out;
  40. __asm__ (
  41. // %0 out
  42. // %1 x
  43. // %2 y
  44. // uint8_t: %An or %n
  45. // uint16_t: %Bn %An
  46. // __uint24: %Cn %Bn %An
  47. // uint32_t: %Dn %Cn %Bn %An
  48. //
  49. //
  50. // B2 A2 *
  51. // A1
  52. //---------
  53. // B0 A0 RR
  54. "clr r26 \n\t"
  55. "mul %A1, %B2 \n\t"
  56. "mov r27, r1 \n\t"
  57. "mul %B1, %C2 \n\t"
  58. "movw %A0, r0 \n\t"
  59. "mul %C1, %C2 \n\t"
  60. "add %B0, r0 \n\t"
  61. "mul %C1, %B2 \n\t"
  62. "add %A0, r0 \n\t"
  63. "adc %B0, r1 \n\t"
  64. "mul %A1, %C2 \n\t"
  65. "add r27, r0 \n\t"
  66. "adc %A0, r1 \n\t"
  67. "adc %B0, r26 \n\t"
  68. "mul %B1, %B2 \n\t"
  69. "add r27, r0 \n\t"
  70. "adc %A0, r1 \n\t"
  71. "adc %B0, r26 \n\t"
  72. "mul %C1, %A2 \n\t"
  73. "add r27, r0 \n\t"
  74. "adc %A0, r1 \n\t"
  75. "adc %B0, r26 \n\t"
  76. "mul %B1, %A2 \n\t"
  77. "add r27, r1 \n\t"
  78. "adc %A0, r26 \n\t"
  79. "adc %B0, r26 \n\t"
  80. "lsl r27 \n\t"
  81. "adc %A0, r26 \n\t"
  82. "adc %B0, r26 \n\t"
  83. "clr r1 \n\t"
  84. : "=&r" (out)
  85. : "r" (x), "r" (y)
  86. : "r0", "r1", "r26" , "r27" //clobbers: Technically these are either scratch registers or always 0 registers, but I'm making sure the compiler knows just in case. R26 is __zero_reg__, R27 is a temporary register.
  87. );
  88. return out;
  89. }
  90. #else //_NO_ASM
  91. FORCE_INLINE uint16_t MUL8x16R8(uint8_t charIn1, uint16_t intIn2)
  92. {
  93. return ((uint32_t)charIn1 * (uint32_t)intIn2) >> 8;
  94. }
  95. FORCE_INLINE uint16_t MUL24x24R24(uint32_t longIn1, uint32_t longIn2)
  96. {
  97. return ((uint64_t)longIn1 * (uint64_t)longIn2) >> 24;
  98. }
  99. #endif //_NO_ASM
  100. FORCE_INLINE unsigned short calc_timer(uint16_t step_rate, uint8_t& step_loops) {
  101. uint16_t timer;
  102. if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
  103. if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
  104. step_rate = (step_rate >> 2)&0x3fff;
  105. step_loops = 4;
  106. }
  107. else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times
  108. step_rate = (step_rate >> 1)&0x7fff;
  109. step_loops = 2;
  110. }
  111. else {
  112. step_loops = 1;
  113. }
  114. if(step_rate < (F_CPU/500000)) step_rate = (F_CPU/500000);
  115. step_rate -= (F_CPU/500000); // Correct for minimal speed
  116. if(step_rate >= (8*256)){ // higher step rate
  117. unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
  118. unsigned char tmp_step_rate = (step_rate & 0x00ff);
  119. uint16_t gain = (uint16_t)pgm_read_word_near(table_address+2);
  120. timer = (unsigned short)pgm_read_word_near(table_address) - MUL8x16R8(tmp_step_rate, gain);
  121. }
  122. else { // lower step rates
  123. unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
  124. table_address += ((step_rate)>>1) & 0xfffc;
  125. timer = (unsigned short)pgm_read_word_near(table_address);
  126. timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
  127. }
  128. if(timer < 100) { timer = 100; }//(20kHz this should never happen)////MSG_STEPPER_TOO_HIGH c=0 r=0
  129. return timer;
  130. }
  131. #endif