andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Convert custom maths to inlines (#10728)

pull/1/head
Scott Lahteine 7 years ago
committed by GitHub
parent
8f3d313086
commit
883b0c9880
No known key found for this signature in database GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 100 additions and 100 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 149
      Marlin/src/HAL/HAL_AVR/math_AVR.h
  2. 8
      Marlin/src/HAL/math_32bit.h
  3. 23
      Marlin/src/module/stepper.cpp
  4. 20
      Marlin/src/module/stepper.h

149
Marlin/src/HAL/HAL_AVR/math_AVR.h
View File

@ -23,92 +23,95 @@
#ifndef _MATH_AVR_H_ #ifndef _MATH_AVR_H_
#define _MATH_AVR_H_ #define _MATH_AVR_H_
#define a(CODE) " " CODE "\n\t"
/** /**
* Optimized math functions for AVR * Optimized math functions for AVR
*/ */
// intRes = longIn1 * longIn2 >> 24 // intRes = longIn1 * longIn2 >> 24
// uses: // uses:
// r26 to store 0 // A[tmp] to store 0
// r27 to store bits 16-23 of the 48bit result. The top bit is used to round the two byte result. // 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. // 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. // 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 // B A are bits 24-39 and are the returned value
// C1 B1 A1 is longIn1 // C B A is longIn1
// D2 C2 B2 A2 is longIn2 // D C B A is longIn2
// //
#define MultiU24X32toH16(intRes, longIn1, longIn2) \ static FORCE_INLINE uint16_t MultiU24X32toH16(uint32_t longIn1, uint32_t longIn2) {
asm volatile ( \ register uint8_t tmp1;
A("clr r26") \ register uint8_t tmp2;
A("mul %A1, %B2") \ register uint16_t intRes;
A("mov r27, r1") \ __asm__ __volatile__(
A("mul %B1, %C2") \ A("clr %[tmp1]")
A("movw %A0, r0") \ A("mul %A[longIn1], %B[longIn2]")
A("mul %C1, %C2") \ A("mov %[tmp2], r1")
A("add %B0, r0") \ A("mul %B[longIn1], %C[longIn2]")
A("mul %C1, %B2") \ A("movw %A[intRes], r0")
A("add %A0, r0") \ A("mul %C[longIn1], %C[longIn2]")
A("adc %B0, r1") \ A("add %B[intRes], r0")
A("mul %A1, %C2") \ A("mul %C[longIn1], %B[longIn2]")
A("add r27, r0") \ A("add %A[intRes], r0")
A("adc %A0, r1") \ A("adc %B[intRes], r1")
A("adc %B0, r26") \ A("mul %A[longIn1], %C[longIn2]")
A("mul %B1, %B2") \ A("add %[tmp2], r0")
A("add r27, r0") \ A("adc %A[intRes], r1")
A("adc %A0, r1") \ A("adc %B[intRes], %[tmp1]")
A("adc %B0, r26") \ A("mul %B[longIn1], %B[longIn2]")
A("mul %C1, %A2") \ A("add %[tmp2], r0")
A("add r27, r0") \ A("adc %A[intRes], r1")
A("adc %A0, r1") \ A("adc %B[intRes], %[tmp1]")
A("adc %B0, r26") \ A("mul %C[longIn1], %A[longIn2]")
A("mul %B1, %A2") \ A("add %[tmp2], r0")
A("add r27, r1") \ A("adc %A[intRes], r1")
A("adc %A0, r26") \ A("adc %B[intRes], %[tmp1]")
A("adc %B0, r26") \ A("mul %B[longIn1], %A[longIn2]")
A("lsr r27") \ A("add %[tmp2], r1")
A("adc %A0, r26") \ A("adc %A[intRes], %[tmp1]")
A("adc %B0, r26") \ A("adc %B[intRes], %[tmp1]")
A("mul %D2, %A1") \ A("lsr %[tmp2]")
A("add %A0, r0") \ A("adc %A[intRes], %[tmp1]")
A("adc %B0, r1") \ A("adc %B[intRes], %[tmp1]")
A("mul %D2, %B1") \ A("mul %D[longIn2], %A[longIn1]")
A("add %B0, r0") \ A("add %A[intRes], r0")
A("clr r1") \ A("adc %B[intRes], r1")
: \ A("mul %D[longIn2], %B[longIn1]")
"=&r" (intRes) \ A("add %B[intRes], r0")
: \ A("clr r1")
"d" (longIn1), \ : [intRes] "=&r" (intRes),
"d" (longIn2) \ [tmp1] "=&r" (tmp1),
: \ [tmp2] "=&r" (tmp2)
"r26" , "r27" \ : [longIn1] "d" (longIn1),
) [longIn2] "d" (longIn2)
: "cc"
);
return intRes;
}
// intRes = intIn1 * intIn2 >> 16 // intRes = intIn1 * intIn2 >> 16
// uses: // uses:
// r26 to store 0 // r26 to store 0
// r27 to store the byte 1 of the 24 bit result // r27 to store the byte 1 of the 24 bit result
#define MultiU16X8toH16(intRes, charIn1, intIn2) \ static FORCE_INLINE uint16_t MultiU16X8toH16(uint8_t charIn1, uint16_t intIn2) {
asm volatile ( \ register uint8_t tmp;
A("clr r26") \ register uint16_t intRes;
A("mul %A1, %B2") \ __asm__ __volatile__ (
A("movw %A0, r0") \ A("clr %[tmp]")
A("mul %A1, %A2") \ A("mul %[charIn1], %B[intIn2]")
A("add %A0, r1") \ A("movw %A[intRes], r0")
A("adc %B0, r26") \ A("mul %[charIn1], %A[intIn2]")
A("lsr r0") \ A("add %A[intRes], r1")
A("adc %A0, r26") \ A("adc %B[intRes], %[tmp]")
A("adc %B0, r26") \ A("lsr r0")
A("clr r1") \ A("adc %A[intRes], %[tmp]")
: \ A("adc %B[intRes], %[tmp]")
"=&r" (intRes) \ A("clr r1")
: \ : [intRes] "=&r" (intRes),
"d" (charIn1), \ [tmp] "=&r" (tmp)
"d" (intIn2) \ : [charIn1] "d" (charIn1),
: \ [intIn2] "d" (intIn2)
"r26" \ : "cc"
) );
return intRes;
}
#endif // _MATH_AVR_H_ #endif // _MATH_AVR_H_

8
Marlin/src/HAL/math_32bit.h
View File

@ -23,11 +23,13 @@
#ifndef MATH_32BIT_H #ifndef MATH_32BIT_H
#define MATH_32BIT_H #define MATH_32BIT_H
#include "../core/macros.h"
/** /**
* Math helper functions for 32 bit CPUs * Math helper functions for 32 bit CPUs
*/ */
static FORCE_INLINE uint32_t MultiU32X24toH32(uint32_t longIn1, uint32_t longIn2) {
#define MultiU32X32toH32(intRes, longIn1, longIn2) intRes = ((uint64_t)longIn1 * longIn2 + 0x80000000) >> 32 return ((uint64_t)longIn1 * longIn2 + 0x00800000) >> 24;
#define MultiU32X24toH32(intRes, longIn1, longIn2) intRes = ((uint64_t)longIn1 * longIn2 + 0x00800000) >> 24 }
#endif // MATH_32BIT_H #endif // MATH_32BIT_H

23
Marlin/src/module/stepper.cpp
View File

@ -1158,6 +1158,12 @@ HAL_STEP_TIMER_ISR {
HAL_timer_isr_epilogue(STEP_TIMER_NUM); HAL_timer_isr_epilogue(STEP_TIMER_NUM);
} }
#ifdef CPU_32_BIT
#define STEP_MULTIPLY(A,B) MultiU32X24toH32(A, B);
#else
#define STEP_MULTIPLY(A,B) MultiU24X32toH16(A, B);
#endif
void Stepper::isr() { void Stepper::isr() {
#define ENDSTOP_NOMINAL_OCR_VAL 1500 * HAL_TICKS_PER_US // Check endstops every 1.5ms to guarantee two stepper ISRs within 5ms for BLTouch #define ENDSTOP_NOMINAL_OCR_VAL 1500 * HAL_TICKS_PER_US // Check endstops every 1.5ms to guarantee two stepper ISRs within 5ms for BLTouch
@ -1525,14 +1531,7 @@ void Stepper::isr() {
? _eval_bezier_curve(acceleration_time) ? _eval_bezier_curve(acceleration_time)
: current_block->cruise_rate; : current_block->cruise_rate;
#else #else
#ifdef CPU_32_BIT acc_step_rate = STEP_MULTIPLY(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate;
MultiU32X24toH32(acc_step_rate, acceleration_time, current_block->acceleration_rate);
#else
MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
#endif
acc_step_rate += current_block->initial_rate;
// upper limit
NOMORE(acc_step_rate, current_block->nominal_rate); NOMORE(acc_step_rate, current_block->nominal_rate);
#endif #endif
@ -1576,18 +1575,14 @@ void Stepper::isr() {
#else #else
// Using the old trapezoidal control // Using the old trapezoidal control
#ifdef CPU_32_BIT step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate);
MultiU32X24toH32(step_rate, deceleration_time, current_block->acceleration_rate);
#else
MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate);
#endif
if (step_rate < acc_step_rate) { // Still decelerating? if (step_rate < acc_step_rate) { // Still decelerating?
step_rate = acc_step_rate - step_rate; step_rate = acc_step_rate - step_rate;
NOLESS(step_rate, current_block->final_rate); NOLESS(step_rate, current_block->final_rate);
} }
else else
step_rate = current_block->final_rate; step_rate = current_block->final_rate;
#endif #endif
// step_rate to timer interval // step_rate to timer interval

20
Marlin/src/module/stepper.h
View File

@ -340,24 +340,24 @@ class Stepper {
#ifdef CPU_32_BIT #ifdef CPU_32_BIT
// In case of high-performance processor, it is able to calculate in real-time // In case of high-performance processor, it is able to calculate in real-time
const uint32_t MIN_TIME_PER_STEP = (HAL_STEPPER_TIMER_RATE) / ((STEP_DOUBLER_FREQUENCY) * 2); const uint32_t min_time_per_step = (HAL_STEPPER_TIMER_RATE) / ((STEP_DOUBLER_FREQUENCY) * 2);
timer = uint32_t(HAL_STEPPER_TIMER_RATE) / step_rate; timer = uint32_t(HAL_STEPPER_TIMER_RATE) / step_rate;
NOLESS(timer, MIN_TIME_PER_STEP); // (STEP_DOUBLER_FREQUENCY * 2 kHz - this should never happen) NOLESS(timer, min_time_per_step); // (STEP_DOUBLER_FREQUENCY * 2 kHz - this should never happen)
#else #else
NOLESS(step_rate, F_CPU / 500000); NOLESS(step_rate, F_CPU / 500000);
step_rate -= F_CPU / 500000; // Correct for minimal speed step_rate -= F_CPU / 500000; // Correct for minimal speed
if (step_rate >= (8 * 256)) { // higher step rate if (step_rate >= (8 * 256)) { // higher step rate
unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate >> 8)][0]; uint8_t tmp_step_rate = (step_rate & 0x00FF);
unsigned char tmp_step_rate = (step_rate & 0x00FF); uint16_t table_address = (uint16_t)&speed_lookuptable_fast[(uint8_t)(step_rate >> 8)][0];
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 = MultiU16X8toH16(tmp_step_rate, gain);
timer = (unsigned short)pgm_read_word_near(table_address) - timer; timer = (uint16_t)pgm_read_word_near(table_address) - timer;
} }
else { // lower step rates else { // lower step rates
unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0]; uint16_t table_address = (uint16_t)&speed_lookuptable_slow[0][0];
table_address += ((step_rate) >> 1) & 0xFFFC; table_address += ((step_rate) >> 1) & 0xFFFC;
timer = (unsigned short)pgm_read_word_near(table_address); timer = (uint16_t)pgm_read_word_near(table_address);
timer -= (((unsigned short)pgm_read_word_near(table_address + 2) * (unsigned char)(step_rate & 0x0007)) >> 3); timer -= (((uint16_t)pgm_read_word_near(table_address + 2) * (uint8_t)(step_rate & 0x0007)) >> 3);
} }
if (timer < 100) { // (20kHz - this should never happen) if (timer < 100) { // (20kHz - this should never happen)
timer = 100; timer = 100;

Write Preview
Loading…
Cancel
Save