Subversion Repositories FlightCtrl

Rev

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

Rev 229 Rev 232
Line 10... Line 10...
10 
Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de
 10 
Please note: All the other files for the project "Mikrokopter" by H.Buss are under the license (license_buss.txt) published by www.mikrokopter.de
11 
*/
 11 
*/
Line 12... Line 12...
12 
 
 12 
 
Line -... Line 13...
- 
 
 13 
#include "main.h"
13 
#include "main.h"
 14 
 
14 
 
 15 
/*
15 
//-----------------------------------------------
 16 
//-----------------------------------------------
16 
// Fast arctan2 with max error of .07 rads
 17 
// Fast arctan2 with max error of .07 rads
17 
// http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
 18 
// http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
Line 42... Line 43...
42 
   
 43 
   
Line 43... Line 44...
43 
   rad *= rad2grad;
 44 
   rad *= rad2grad;
44 
   
 45 
   
- 
 
 46 
   return(rad*m);
- 
 
 47 
}
- 
 
 48 
*/
Line 45... Line -...
45 
   return(rad*m);
 - 
 
46 
}
 - 
 
47 
 
 - 
 
48 
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
49 
Peter Muehlenbrock
 - 
 
50 
arctan in brute-force Art
 - 
 
51 
Stand 1.10.2007
 - 
 
52 
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 49 
 
53 
*/
 50 
const uint8_t pgm_atan[270] PROGMEM = {0,1,2,3,5,6,7,8,9,10,11,12,13,15,16,17,18,19,20,21,22,23,24,25,26,27,27,28,29,30,31,32,33,33,34,35,36,37,37,38,39,39,40,41,41,42,43,43,44,44,45,46,46,47,47,48,48,49,49,50,50,51,51,52,52,52,53,53,54,54,54,55,55,56,56,56,57,57,57,58,58,58,59,59,59,60,60,60,60,61,61,61,61,62,62,62,62,63,63,63,63,64,64,64,64,65,65,65,65,65,66,66,66,66,66,67,67,67,67,67,67,68,68,68,68,68,68,69,69,69,69,69,69,69,70,70,70,70,70,70,70,70,71,71,71,71,71,71,71,71,72,72,72,72,72,72,72,72,72,73,73,73,73,73,73,73,73,73,73,74,74,74,74,74,74,74,74,74,74,74,74,75,75,75,75,75,75,75,75,75,75,75,75,75,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79};
54 
// arctan Funktion: Eingabewert x,y Rueckgabe =arctan(x,y) in grad
 - 
 
55 
signed int arctan_i(signed int x,  signed int y)
 - 
 
56 
{
 - 
 
57 
        short int change_xy = 0;
 51 
 
58 
        signed int i;
 - 
 
59 
        long signed int wert;
 - 
 
60 
        int return_value;
 - 
 
61 
 
 - 
 
62 
        if ((abs(x)) > (abs(y)))  // x,y Werte vertauschen damit arctan <45 grad bleibt
 52 
signed int arctan_i(signed int x,  signed int y)
63 
        {      
 - 
 
64 
                change_xy       = 1;
 - 
 
65 
                i                       = x;
 - 
 
Line 66... Line -...
66 
                x                       = y;
 - 
 
67 
                y                       = i;
 - 
 
68 
        }
 - 
 
69 
       
 53 
{
Line -... Line 54...
- 
 
 54 
        int i,angle;
70 
        // Quadranten ermitteln
 55 
        int8_t m;
71 
 
 56 
       
72 
        // Wert durch lineare Interpolation ermitteln
57 
        if (!x && !y) return 0;         //atan2 = 0 für x und y = 0
73 
        if (!y && !x)  return 0; // Division durch 0 nicht erlaubt
 - 
 
74 
       
 - 
 
75 
        else wert= abs((x*1000)/y);
 - 
 
76 
 
 - 
 
77 
        if (wert <=268) //0...0.0,268  entsprechend 0..15 Grad
 - 
 
78 
        {
 - 
 
79 
                return_value = (signed int)((wert*100)/(268-0)*(15-0)/100) +0;
 - 
 
80 
        }
 - 
 
81 
        else if (wert <=578) //0,268...0.0,568  entsprechend 15..30 Grad
 - 
 
82 
        {      
 - 
 
83 
                return_value = (signed int)((((wert-268)*100)/(578-268)*(30-15))/100) +15;             
 - 
 
84 
        }
 58 
       
85 
        else //0,568...1  entsprechend 30..45 Grad
 59 
        if (y < 0) m=-1;
86 
        {      
 - 
 
87 
                return_value = (signed int)((((wert-578)*50)/(1000-578)*(45-30))/50) +30;              
 - 
 
Line -... Line 60...
- 
 
 60 
        else m=1;
- 
 
 61 
       
- 
 
 62 
        if (x==0) return (90*m);                // atan2 = 90° für x = 0
- 
 
 63 
       
- 
 
 64 
        i = abs(((float)y / x) * 50.0);         // Berechne i für die Lookup table (Schrittweite atan(x) ist 0.02 -> *50)
- 
 
 65 
 
88 
        }
 66 
        if (i<270) angle = pgm_read_byte(&pgm_atan[i]); // Lookup für 1° bis 79°
- 
 
 67 
        else if (i>5750) angle = 90;                                            // Grenzwert ist 90°
- 
 
 68 
        else if (i>=1910) angle = 89;                                           // 89° bis 80° über Wertebereiche
- 
 
 69 
        else if (i>=1150) angle = 88;
- 
 
 70 
        else if (i>=820) angle = 87;
- 
 
 71 
        else if (i>=640) angle = 86;
- 
 
 72 
        else if (i>=520) angle = 85;
- 
 
 73 
        else if (i>=440) angle = 84;
- 
 
 74 
        else if (i>=380) angle = 83;
- 
 
 75 
        else if (i>=335) angle = 82;
89 
 
 76 
        else if (i>=299) angle = 81;
Line 145... Line 132...
145 
 
 132 
 
146 
return (sinus*m*n);
 133 
return (sinus*m*n);
Line 147... Line -...
147 
}
 - 
 
148 
 
 - 
 
149 
 
 134 
}
Line 150... Line 135...
150 
 
 135 
 
Line 151... Line 136...
151 
 
 136 
 
152 
 
 137 
/*
Line 181... Line 166...
181 
 
 166 
 
Line 182... Line 167...
182 
winkel = pgm_read_word(&pgm_sinus[winkel]);
 167 
winkel = pgm_read_word(&pgm_sinus[winkel]);
183 
 
 168 
 
- 
 
 169 
return (winkel*m*n);