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

Move utility functions from dda.c to dda_math.c. 

Simple cleanup, no functional change.
This commit is contained in:
Markus Hitter 2012-06-27 01:45:41 +02:00
parent e0959f7ce2
commit 81f85b018d
4 changed files with 125 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

121
dda.c
View File

@ -50,127 +50,6 @@ TARGET current_position __attribute__ ((__section__ (".bss")));
/// \brief numbers for tracking the current state of movement
MOVE_STATE move_state __attribute__ ((__section__ (".bss")));
/*
utility functions
*/
// courtesy of http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml
/*! linear approximation 2d distance formula
\param dx distance in X plane
\param dy distance in Y plane
\return 3-part linear approximation of \f$\sqrt{\Delta x^2 + \Delta y^2}\f$
see http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml
*/
uint32_t approx_distance( uint32_t dx, uint32_t dy )
{
uint32_t min, max, approx;
if ( dx < dy )
{
min = dx;
max = dy;
} else {
min = dy;
max = dx;
}
approx = ( max * 1007 ) + ( min * 441 );
if ( max < ( min << 4 ))
approx -= ( max * 40 );
// add 512 for proper rounding
return (( approx + 512 ) >> 10 );
}
// courtesy of http://www.oroboro.com/rafael/docserv.php/index/programming/article/distance
/*! linear approximation 3d distance formula
\param dx distance in X plane
\param dy distance in Y plane
\param dz distance in Z plane
\return 3-part linear approximation of \f$\sqrt{\Delta x^2 + \Delta y^2 + \Delta z^2}\f$
see http://www.oroboro.com/rafael/docserv.php/index/programming/article/distance
*/
uint32_t approx_distance_3( uint32_t dx, uint32_t dy, uint32_t dz )
{
uint32_t min, med, max, approx;
if ( dx < dy )
{
min = dy;
med = dx;
} else {
min = dx;
med = dy;
}
if ( dz < min )
{
max = med;
med = min;
min = dz;
} else if ( dz < med ) {
max = med;
med = dz;
} else {
max = dz;
}
approx = ( max * 860 ) + ( med * 851 ) + ( min * 520 );
if ( max < ( med << 1 )) approx -= ( max * 294 );
if ( max < ( min << 2 )) approx -= ( max * 113 );
if ( med < ( min << 2 )) approx -= ( med * 40 );
// add 512 for proper rounding
return (( approx + 512 ) >> 10 );
}
/*!
integer square root algorithm
\param a find square root of this number
\return sqrt(a - 1) < returnvalue <= sqrt(a)
see http://www.embedded-systems.com/98/9802fe2.htm
*/
// 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;
uint16_t i;
for (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
/*! crude logarithm algorithm
\param v value to find \f$log_2\f$ of
\return floor(log(v) / log(2))
*/
const uint8_t msbloc (uint32_t v) {
uint8_t i;
uint32_t c;
for (i = 31, c = 0x80000000; i; i--) {
if (v & c)
return i;
c >>= 1;
}
return 0;
}
/*! Inititalise DDA movement structures
*/
void dda_init(void) {

6
dda.h
View File

@ -159,12 +159,6 @@ extern TARGET current_position;
methods
*/
uint32_t approx_distance( uint32_t dx, uint32_t dy ) __attribute__ ((hot));
uint32_t approx_distance_3( uint32_t dx, uint32_t dy, uint32_t dz ) __attribute__ ((hot));
// const because return value is always the same given the same v
const uint8_t msbloc (uint32_t v) __attribute__ ((const));
// initialize dda structures
void dda_init(void);

113
dda_maths.c
View File

@ -1,6 +1,6 @@
/** \file
\brief Mathematic algorithms for the digital differential analyser (DDA).
\brief Mathematic algorithms for the digital differential analyser (DDA).
*/
#include "dda_maths.h"
@ -63,3 +63,114 @@ const int32_t muldivQR(int32_t multiplicand, uint32_t qn, uint32_t rn,
return negative_flag ? -((int32_t)quotient) : (int32_t)quotient;
}
// courtesy of http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml
/*! linear approximation 2d distance formula
\param dx distance in X plane
\param dy distance in Y plane
\return 3-part linear approximation of \f$\sqrt{\Delta x^2 + \Delta y^2}\f$
see http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml
*/
uint32_t approx_distance(uint32_t dx, uint32_t dy) {
uint32_t min, max, approx;
if ( dx < dy ) {
min = dx;
max = dy;
} else {
min = dy;
max = dx;
}
approx = ( max * 1007 ) + ( min * 441 );
if ( max < ( min << 4 ))
approx -= ( max * 40 );
// add 512 for proper rounding
return (( approx + 512 ) >> 10 );
}
// courtesy of http://www.oroboro.com/rafael/docserv.php/index/programming/article/distance
/*! linear approximation 3d distance formula
\param dx distance in X plane
\param dy distance in Y plane
\param dz distance in Z plane
\return 3-part linear approximation of \f$\sqrt{\Delta x^2 + \Delta y^2 + \Delta z^2}\f$
see http://www.oroboro.com/rafael/docserv.php/index/programming/article/distance
*/
uint32_t approx_distance_3(uint32_t dx, uint32_t dy, uint32_t dz) {
uint32_t min, med, max, approx;
if ( dx < dy ) {
min = dy;
med = dx;
} else {
min = dx;
med = dy;
}
if ( dz < min ) {
max = med;
med = min;
min = dz;
} else if ( dz < med ) {
max = med;
med = dz;
} else {
max = dz;
}
approx = ( max * 860 ) + ( med * 851 ) + ( min * 520 );
if ( max < ( med << 1 )) approx -= ( max * 294 );
if ( max < ( min << 2 )) approx -= ( max * 113 );
if ( med < ( min << 2 )) approx -= ( med * 40 );
// add 512 for proper rounding
return (( approx + 512 ) >> 10 );
}
/*!
integer square root algorithm
\param a find square root of this number
\return sqrt(a - 1) < returnvalue <= sqrt(a)
see http://www.embedded-systems.com/98/9802fe2.htm
*/
// 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;
uint16_t i;
for (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
/*! crude logarithm algorithm
\param v value to find \f$log_2\f$ of
\return floor(log(v) / log(2))
*/
const uint8_t msbloc (uint32_t v) {
uint8_t i;
uint32_t c;
for (i = 31, c = 0x80000000; i; i--) {
if (v & c)
return i;
c >>= 1;
}
return 0;
}

13
dda_maths.h
View File

@ -50,4 +50,17 @@ inline int32_t um_to_steps_e(int32_t distance) {
STEPS_PER_M_E % 1000000UL, 1000000UL);
}
// approximate 2D distance
uint32_t approx_distance(uint32_t dx, uint32_t dy);
// approximate 3D distance
uint32_t approx_distance_3(uint32_t dx, uint32_t dy, uint32_t dz);
// integer square root algorithm
uint16_t int_sqrt(uint32_t a);
// this is an ultra-crude pseudo-logarithm routine, such that:
// 2 ^ msbloc(v) >= v
const uint8_t msbloc (uint32_t v);
#endif /* _DDA_MATHS_H */