132 lines
3.3 KiB
C
132 lines
3.3 KiB
C
#ifndef SPEED_LOOKUPTABLE_H
|
|
#define SPEED_LOOKUPTABLE_H
|
|
|
|
#include "Marlin.h"
|
|
|
|
extern const uint16_t speed_lookuptable_fast[256][2] PROGMEM;
|
|
extern const uint16_t speed_lookuptable_slow[256][2] PROGMEM;
|
|
|
|
#ifndef _NO_ASM
|
|
|
|
// intRes = intIn1 * intIn2 >> 16
|
|
// uses:
|
|
// r26 to store 0
|
|
// r27 to store the byte 1 of the 24 bit result
|
|
#define MultiU16X8toH16(intRes, charIn1, intIn2) \
|
|
asm volatile ( \
|
|
"clr r26 \n\t" \
|
|
"mul %A1, %B2 \n\t" \
|
|
"movw %A0, r0 \n\t" \
|
|
"mul %A1, %A2 \n\t" \
|
|
"add %A0, r1 \n\t" \
|
|
"adc %B0, r26 \n\t" \
|
|
"lsr r0 \n\t" \
|
|
"adc %A0, r26 \n\t" \
|
|
"adc %B0, r26 \n\t" \
|
|
"clr r1 \n\t" \
|
|
: \
|
|
"=&r" (intRes) \
|
|
: \
|
|
"d" (charIn1), \
|
|
"d" (intIn2) \
|
|
: \
|
|
"r26" \
|
|
)
|
|
|
|
// intRes = longIn1 * longIn2 >> 24
|
|
// uses:
|
|
// r26 to store 0
|
|
// r27 to store the byte 1 of the 48bit result
|
|
#define MultiU24X24toH16(intRes, longIn1, longIn2) \
|
|
asm volatile ( \
|
|
"clr r26 \n\t" \
|
|
"mul %A1, %B2 \n\t" \
|
|
"mov r27, r1 \n\t" \
|
|
"mul %B1, %C2 \n\t" \
|
|
"movw %A0, r0 \n\t" \
|
|
"mul %C1, %C2 \n\t" \
|
|
"add %B0, r0 \n\t" \
|
|
"mul %C1, %B2 \n\t" \
|
|
"add %A0, r0 \n\t" \
|
|
"adc %B0, r1 \n\t" \
|
|
"mul %A1, %C2 \n\t" \
|
|
"add r27, r0 \n\t" \
|
|
"adc %A0, r1 \n\t" \
|
|
"adc %B0, r26 \n\t" \
|
|
"mul %B1, %B2 \n\t" \
|
|
"add r27, r0 \n\t" \
|
|
"adc %A0, r1 \n\t" \
|
|
"adc %B0, r26 \n\t" \
|
|
"mul %C1, %A2 \n\t" \
|
|
"add r27, r0 \n\t" \
|
|
"adc %A0, r1 \n\t" \
|
|
"adc %B0, r26 \n\t" \
|
|
"mul %B1, %A2 \n\t" \
|
|
"add r27, r1 \n\t" \
|
|
"adc %A0, r26 \n\t" \
|
|
"adc %B0, r26 \n\t" \
|
|
"lsr r27 \n\t" \
|
|
"adc %A0, r26 \n\t" \
|
|
"adc %B0, r26 \n\t" \
|
|
"clr r1 \n\t" \
|
|
: \
|
|
"=&r" (intRes) \
|
|
: \
|
|
"d" (longIn1), \
|
|
"d" (longIn2) \
|
|
: \
|
|
"r26" , "r27" \
|
|
)
|
|
|
|
#else //_NO_ASM
|
|
|
|
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) {
|
|
uint16_t timer;
|
|
if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
|
|
|
|
if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
|
|
step_rate = (step_rate >> 2)&0x3fff;
|
|
step_loops = 4;
|
|
}
|
|
else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times
|
|
step_rate = (step_rate >> 1)&0x7fff;
|
|
step_loops = 2;
|
|
}
|
|
else {
|
|
step_loops = 1;
|
|
}
|
|
|
|
if(step_rate < (F_CPU/500000)) step_rate = (F_CPU/500000);
|
|
step_rate -= (F_CPU/500000); // Correct for minimal speed
|
|
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);
|
|
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;
|
|
}
|
|
else { // lower step rates
|
|
unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
|
|
table_address += ((step_rate)>>1) & 0xfffc;
|
|
timer = (unsigned short)pgm_read_word_near(table_address);
|
|
timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
|
|
}
|
|
if(timer < 100) { timer = 100; }//(20kHz this should never happen)////MSG_STEPPER_TOO_HIGH c=0 r=0
|
|
return timer;
|
|
}
|
|
|
|
#endif
|