Subversion Repositories FlightCtrl

Rev

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

Rev 1378 Rev 1550
Line 37... Line 37...
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));
40
        return (c_sin_8192(90 - angle));
41
}
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);
-
 
103
}
-
 
104
 
-
 
105
 
-
 
106
 
-