Subversion Repositories FlightCtrl

Rev

Rev 214 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 214 Rev 226
Line 11... Line 11...
11 
*/
 11 
*/
Line 12... Line 12...
12 
 
 12 
 
Line 13... Line 13...
13 
#include "main.h"
 13 
#include "main.h"
14 
 
 14 
 
15 
//-----------------------------------------------
 15 
//-----------------------------------------------
16 
// Fast arctan2 with max error of .01 rads
 16 
// Fast arctan2 with max error of .07 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 
float arctan_f(signed int x, signed int y)
 19 
signed char arctan_f(signed int x, signed int y)
20 
{  
 20 
{  
- 
 
 21 
        float rad,r;
- 
 
 22 
        short int m;
21 
   float rad,r;
 23 
        #define coeff_1         0.7854
Line 22... Line 24...
22 
   signed int angle;
 24 
        #define coeff_2         2.3562
Line 23... Line 25...
23 
   short int m;  
 25 
        #define rad2grad        57.2958  
24 
   
 26 
   
Line 25... Line 27...
25 
   if (!x && !y) return 0;
 27 
   if (!x && !y) return 0;
26 
   
 28 
   
27 
   if (y < 0) {y = abs(y); m = -1;}
 29 
   if (y < 0) {y = abs(y); m = -1;}
28 
   else m = 1;
 30 
   else m = 1;
29 
   
 31 
   
30 
   if (x>=0)
 32 
   if (x>=0)
31 
   {
 33 
   {
32 
      r = (x - y) / (x + y);
 34 
      r = (float)(x - y) / (float)(x + y);
33 
      rad = 0.1963*r*r*r - 0.9817*r + 0.7854;
 35 
      rad = coeff_1 - coeff_1*r;
34 
   }
 36 
   }
35 
   else
 37 
   else
- 
 
 38 
   {
- 
 
 39 
      r = (float)(x + y) / (float)(y - x);
36 
   {
 40 
      rad = coeff_2 - coeff_1*r;
37 
      r = (x + y) / (y - x);
 41 
   }
Line 38... Line 42...
38 
      rad = 0.1963*r*r*r - 0.9817*r + 2.3562;
 42 
   
39 
   }
 43 
   rad *= rad2grad;
40 
     
 44 
   
41 
   return(rad*m);
 45 
   return(rad*m);
42 
}
 46 
}
43 
 
 47 
 
44 
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
45 
Peter Muehlenbrock
 48 
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
46 
arctan in brute-force Art
 49 
Peter Muehlenbrock
47 
Stand 1.10.2007
 50 
arctan in brute-force Art
48 
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 51 
Stand 1.10.2007
49 
*/
 52 
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
50 
 
 53 
*/
51 
// arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad
 54 
// arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad
Line 65... Line 68...
65 
        }
 68 
        }
Line 66... Line 69...
66 
       
 69 
       
Line 67... Line 70...
67 
        // Quadranten ermitteln
 70 
        // Quadranten ermitteln
68 
 
 71 
 
- 
 
 72 
        // Wert durch lineare Interpolation ermitteln
69 
        // Wert durch lineare Interpolation ermitteln
73 
        if (!y && !x)  return 0; // Division durch 0 nicht erlaubt
Line 70... Line 74...
70 
        if ((y == 0) && (x == 0))  wert =1; // Division durch 0 nicht erlaubt
 74 
       
71 
        else wert= abs((x*1000)/y);
 75 
        else wert= abs((x*1000)/y);
72 
 
 76 
 
Line 88... Line 92...
88 
        else if ((x < 0) && (y >= 0)) return_value = - return_value;
 92 
        else if ((x < 0) && (y >= 0)) return_value = - return_value;
Line 89... Line 93...
89 
 
 93 
 
90 
return return_value;
 94 
return return_value;
Line 91... Line -...
91 
}
 - 
 
92 
 
 - 
 
93 
// cosinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =cos(winkel)*1000
 - 
 
94 
signed int cos_i(signed int winkel)
 - 
 
95 
{
 - 
 
96 
  winkel = sin_i(90-winkel);
 - 
 
Line 97... Line 95...
97 
  return winkel;
 95 
}
Line -... Line 96...
- 
 
 96 
 
- 
 
 97 
 
- 
 
 98 
 
- 
 
 99 
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};
- 
 
 100 
 
- 
 
 101 
// cosinus Funktion: Eingabewert Winkel in Grad, Rueckgabe =cos(winkel)*1000
98 
}
 102 
signed int cos_i(signed int winkel)
99 
 
 103 
{
100 
 
 104 
  return (sin_i(90-winkel));
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};
 105 
}
Line 124... Line 128...
124 
 
 128 
 
125 
return (winkel*m*n);
 129 
return (winkel*m*n);
Line -... Line 130...
- 
 
 130 
}
- 
 
 131 
 
- 
 
 132 
 
- 
 
 133 
 
- 
 
 134 
const float pgm_sinus_f [91] PROGMEM = {0.00,0.02,0.03,0.05,0.07,0.09,0.10,0.12,0.14,0.16,0.17,0.19,0.21,0.23,0.24,0.26,0.28,0.29,0.31,0.33,0.34,0.36,0.38,0.39,0.41,0.42,0.44,0.45,0.47,0.49,0.50,0.52,0.53,0.55,0.56,0.57,0.59,0.60,0.62,0.63,0.64,0.66,0.67,0.68,0.70,0.71,0.72,0.73,0.74,0.76,0.77,0.78,0.79,0.80,0.81,0.82,0.83,0.84,0.85,0.86,0.87,0.88,0.88,0.89,0.90,0.91,0.91,0.92,0.93,0.93,0.94,0.95,0.95,0.96,0.96,0.97,0.97,0.97,0.98,0.98,0.98,0.99,0.99,0.99,0.99,1.00,1.00,1.00,1.00,1.00,1.00};
- 
 
 135 
 
- 
 
 136 
inline float pgm_read_float(const float *addr)
- 
 
 137 
{      
- 
 
 138 
        union
- 
 
 139 
        {
- 
 
 140 
                uint16_t i[2];  // 2 16-bit-Worte
- 
 
 141 
                float f;
- 
 
 142 
        } u;
- 
 
 143 
       
- 
 
 144 
        u.i[0]=pgm_read_word((PGM_P)addr);
- 
 
 145 
        u.i[1]=pgm_read_word((PGM_P)addr+2);
- 
 
 146 
       
- 
 
 147 
        return u.f;
- 
 
 148 
}
- 
 
 149 
 
- 
 
 150 
// cosinus Funktion: Eingabewert Winkel in Grad
- 
 
 151 
float cos_f(signed int winkel)
- 
 
 152 
{
- 
 
 153 
 return (sin_f(90-winkel));
- 
 
 154 
}
- 
 
 155 
 
- 
 
 156 
// sinus Funktion: Eingabewert Winkel in Grad
- 
 
 157 
float sin_f(signed int winkel)
- 
 
 158 
{
- 
 
 159 
 short int m,n;
- 
 
 160 
 float sinus;
- 
 
 161 
 
- 
 
 162 
 //winkel = winkel % 360;
- 
 
 163 
 
- 
 
 164 
 if (winkel < 0)
- 
 
 165 
 {
- 
 
 166 
        m = -1;
- 
 
 167 
        winkel = abs(winkel);
- 
 
 168 
 }
- 
 
 169 
 else m = +1;
- 
 
 170 
 
- 
 
 171 
 // Quadranten auswerten
- 
 
 172 
 if ((winkel > 90 ) && (winkel <= 180)) {winkel = 180 - winkel; n = 1;}
- 
 
 173 
 else if ((winkel > 180 ) && (winkel <= 270)) {winkel = winkel - 180; n = -1;}
- 
 
 174 
 else if ((winkel > 270) && (winkel <= 360)) {winkel = 360 - winkel; n = -1;}
- 
 
 175 
 else n = 1; //0 - 90 Grad
- 
 
 176 
 
- 
 
 177 
sinus = pgm_read_float(&pgm_sinus_f[winkel]);