josh
/
Teacup_Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added integer square root algorithm for future use

This commit is contained in:
Michael Moon 2011-02-24 18:49:28 +11:00
parent 4ca9e470a5
commit 4aefda6f17
1 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
dda.c
View File

@ -100,6 +100,25 @@ uint32_t approx_distance_3( uint32_t dx, uint32_t dy, uint32_t dz )
return (( approx + 512 ) >> 10 );
}
// courtesy of http://www.embedded-systems.com/98/9802fe2.htm
uint16_t int_sqrt(uint32_t a) {
uint32_t rem = 0;
uint32_t root = 0;
for (uint16_t i = 0; i < 16; i++) {
root <<= 1;
rem = ((rem << 2) + (a >> 30));
a <<= 2;
root++;
if (root <= rem) {
rem -= root;
root++;
}
else
root--;
}
return (uint16_t) ((root >> 1) & 0xFFFFL);
}
// this is an ultra-crude pseudo-logarithm routine, such that:
// 2 ^ msbloc(v) >= v
const uint8_t msbloc (uint32_t v) {