faster _atan2 - saves about 0.13 ms cycletime

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
Post Reply
happul3
Posts: 44
Joined: Mon Apr 21, 2014 1:54 am

faster _atan2 - saves about 0.13 ms cycletime

Post by happul3 »

Bench and flight tested, but only briefly. Use at your own risk.

Code: Select all

//lut-based atan2, returns angle in 0.1 degree units, granularity varies between 0.1 and 0.2 degrees, max absolute error 0.05 degrees

int16_t atan2_tbl[256] = {0,2,4,7,9,11,13,16,18,20,22,25,27,29,31,34,36,38,40,42,45,47,49,51,54,56,58,60,62,
65,67,69,71,73,76,78,80,82,84,87,89,91,93,95,98,100,102,104,106,108,111,113,115,117,119,121,123,126,128,130,
132,134,136,138,140,142,145,147,149,151,153,155,157,159,161,163,165,167,169,171,174,176,178,180,182,184,186,
188,190,192,194,196,198,200,202,204,206,208,209,211,213,215,217,219,221,223,225,227,229,231,233,234,236,238,
240,242,244,246,247,249,251,253,255,257,258,260,262,264,266,267,269,271,273,275,276,278,280,282,283,285,287,
288,290,292,294,295,297,299,300,302,304,305,307,309,310,312,314,315,317,318,320,322,323,325,326,328,330,331,
333,334,336,337,339,341,342,344,345,347,348,350,351,353,354,356,357,359,360,361,363,364,366,367,369,370,372,
373,374,376,377,379,380,381,383,384,386,387,388,390,391,392,394,395,396,398,399,400,402,403,404,405,407,408,
409,411,412,413,414,416,417,418,419,421,422,423,424,426,427,428,429,430,432,433,434,435,436,437,439,440,441,
442,443,444,445,447,448,449};

typedef union {
  int32_t i;
  uint32_t ui;
  unsigned char b[4];
}
i2b ;

int16_t _atan2(int32_t y, int32_t x) { 
  int8_t ny=0, nx=0;
  int16_t result;
  i2b xx,yy;
  if(y < 0) {
    y = -y;
    ny=1;
  }
  if(x < 0) {
    x = -x;
    nx=1;
  }
  xx.i=x; yy.i=y; //assigments do not emit assembly code, yet allow addressing individual byte in int32s shaving off about 10 cycles
  if(y==x)
    result=450;
  else if(x>y) { // 0-45 octant, we want y/(x/256) or (y*256)/x
    if(xx.b[3])   //shortest, emits only 2 instructions (and, breq); alternatives like if(x&0xff000000) or if((uint_32)x>>24) emit at least 8 instructions
          result=atan2_tbl[(uint8_t)(y/(x>>8))]; 
    else           
          result=atan2_tbl[(uint8_t)((y<<8)/x)]; // safe because y<x<=0xffff 
  }
  else { // 45-90 octant, we want x/(y/256) or (x*256)/y
    if(yy.b[3])
          result=atan2_tbl[(uint8_t)(x/(y>>8))];   
    else           
          result=atan2_tbl[(uint8_t)((x<<8)/y)]; 
    result = 900 - result;
  }
  if(ny)
    if(nx) result -= 1800;
    else result=-result; 
  else
    if(nx) result = 1800-result;

  return result;
}


User avatar
Hamburger
Posts: 2578
Joined: Tue Mar 01, 2011 2:14 pm
Location: air
Contact:

Re: faster _atan2 - saves about 0.13 ms cycletime

Post by Hamburger »

before we get excited - where does this come from, please? Under what licence?

happul3
Posts: 44
Joined: Mon Apr 21, 2014 1:54 am

Re: faster _atan2 - saves about 0.13 ms cycletime

Post by happul3 »

I wrote it from scratch. Have not thought about license. What's the least restrictive terms? Pubic domain, right?

If that's the case, I declare it pubic domain :)

Post Reply