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

Also change MUL24x24R24 to the new format and fix rounding

Browse Source
This commit is contained in:
Alex Voinea 2022-02-06 01:07:10 +01:00
parent d06e74dd6f
commit 70cb30208c
2 changed files with 59 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
Firmware/speed_lookuptable.h
View File

@ -39,61 +39,70 @@ FORCE_INLINE uint16_t MUL8x16R8(uint8_t x, uint16_t y) {
return out; return out;
} }
// intRes = longIn1 * longIn2 >> 24 // return ((x * y) >> 24) with rounding when shifting right
// uses: FORCE_INLINE uint16_t MUL24x24R24(__uint24 x, __uint24 y) {
// r26 to store 0 uint16_t out;
// r27 to store the byte 1 of the 48bit result __asm__ (
#define MultiU24X24toH16(intRes, longIn1, longIn2) \ // %0 out
asm volatile ( \ // %1 x
"clr r26 \n\t" \ // %2 y
"mul %A1, %B2 \n\t" \ // uint8_t: %An or %n
"mov r27, r1 \n\t" \ // uint16_t: %Bn %An
"mul %B1, %C2 \n\t" \ // __uint24: %Cn %Bn %An
"movw %A0, r0 \n\t" \ // uint32_t: %Dn %Cn %Bn %An
"mul %C1, %C2 \n\t" \ //
"add %B0, r0 \n\t" \ //
"mul %C1, %B2 \n\t" \ // B2 A2 *
"add %A0, r0 \n\t" \ // A1
"adc %B0, r1 \n\t" \ //---------
"mul %A1, %C2 \n\t" \ // B0 A0 RR
"add r27, r0 \n\t" \ "clr r26 \n\t"
"adc %A0, r1 \n\t" \ "mul %A1, %B2 \n\t"
"adc %B0, r26 \n\t" \ "mov r27, r1 \n\t"
"mul %B1, %B2 \n\t" \ "mul %B1, %C2 \n\t"
"add r27, r0 \n\t" \ "movw %A0, r0 \n\t"
"adc %A0, r1 \n\t" \ "mul %C1, %C2 \n\t"
"adc %B0, r26 \n\t" \ "add %B0, r0 \n\t"
"mul %C1, %A2 \n\t" \ "mul %C1, %B2 \n\t"
"add r27, r0 \n\t" \ "add %A0, r0 \n\t"
"adc %A0, r1 \n\t" \ "adc %B0, r1 \n\t"
"adc %B0, r26 \n\t" \ "mul %A1, %C2 \n\t"
"mul %B1, %A2 \n\t" \ "add r27, r0 \n\t"
"add r27, r1 \n\t" \ "adc %A0, r1 \n\t"
"adc %A0, r26 \n\t" \ "adc %B0, r26 \n\t"
"adc %B0, r26 \n\t" \ "mul %B1, %B2 \n\t"
"lsr r27 \n\t" \ "add r27, r0 \n\t"
"adc %A0, r26 \n\t" \ "adc %A0, r1 \n\t"
"adc %B0, r26 \n\t" \ "adc %B0, r26 \n\t"
"clr r1 \n\t" \ "mul %C1, %A2 \n\t"
: \ "add r27, r0 \n\t"
"=&r" (intRes) \ "adc %A0, r1 \n\t"
: \ "adc %B0, r26 \n\t"
"d" (longIn1), \ "mul %B1, %A2 \n\t"
"d" (longIn2) \ "add r27, r1 \n\t"
: \ "adc %A0, r26 \n\t"
"r26" , "r27" \ "adc %B0, r26 \n\t"
) "lsl r27 \n\t"
"adc %A0, r26 \n\t"
"adc %B0, r26 \n\t"
"clr r1 \n\t"
: "=&r" (out)
: "r" (x), "r" (y)
: "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.
);
return out;
}
#else //_NO_ASM #else //_NO_ASM
static inline void MultiU16X8toH16(uint16_t& intRes, uint8_t& charIn1, uint16_t& intIn2) FORCE_INLINE uint16_t MUL8x16R8(uint8_t charIn1, uint16_t intIn2)
{ {
intRes = ((uint32_t)charIn1 * (uint32_t)intIn2) >> 8; return ((uint32_t)charIn1 * (uint32_t)intIn2) >> 8;
} }
static inline void MultiU24X24toH16(uint16_t& intRes, uint32_t& longIn1, uint32_t& longIn2) FORCE_INLINE uint16_t MUL24x24R24(uint32_t longIn1, uint32_t longIn2)
{ {
intRes = ((uint64_t)longIn1 * (uint64_t)longIn2) >> 24; return ((uint64_t)longIn1 * (uint64_t)longIn2) >> 24;
} }
#endif //_NO_ASM #endif //_NO_ASM

5
Firmware/stepper.cpp
View File

@ -818,7 +818,7 @@ FORCE_INLINE void isr() {
//WRITE_NC(LOGIC_ANALYZER_CH1, true); //WRITE_NC(LOGIC_ANALYZER_CH1, true);
if (step_events_completed.wide <= 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 // 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 = MUL24x24R24(acceleration_time, current_block->acceleration_rate);
acc_step_rate += uint16_t(current_block->initial_rate); acc_step_rate += uint16_t(current_block->initial_rate);
// upper limit // upper limit
if(acc_step_rate > uint16_t(current_block->nominal_rate)) if(acc_step_rate > uint16_t(current_block->nominal_rate))
@ -838,8 +838,7 @@ FORCE_INLINE void isr() {
#endif #endif
} }
else if (step_events_completed.wide > current_block->decelerate_after) { else if (step_events_completed.wide > current_block->decelerate_after) {
uint16_t step_rate; uint16_t step_rate = MUL24x24R24(deceleration_time, current_block->acceleration_rate);
MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
if (step_rate > acc_step_rate) { // Check step_rate stays positive if (step_rate > acc_step_rate) { // Check step_rate stays positive
step_rate = uint16_t(current_block->final_rate); step_rate = uint16_t(current_block->final_rate);