The Fast and the Furious World (of Trigonometric Functions on ATMega328)
Atan2 algorithms - speed vs accuracy
Running trigonometric functions on an 8-bit AVR is inevitably going to be slow. Floating point operations come with speed overheads and the most accurate algorithms usually come at a further timing price. The arctangent function (atan/atan2) is ubiquitously useful, and especially so for Quadcopter's and robotics. Briefly, an atan2 function takes Y and X as arguments and works out which quadrant the tangent angle is in, and uses that to call a corresponding atan function. I have been investigated the timings of various trigonometric functions on an ATMega328p chip running at 16 MHz, and in particular exploring 4 atan2 implementations and their trade-offs for accuracy.
Above: Accuracy of 4 atan2 implementations (Lyons, Algorithm A, Algorithm B, EEPROM lookup table arctan2 implementation). The Y-axis shows the deviation of each function from Excel's atan2 function. The inset table shows various timings I've measured for the various atan2 functions and, for reference, other standard trig functions. The values shown are the average time for a single call, averaged over a full circle of angles (0-360).
inline double Lyons_atan(double ratio)
{
return ratio/(1+0.28125*ratio*ratio);
}
inline double Algorithm_atan_A(double ratio)
{
return (M_PI/4 + 0.273*(1-fabs(ratio)))*ratio;
}
inline double Algorithm_atan_B(double ratio)
{
return ratio*(M_PI/4 - (fabs(ratio)-1)*(0.2447+0.663*fabs(ratio)));
}
Note that the parameter ratio must be between 0-1, and is handled by calling atan2. The atan2 funtions then wrap around the corresponding atan functions using a macro definition like so:
#define atan2_wrapper(TANFUNC, ANGLE90, ANGLE180) \
uint8_t XY, Xpos, Ypos;\
XY = fabs(X)>=fabs(Y); Xpos = X>0; Ypos = Y>0; uint8_t octant = (XY<<2)|(Xpos<<1)|Ypos;\
switch (octant)\
{\
case 0: return -ANGLE90-TANFUNC(X/Y);\
case 1: return ANGLE90+TANFUNC(-X/Y);\
case 2: return -ANGLE90+TANFUNC(-X/Y);\
case 3: return ANGLE90-TANFUNC(X/Y);\
case 4: return -ANGLE180+TANFUNC(Y/X);\
case 5: return ANGLE180-TANFUNC(-Y/X);\
case 6: return -TANFUNC(-Y/X);\
case 7: return TANFUNC(Y/X);\
}\
return 0;\
{
atan2_wrapper(Algorithm_atan_A, M_PI/2, M_PI);
}
The accuracy of any of the functions is probably suitable for a quadcopter - I've had no issues flying with the Lyons atan2 function. Further optimisation could be done to convert from floating point to fixed point.
double? atanf is your best frined!
ReplyDelete