Subversion Repositories FlightCtrl

Rev

Rev 205 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 205 Rev 214
Line 14... Line 14...
14 
 
 14 
 
15 
//-----------------------------------------------
 15 
//-----------------------------------------------
16 
// Fast arctan2 with max error of .01 rads
 16 
// Fast arctan2 with max error of .01 rads
17 
// http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
 17 
// http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
18 
//-----------------------------------------------
 18 
//-----------------------------------------------
19 
signed int arctan_i(signed int x, signed int y)
 19 
float arctan_f(signed int x, signed int y)
20 
{  
 20 
{  
21 
   float rad,r;
 21 
   float rad,r;
22 
   signed int angle;
 22 
   signed int angle;
Line 35... Line 35...
35 
   else
 35 
   else
36 
   {
 36 
   {
37 
      r = (x + y) / (y - x);
 37 
      r = (x + y) / (y - x);
38 
      rad = 0.1963*r*r*r - 0.9817*r + 2.3562;
 38 
      rad = 0.1963*r*r*r - 0.9817*r + 2.3562;
39 
   }
 39 
   }
40 
   
 - 
 
41 
   angle = (180 * rad) / 3.1416;
 - 
 
Line 42... Line 40...
42 
     
 40 
     
- 
 
 41 
   return(rad*m);
- 
 
 42 
}
- 
 
 43 
 
- 
 
 44 
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 45 
Peter Muehlenbrock
- 
 
 46 
arctan in brute-force Art
- 
 
 47 
Stand 1.10.2007
- 
 
 48 
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 49 
*/
- 
 
 50 
 
- 
 
 51 
// arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad
- 
 
 52 
int8_t arctan_i( signed int x,  signed int y)
- 
 
 53 
{
- 
 
 54 
        short int change_xy = 0;
- 
 
 55 
        signed int i;
- 
 
 56 
        long signed int wert;
- 
 
 57 
        int return_value;
- 
 
 58 
 
- 
 
 59 
        if ((abs(x)) > (abs(y)))  // x,y Werte vertauschen damit arctan <45 grad bleibt
- 
 
 60 
        {      
- 
 
 61 
                change_xy       = 1;
- 
 
 62 
                i                       = x;
- 
 
 63 
                x                       = y;
- 
 
 64 
                y                       = i;
- 
 
 65 
        }
- 
 
 66 
       
- 
 
 67 
        // Quadranten ermitteln
- 
 
 68 
 
- 
 
 69 
        // Wert durch lineare Interpolation ermitteln
- 
 
 70 
        if ((y == 0) && (x == 0))  wert =1; // Division durch 0 nicht erlaubt
- 
 
 71 
        else wert= abs((x*1000)/y);
- 
 
 72 
 
- 
 
 73 
        if (wert <=268) //0...0.0,268  entsprechend 0..15 Grad
- 
 
 74 
        {
- 
 
 75 
                return_value = (signed int)((wert*100)/(268-0)*(15-0)/100) +0;
- 
 
 76 
        }
- 
 
 77 
        else if (wert <=578) //0,268...0.0,568  entsprechend 15..30 Grad
- 
 
 78 
        {      
- 
 
 79 
                return_value = (signed int)((((wert-268)*100)/(578-268)*(30-15))/100) +15;             
- 
 
 80 
        }
- 
 
 81 
        else //0,568...1  entsprechend 30..45 Grad
- 
 
 82 
        {      
- 
 
 83 
                return_value = (signed int)((((wert-578)*50)/(1000-578)*(45-30))/50) +30;              
- 
 
 84 
        }
- 
 
 85 
 
- 
 
 86 
        if (change_xy == 0)  return_value = 90-return_value; //Quadrant 45..90 Grad
- 
 
 87 
        if ((x >= 0) && (y <0)) return_value = - return_value;
- 
 
 88 
        else if ((x < 0) && (y >= 0)) return_value = - return_value;
- 
 
 89 
 
43 
   return(angle*m);
 90 
return return_value;
Line 44... Line 91...
44 
}
 91 
}
45 
 
 92 
 
46 
// cosinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =cos(winkel)*1000
 93 
// cosinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =cos(winkel)*1000
47 
signed int cos_i(signed int winkel)
 94 
signed int cos_i(signed int winkel)
48 
{
 95 
{
49 
  winkel = sin_i(90-winkel);
 96 
  winkel = sin_i(90-winkel);
Line 50... Line 97...
50 
  return winkel;
 97 
  return winkel;
Line 51... Line 98...
51 
}
 98 
}
52 
 
 99 
 
53 
 
 100 
 
54 
const unsigned int pgm_sinus[92] PROGMEM = {0,0,17,35,52,70,87,105,122,139,156,174,191,208,225,242,259,276,292,309,326,342,358,375,391,407,423,438,454,469,485,500,515,530,545,559,574,588,602,616,629,643,656,669,682,695,707,719,731,743,755,766,777,788,799,809,819,829,839,848,857,866,875,883,891,899,906,914,921,927,934,940,946,951,956,961,966,970,974,978,982,985,988,990,993,995,996,998,999,999,1000,1000};
 101 
const unsigned int pgm_sinus[91] PROGMEM = {0,17,35,52,70,87,105,122,139,156,174,191,208,225,242,259,276,292,309,326,342,358,375,391,407,423,438,454,469,485,500,515,530,545,559,574,588,602,616,629,643,656,669,682,695,707,719,731,743,755,766,777,788,799,809,819,829,839,848,857,866,875,883,891,899,906,914,921,927,934,940,946,951,956,961,966,970,974,978,982,985,988,990,993,995,996,998,999,999,1000,1000};
Line 74... Line 121...
74 
 else n = 1; //0 - 90 Grad
 121 
 else n = 1; //0 - 90 Grad
Line 75... Line 122...
75 
 
 122 
 
Line 76... Line 123...
76 
winkel = pgm_read_word(&pgm_sinus[winkel]);
 123 
winkel = pgm_read_word(&pgm_sinus[winkel]);
77 
 
 - 
 
78 
return (winkel*m*n);
 124