Subversion Repositories FlightCtrl

Rev

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

Rev 1344 Rev 1490
Line 36... Line 36...
36 
 
 36 
 
37 
// Cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
 37 
// Cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
38 
int16_t c_cos_8192(int16_t angle)
 38 
int16_t c_cos_8192(int16_t angle)
39 
{
 39 
{
40 
        return (c_sin_8192(90 - angle));
 - 
 
41 
}
 - 
 
42 
 
 - 
 
43 
 
 - 
 
44 
// Arcustangens returns degree in a range of +/. 180 deg
 - 
 
45 
const uint8_t pgm_atanlookup[346] PROGMEM = {0,1,2,3,4,4,5,6,7,8,9,10,11,11,12,13,14,15,16,17,17,18,19,20,21,21,22,23,24,24,25,26,27,27,28,29,29,30,31,31,32,33,33,34,35,35,36,36,37,37,38,39,39,40,40,41,41,42,42,43,43,44,44,45,45,45,46,46,47,47,48,48,48,49,49,50,50,50,51,51,51,52,52,52,53,53,53,54,54,54,55,55,55,55,56,56,56,57,57,57,57,58,58,58,58,59,59,59,59,60,60,60,60,60,61,61,61,61,62,62,62,62,62,63,63,63,63,63,63,64,64,64,64,64,64,65,65,65,65,65,65,66,66,66,66,66,66,66,67,67,67,67,67,67,67,68,68,68,68,68,68,68,68,69,69,69,69,69,69,69,69,69,70,70,70,70,70,70,70,70,70,71,71,71,71,71,71,71,71,71,71,71,72,72,72,72,72,72,72,72,72,72,72,73,73,73,73,73,73,73,73,73,73,73,73,73,73,74,74,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,75,75,75,75,76,76,76,76,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,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,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,79,79,79,79,79,79,79};
 - 
 
46 
 
 - 
 
47 
int16_t c_atan2(int16_t y, int16_t x)
 - 
 
48 
{
 - 
 
49 
        int16_t index, angle;
 - 
 
50 
        int8_t m;
 - 
 
51 
 
 - 
 
52 
        if (!x && !y) return 0;         //atan2(0, 0) is undefined
 - 
 
53 
 
 - 
 
54 
        if (y < 0) m = -1;
 - 
 
55 
        else m = 1;
 - 
 
56 
 
 - 
 
57 
        if (!x) return (90 * m);                // atan2(y,0) = +/- 90 deg
 - 
 
58 
 
 - 
 
59 
        index = (int16_t)(((int32_t)y * 64) / x);// calculate index for lookup table
 - 
 
60 
        if (index < 0) index = -index;
 - 
 
61 
 
 - 
 
62 
        if (index < 346) angle = pgm_read_byte(&pgm_atanlookup[index]); // lookup for 0 deg to 79 deg
 - 
 
63 
        else if (index > 7334) angle = 90;                                              // limit is 90 deg
 - 
 
64 
        else if (index > 2444) angle = 89;                                              // 89 deg to 80 deg is mapped via intervalls
 - 
 
65 
        else if (index > 1465) angle = 88;
 - 
 
66 
        else if (index > 1046) angle = 87;
 - 
 
67 
        else if (index > 813) angle = 86;
 - 
 
68 
        else if (index > 664) angle = 85;
 - 
 
69 
        else if (index > 561) angle = 84;
 - 
 
70 
        else if (index > 486) angle = 83;
 - 
 
71 
        else if (index > 428) angle = 82;
 - 
 
72 
        else if (index > 382) angle = 81;
 - 
 
73 
        else angle = 80; // (index>345)
 - 
 
74 
 
 - 
 
75 
        if (x > 0) return (angle * m);  // 1st and 4th quadrant
 - 
 
76 
        else if ((x < 0) && (m > 0)) return (180 - angle);      // 2nd quadrant
 - 
 
77 
        else return (angle - 180); // ( (x < 0) && (y < 0))     3rd quadrant
 - 
 
78 
}
 - 
 
79 
 
 - 
 
80 
 
 - 
 
81 
// Integer square root
 - 
 
82 
// For details of the algorithm see the article http://www.embedded.com/98/9802fe2.htm
 - 
 
83 
uint32_t c_sqrt(uint32_t a)
 - 
 
84 
{
 - 
 
85 
        uint32_t rem = 0;
 - 
 
86 
        uint32_t root = 0;
 - 
 
87 
        uint8_t i;
 - 
 
88 
 
 - 
 
89 
        for(i = 0; i < 16; i++)
 - 
 
90 
        {
 - 
 
91 
                root <<= 1;
 - 
 
92 
                rem = ((rem << 2) + (a >> 30));
 - 
 
93 
                a <<= 2;
 - 
 
94 
                root++;
 - 
 
95 
                if(root <= rem)
 - 
 
96 
                {
 - 
 
97 
                        rem -= root;
 - 
 
98 
                        root++;
 - 
 
99 
                }
 - 
 
100 
                else root--;
 - 
 
101 
        }
 - 
 
102 
        return (root >> 1);
 40 
        return (c_sin_8192(90 - angle));
103 
}
 - 
 
104 
 
 - 
 
105 
 
 - 
 
106 
 
 41 
}