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

More concise commentary in planner.cpp

pull/1/head
Scott Lahteine 7 years ago
parent
6919e87656
commit
8f14ef855d
1 changed files with 85 additions and 145 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. 230
      Marlin/src/module/planner.cpp

230
Marlin/src/module/planner.cpp
View File

@ -235,145 +235,86 @@ void Planner::init() {
#if ENABLED(S_CURVE_ACCELERATION) #if ENABLED(S_CURVE_ACCELERATION)
#ifdef __AVR__ #ifdef __AVR__
// This routine, for AVR, returns 0x1000000 / d, but trying to get the inverse as /**
// fast as possible. A fast converging iterative Newton-Raphson method is able to * This routine returns 0x1000000 / d, getting the inverse as fast as possible.
// reach full precision in just 1 iteration, and takes 211 cycles (worst case, mean * A fast-converging iterative Newton-Raphson method can reach full precision in
// case is less, up to 30 cycles for small divisors), instead of the 500 cycles a * just 1 iteration, and takes 211 cycles (worst case; the mean case is less, up
// normal division would take. * to 30 cycles for small divisors), instead of the 500 cycles a normal division
// * would take.
// Inspired by the following page, *
// https://stackoverflow.com/questions/27801397/newton-raphson-division-with-big-integers * Inspired by the following page:
// * https://stackoverflow.com/questions/27801397/newton-raphson-division-with-big-integers
// Suppose we want to calculate *
// floor(2 ^ k / B) where B is a positive integer * Suppose we want to calculate floor(2 ^ k / B) where B is a positive integer
// Then * Then, B must be <= 2^k, otherwise, the quotient is 0.
// B must be <= 2^k, otherwise, the quotient is 0. *
// * The Newton - Raphson iteration for x = B / 2 ^ k yields:
// The Newton - Raphson iteration for x = B / 2 ^ k yields: * q[n + 1] = q[n] * (2 - q[n] * B / 2 ^ k)
// q[n + 1] = q[n] * (2 - q[n] * B / 2 ^ k) *
// * This can be rearranged to:
// We can rearrange it as: * q[n + 1] = q[n] * (2 ^ (k + 1) - q[n] * B) >> k
// q[n + 1] = q[n] * (2 ^ (k + 1) - q[n] * B) >> k *
// * Each iteration requires only integer multiplications and bit shifts.
// Each iteration of this kind requires only integer multiplications * It doesn't necessarily converge to floor(2 ^ k / B) but in the worst case
// and bit shifts. * it eventually alternates between floor(2 ^ k / B) and ceil(2 ^ k / B).
// Does it converge to floor(2 ^ k / B) ?: Not necessarily, but, in * So it checks for this case and extracts floor(2 ^ k / B).
// the worst case, it eventually alternates between floor(2 ^ k / B) *
// and ceiling(2 ^ k / B)). * A simple but important optimization for this approach is to truncate
// So we can use some not-so-clever test to see if we are in this * multiplications (i.e., calculate only the higher bits of the product) in the
// case, and extract floor(2 ^ k / B). * early iterations of the Newton - Raphson method. This is done so the results
// Lastly, a simple but important optimization for this approach is to * of the early iterations are far from the quotient. Then it doesn't matter if
// truncate multiplications (i.e.calculate only the higher bits of the * they are done inaccurately.
// product) in the early iterations of the Newton - Raphson method.The * It's important to pick a good starting value for x. Knowing how many
// reason to do so, is that the results of the early iterations are far * digits the divisor has, it can be estimated:
// from the quotient, and it doesn't matter to perform them inaccurately. *
// Finally, we should pick a good starting value for x. Knowing how many * 2^k / x = 2 ^ log2(2^k / x)
// digits the divisor has, we can estimate it: * 2^k / x = 2 ^(log2(2^k)-log2(x))
// * 2^k / x = 2 ^(k*log2(2)-log2(x))
// 2^k / x = 2 ^ log2(2^k / x) * 2^k / x = 2 ^ (k-log2(x))
// 2^k / x = 2 ^(log2(2^k)-log2(x)) * 2^k / x >= 2 ^ (k-floor(log2(x)))
// 2^k / x = 2 ^(k*log2(2)-log2(x)) * floor(log2(x)) is simply the index of the most significant bit set.
// 2^k / x = 2 ^ (k-log2(x)) *
// 2^k / x >= 2 ^ (k-floor(log2(x))) * If this estimation can be improved even further the number of iterations can be
// floor(log2(x)) simply is the index of the most significant bit set. * reduced a lot, saving valuable execution time.
// * The paper "Software Integer Division" by Thomas L.Rodeheffer, Microsoft
// If we could improve this estimation even further, then the number of * Research, Silicon Valley,August 26, 2008, available at
// iterations can be dropped quite a bit, thus saving valuable execution time. * https://www.microsoft.com/en-us/research/wp-content/uploads/2008/08/tr-2008-141.pdf
// The paper "Software Integer Division" by Thomas L.Rodeheffer, Microsoft * suggests, for its integer division algorithm, using a table to supply the first
// Research, Silicon Valley,August 26, 2008, that is available at * 8 bits of precision, then, due to the quadratic convergence nature of the
// https://www.microsoft.com/en-us/research/wp-content/uploads/2008/08/tr-2008-141.pdf * Newton-Raphon iteration, just 2 iterations should be enough to get maximum
// suggests , for its integer division algorithm, that using a table to supply the * precision of the division.
// first 8 bits of precision, and due to the quadratic convergence nature of the * By precomputing values of inverses for small denominator values, just one
// Newton-Raphon iteration, then just 2 iterations should be enough to get * Newton-Raphson iteration is enough to reach full precision.
// maximum precision of the division. * This code uses the top 9 bits of the denominator as index.
// If we precompute values of inverses for small denominator values, then *
// just one Newton-Raphson iteration is enough to reach full precision * The AVR assembly function implements this C code using the data below:
// We will use the top 9 bits of the denominator as index. *
// * // For small divisors, it is best to directly retrieve the results
// The AVR assembly function is implementing the following C code, included * if (d <= 110) return pgm_read_dword(&small_inv_tab[d]);
// here as reference: *
// * // Compute initial estimation of 0x1000000/x -
// uint32_t get_period_inverse(uint32_t d) { * // Get most significant bit set on divider
// static const uint8_t inv_tab[256] = { * uint8_t idx = 0;
// 255,253,252,250,248,246,244,242,240,238,236,234,233,231,229,227, * uint32_t nr = d;
// 225,224,222,220,218,217,215,213,212,210,208,207,205,203,202,200, * if (!(nr & 0xFF0000)) {
// 199,197,195,194,192,191,189,188,186,185,183,182,180,179,178,176, * nr <<= 8; idx += 8;
// 175,173,172,170,169,168,166,165,164,162,161,160,158,157,156,154, * if (!(nr & 0xFF0000)) { nr <<= 8; idx += 8; }
// 153,152,151,149,148,147,146,144,143,142,141,139,138,137,136,135, * }
// 134,132,131,130,129,128,127,126,125,123,122,121,120,119,118,117, * if (!(nr & 0xF00000)) { nr <<= 4; idx += 4; }
// 116,115,114,113,112,111,110,109,108,107,106,105,104,103,102,101, * if (!(nr & 0xC00000)) { nr <<= 2; idx += 2; }
// 100,99,98,97,96,95,94,93,92,91,90,89,88,88,87,86, * if (!(nr & 0x800000)) { nr <<= 1; idx += 1; }
// 85,84,83,82,81,80,80,79,78,77,76,75,74,74,73,72, *
// 71,70,70,69,68,67,66,66,65,64,63,62,62,61,60,59, * // Isolate top 9 bits of the denominator, to be used as index into the initial estimation table
// 59,58,57,56,56,55,54,53,53,52,51,50,50,49,48,48, * uint32_t tidx = nr >> 15, // top 9 bits. bit8 is always set
// 47,46,46,45,44,43,43,42,41,41,40,39,39,38,37,37, * ie = inv_tab[tidx & 0xFF] + 256, // Get the table value. bit9 is always set
// 36,35,35,34,33,33,32,32,31,30,30,29,28,28,27,27, * x = idx <= 8 ? (ie >> (8 - idx)) : (ie << (idx - 8)); // Position the estimation at the proper place
// 26,25,25,24,24,23,22,22,21,21,20,19,19,18,18,17, *
// 17,16,15,15,14,14,13,13,12,12,11,10,10,9,9,8, * x = uint32_t((x * uint64_t(_BV(25) - x * d)) >> 24); // Refine estimation by newton-raphson. 1 iteration is enough
// 8,7,7,6,6,5,5,4,4,3,3,2,2,1,0,0 * const uint32_t r = _BV(24) - x * d; // Estimate remainder
// }; * if (r >= d) x++; // Check whether to adjust result
// * return uint32_t(x); // x holds the proper estimation
// // For small denominators, it is cheaper to directly store the result, *
// // because those denominators would require 2 Newton-Raphson iterations */
// // to converge to the required result precision. For bigger ones, just
// // ONE Newton-Raphson iteration is enough to get maximum precision!
// static const uint32_t small_inv_tab[111] PROGMEM = {
// 16777216,16777216,8388608,5592405,4194304,3355443,2796202,2396745,2097152,1864135,1677721,1525201,1398101,1290555,1198372,1118481,
// 1048576,986895,932067,883011,838860,798915,762600,729444,699050,671088,645277,621378,599186,578524,559240,541200,
// 524288,508400,493447,479349,466033,453438,441505,430185,419430,409200,399457,390167,381300,372827,364722,356962,
// 349525,342392,335544,328965,322638,316551,310689,305040,299593,294337,289262,284359,279620,275036,270600,266305,
// 262144,258111,254200,250406,246723,243148,239674,236298,233016,229824,226719,223696,220752,217885,215092,212369,
// 209715,207126,204600,202135,199728,197379,195083,192841,190650,188508,186413,184365,182361,180400,178481,176602,
// 174762,172960,171196,169466,167772,166111,164482,162885,161319,159783,158275,156796,155344,153919,152520
// };
//
// // For small divisors, it is best to directly retrieve the results
// if (d <= 110)
// return pgm_read_dword(&small_inv_tab[d]);
//
// // Compute initial estimation of 0x1000000/x -
// // Get most significant bit set on divider
// uint8_t idx = 0;
// uint32_t nr = d;
// if (!(nr & 0xFF0000)) {
// nr <<= 8;
// idx += 8;
// if (!(nr & 0xFF0000)) {
// nr <<= 8;
// idx += 8;
// }
// }
// if (!(nr & 0xF00000)) {
// nr <<= 4;
// idx += 4;
// }
// if (!(nr & 0xC00000)) {
// nr <<= 2;
// idx += 2;
// }
// if (!(nr & 0x800000)) {
// nr <<= 1;
// idx += 1;
// }
//
// // Isolate top 9 bits of the denominator, to be used as index into the initial estimation table
// uint32_t tidx = nr >> 15; // top 9 bits. bit8 is always set
// uint32_t ie = inv_tab[tidx & 0xFF] + 256; // Get the table value. bit9 is always set
// uint32_t x = idx <= 8 ? (ie >> (8 - idx)) : (ie << (idx - 8)); // Position the estimation at the proper place
//
// // Now, refine estimation by newton-raphson. 1 iteration is enough
// x = uint32_t((x * uint64_t((1 << 25) - x * d)) >> 24);
//
// // Estimate remainder
// uint32_t r = (1 << 24) - x * d;
//
// // Check if we must adjust result
// if (r >= d) x++;
//
// // x holds the proper estimation
// return uint32_t(x);
// }
//
static uint32_t get_period_inverse(uint32_t d) { static uint32_t get_period_inverse(uint32_t d) {
static const uint8_t inv_tab[256] PROGMEM = { static const uint8_t inv_tab[256] PROGMEM = {
@ -409,13 +350,12 @@ void Planner::init() {
}; };
// For small divisors, it is best to directly retrieve the results // For small divisors, it is best to directly retrieve the results
if (d <= 110) if (d <= 110) return pgm_read_dword(&small_inv_tab[d]);
return pgm_read_dword(&small_inv_tab[d]);
register uint8_t r8 = d & 0xFF; register uint8_t r8 = d & 0xFF,
register uint8_t r9 = (d >> 8) & 0xFF; r9 = (d >> 8) & 0xFF,
register uint8_t r10 = (d >> 16) & 0xFF; r10 = (d >> 16) & 0xFF,
register uint8_t r2,r3,r4,r5,r6,r7,r11,r12,r13,r14,r15,r16,r17,r18; r2,r3,r4,r5,r6,r7,r11,r12,r13,r14,r15,r16,r17,r18;
register const uint8_t* ptab = inv_tab; register const uint8_t* ptab = inv_tab;
__asm__ __volatile__( __asm__ __volatile__(

Write Preview
Loading…
Cancel
Save