3d_print_resources/Marlin
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Convert maths macros to inlines

Browse source 
For parity with #10728

Co-Authored-By: ejtagle <ejtagle@hotmail.com>
This commit is contained in:
Scott Lahteine 2018-05-13 10:36:37 -05:00
parent 431a81fc66
commit 7c6dd087c9
2 changed files with 85 additions and 85 deletions
Download patch file Download diff file Expand all files Collapse all files

114
Marlin/stepper.cpp
View file

@ -243,59 +243,63 @@ volatile int32_t Stepper::endstops_trigsteps[XYZ];
// intRes = longIn1 * longIn2 >> 24
// uses:
// r26 to store 0
// r27 to store bits 16-23 of the 48bit result. The top bit is used to round the two byte result.
// A[tmp] to store 0
// B[tmp] to store bits 16-23 of the 48bit result. The top bit is used to round the two byte result.
// note that the lower two bytes and the upper byte of the 48bit result are not calculated.
// this can cause the result to be out by one as the lower bytes may cause carries into the upper ones.
// B0 A0 are bits 24-39 and are the returned value
// C1 B1 A1 is longIn1
// D2 C2 B2 A2 is longIn2
// B A are bits 24-39 and are the returned value
// C B A is longIn1
// D C B A is longIn2
//
#define MultiU24X32toH16(intRes, longIn1, longIn2) \
asm volatile ( \
A("clr r26") \
A("mul %A1, %B2") \
A("mov r27, r1") \
A("mul %B1, %C2") \
A("movw %A0, r0") \
A("mul %C1, %C2") \
A("add %B0, r0") \
A("mul %C1, %B2") \
A("add %A0, r0") \
A("adc %B0, r1") \
A("mul %A1, %C2") \
A("add r27, r0") \
A("adc %A0, r1") \
A("adc %B0, r26") \
A("mul %B1, %B2") \
A("add r27, r0") \
A("adc %A0, r1") \
A("adc %B0, r26") \
A("mul %C1, %A2") \
A("add r27, r0") \
A("adc %A0, r1") \
A("adc %B0, r26") \
A("mul %B1, %A2") \
A("add r27, r1") \
A("adc %A0, r26") \
A("adc %B0, r26") \
A("lsr r27") \
A("adc %A0, r26") \
A("adc %B0, r26") \
A("mul %D2, %A1") \
A("add %A0, r0") \
A("adc %B0, r1") \
A("mul %D2, %B1") \
A("add %B0, r0") \
A("clr r1") \
: \
"=&r" (intRes) \
: \
"d" (longIn1), \
"d" (longIn2) \
: \
"r26" , "r27" \
)
static FORCE_INLINE uint16_t MultiU24X32toH16(uint32_t longIn1, uint32_t longIn2) {
register uint8_t tmp1;
register uint8_t tmp2;
register uint16_t intRes;
__asm__ __volatile__(
A("clr %[tmp1]")
A("mul %A[longIn1], %B[longIn2]")
A("mov %[tmp2], r1")
A("mul %B[longIn1], %C[longIn2]")
A("movw %A[intRes], r0")
A("mul %C[longIn1], %C[longIn2]")
A("add %B[intRes], r0")
A("mul %C[longIn1], %B[longIn2]")
A("add %A[intRes], r0")
A("adc %B[intRes], r1")
A("mul %A[longIn1], %C[longIn2]")
A("add %[tmp2], r0")
A("adc %A[intRes], r1")
A("adc %B[intRes], %[tmp1]")
A("mul %B[longIn1], %B[longIn2]")
A("add %[tmp2], r0")
A("adc %A[intRes], r1")
A("adc %B[intRes], %[tmp1]")
A("mul %C[longIn1], %A[longIn2]")
A("add %[tmp2], r0")
A("adc %A[intRes], r1")
A("adc %B[intRes], %[tmp1]")
A("mul %B[longIn1], %A[longIn2]")
A("add %[tmp2], r1")
A("adc %A[intRes], %[tmp1]")
A("adc %B[intRes], %[tmp1]")
A("lsr %[tmp2]")
A("adc %A[intRes], %[tmp1]")
A("adc %B[intRes], %[tmp1]")
A("mul %D[longIn2], %A[longIn1]")
A("add %A[intRes], r0")
A("adc %B[intRes], r1")
A("mul %D[longIn2], %B[longIn1]")
A("add %B[intRes], r0")
A("clr r1")
: [intRes] "=&r" (intRes),
[tmp1] "=&r" (tmp1),
[tmp2] "=&r" (tmp2)
: [longIn1] "d" (longIn1),
[longIn2] "d" (longIn2)
: "cc"
);
return intRes;
}
// Some useful constants
@ -1506,10 +1510,7 @@ void Stepper::isr() {
? _eval_bezier_curve(acceleration_time)
: current_block->cruise_rate;
#else
MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
acc_step_rate += current_block->initial_rate;
// upper limit
acc_step_rate = MultiU24X32toH16(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate;
NOMORE(acc_step_rate, current_block->nominal_rate);
#endif
@ -1540,7 +1541,6 @@ void Stepper::isr() {
#if ENABLED(BEZIER_JERK_CONTROL)
// If this is the 1st time we process the 2nd half of the trapezoid...
if (!bezier_2nd_half) {
// Initialize the Bézier speed curve
_calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse);
bezier_2nd_half = true;
@ -1553,14 +1553,14 @@ void Stepper::isr() {
#else
// Using the old trapezoidal control
MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate);
step_rate = MultiU24X32toH16(deceleration_time, current_block->acceleration_rate);
if (step_rate < acc_step_rate) { // Still decelerating?
step_rate = acc_step_rate - step_rate;
NOLESS(step_rate, current_block->final_rate);
}
else
step_rate = current_block->final_rate;
#endif
// step_rate to timer interval