Subversion Repositories FlightCtrl

Rev

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

Rev 838 Rev 966
Line 7... Line 7...
7 
// Sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
 7 
// Sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
8 
const uint16_t pgm_sinlookup[91] PROGMEM = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192};
 8 
const uint16_t pgm_sinlookup[91] PROGMEM = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192};
Line 9... Line 9...
9 
 
 9 
 
10 
int16_t c_sin_8192(int16_t angle)
 10 
int16_t c_sin_8192(int16_t angle)
11 
{
 11 
{
12 
        int8_t m,n;
 12 
  int8_t m,n;
13 
        int16_t sinus;
 13 
  int16_t sinus;
14 
 
 14 
 
15 
        // avoid negative angles
 15 
  // avoid negative angles
16 
        if (angle < 0)
 16 
  if (angle < 0)
17 
        {
 17 
  {
18 
                m = -1;
 18 
    m = -1;
19 
                angle = abs(angle);
 19 
    angle = abs(angle);
20 
        }
 20 
  }
21 
        else m = +1;
 21 
  else m = +1;
22 
 
 22 
 
23 
        // fold angle to intervall 0 to 359
 23 
  // fold angle to intervall 0 to 359
24 
        angle %= 360;
 24 
  angle %= 360;
25 
 
 25 
 
26 
        // check quadrant
 26 
  // check quadrant
27 
        if (angle <= 90) n=1; // first quadrant
 27 
  if (angle <= 90) n=1; // first quadrant
28 
        else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant
 28 
  else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant
29 
        else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant
 29 
  else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant
30 
        else {angle = 360 - angle; n = -1;}     //fourth quadrant
 30 
  else {angle = 360 - angle; n = -1;}   //fourth quadrant
31 
        // get lookup value
 31 
  // get lookup value
32 
        sinus = pgm_read_word(&pgm_sinlookup[angle]);
 32 
  sinus = pgm_read_word(&pgm_sinlookup[angle]);
33 
        // calculate sinus value
 33 
  // calculate sinus value
34 
        return (sinus * m * n);
 34 
  return (sinus * m * n);
Line 35... Line 35...
35 
}
 35 
}
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 
{
Line 40... Line 40...
40 
        return (c_sin_8192(90 - angle));
 40 
  return (c_sin_8192(90 - angle));
41 
}
 41 
}
Line 42... Line 42...
42 
 
 42 
 
43 
// Arcustangens returns degree in a range of +/. 180 deg
 43 
// Arcustangens returns degree in a range of +/. 180 deg
44 
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};
 44 
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};
45 
 
 45 
 
46 
int16_t c_atan2(int16_t y, int16_t x)
 46 
int16_t c_atan2(int16_t y, int16_t x)
47 
{
 47 
{
48 
        int16_t index, angle;
 48 
  int16_t index, angle;
49 
        int8_t m;
 49 
  int8_t m;
50 
 
 50 
 
51 
        if (!x && !y) return 0;         //atan2(0, 0) is undefined
 51 
  if (!x && !y) return 0;               //atan2(0, 0) is undefined
52 
 
 52 
 
53 
        if (y < 0) m = -1;
 53 
  if (y < 0) m = -1;
54 
        else m = 1;
 54 
  else m = 1;
55 
 
 55 
 
56 
        if (!x) return (90 * m);                // atan2(y,0) = +/- 90 deg
 56 
  if (!x) return (90 * m);              // atan2(y,0) = +/- 90 deg
57 
 
 57 
 
58 
        index = (int16_t)(((int32_t)y * 64) / x);// calculate index for lookup table
 58 
  index = (int16_t)(((int32_t)y * 64) / x);// calculate index for lookup table
59 
        if (index < 0) index = -index;
 59 
  if (index < 0) index = -index;
60 
 
 60 
 
61 
        if (index < 346) angle = pgm_read_byte(&pgm_atanlookup[index]); // lookup for 0 deg to 79 deg
 61 
  if (index < 346) angle = pgm_read_byte(&pgm_atanlookup[index]);       // lookup for 0 deg to 79 deg
62 
        else if (index > 7334) angle = 90;                                              // limit is 90 deg
 62 
  else if (index > 7334) angle = 90;                                            // limit is 90 deg
63 
        else if (index > 2444) angle = 89;                                              // 89 deg to 80 deg is mapped via intervalls
 63 
  else if (index > 2444) angle = 89;                                            // 89 deg to 80 deg is mapped via intervalls
64 
        else if (index > 1465) angle = 88;
 64 
  else if (index > 1465) angle = 88;
65 
        else if (index > 1046) angle = 87;
 65 
  else if (index > 1046) angle = 87;
66 
        else if (index > 813) angle = 86;
 66 
  else if (index > 813) angle = 86;
67 
        else if (index > 664) angle = 85;
 67 
  else if (index > 664) angle = 85;
68 
        else if (index > 561) angle = 84;
 68 
  else if (index > 561) angle = 84;
69 
        else if (index > 486) angle = 83;
 69 
  else if (index > 486) angle = 83;
70 
        else if (index > 428) angle = 82;
 70 
  else if (index > 428) angle = 82;
71 
        else if (index > 382) angle = 81;
 71 
  else if (index > 382) angle = 81;
72 
        else angle = 80; // (index>345)
 72 
  else angle = 80; // (index>345)
73 
 
 73 
 
Line 74... Line 74...
74 
        if (x > 0) return (angle * m);  // 1st and 4th quadrant
 74 
  if (x > 0) return (angle * m);        // 1st and 4th quadrant
75 
        else if ((x < 0) && (m > 0)) return (180 - angle);      // 2nd quadrant
 75 
  else if ((x < 0) && (m > 0)) return (180 - angle);    // 2nd quadrant
Line 76... Line 76...
76 
        else return (angle - 180); // ( (x < 0) && (y < 0))     3rd quadrant
 76 
  else return (angle - 180); // ( (x < 0) && (y < 0))   3rd quadrant
77 
}
 77 
}
78 
 
 78 
 
79 
// Arcustangens returns degree in a range of +/. 180 deg
 79 
// Arcustangens returns degree in a range of +/. 180 deg
80 
const uint8_t pgm_asinlookup[128] PROGMEM = {0,0,1,1,2,2,3,3,4,4,4,5,5,6,6,7,7,8,8,9,9,9,10,10,11,11,12,12,13,13,14,14,14,15,15,16,16,17,17,18,18,19,19,20,20,21,21,22,22,23,23,23,24,24,25,25,26,26,27,27,28,28,29,29,30,31,31,32,32,33,33,34,34,35,35,36,36,37,38,38,39,39,40,40,41,42,42,43,43,44,45,45,46,47,47,48,49,49,50,51,51,52,53,54,54,55,56,57,58,58,59,60,61,62,63,64,65,66,67,68,70,71,72,74,76,78,80,83};
 80 
const uint8_t pgm_asinlookup[128] PROGMEM = {0,0,1,1,2,2,3,3,4,4,4,5,5,6,6,7,7,8,8,9,9,9,10,10,11,11,12,12,13,13,14,14,14,15,15,16,16,17,17,18,18,19,19,20,20,21,21,22,22,23,23,23,24,24,25,25,26,26,27,27,28,28,29,29,30,31,31,32,32,33,33,34,34,35,35,36,36,37,38,38,39,39,40,40,41,42,42,43,43,44,45,45,46,47,47,48,49,49,50,51,51,52,53,54,54,55,56,57,58,58,59,60,61,62,63,64,65,66,67,68,70,71,72,74,76,78,80,83};
81 
 
 81 
 
82 
int c_asin_8192(int y)
 82 
int c_asin_8192(int y)
83 
{
 83 
{
84 
        int index, m;
 84 
  int index, m;
85 
        if (y < 0)
 85 
  if (y < 0)
86 
        {      
 86 
  {    
87 
                m = -1;
 87 
    m = -1;
88 
                index = -y / 64;
 88 
    index = -y / 64;
89 
        }
 89 
  }
90 
        else
 90 
  else
91 
        {
 91 
  {
92 
                m = 1;
 92 
    m = 1;
93 
                index = y / 64;
 93 
    index = y / 64;
94 
        }      
 94 
  }    
95 
 
 95 
 
96 
        if (index > 127)
 96 
  if (index > 127)
97 
        {      
 97 
  {    
98 
                index = 127;
 98 
    index = 127;
99 
        }
 99 
  }
100 
        return(m * pgm_read_byte(&pgm_asinlookup[index]));
 100 
  return(m * pgm_read_byte(&pgm_asinlookup[index]));
101 
}
 101 
}
102 
// integer square root
 102 
// integer square root
103 
uint32_t c_sqrt(uint32_t number)
 103 
uint32_t c_sqrt(uint32_t number)
104 
{
 104 
{
105 
        uint32_t s1, s2;
 105 
  uint32_t s1, s2;
106 
        uint8_t iter = 0;
 106 
  uint8_t iter = 0;
107 
        // initialization of iteration
 107 
  // initialization of iteration
108 
        s2 = number;
 108 
  s2 = number;
109 
        do // iterative formula to solve x^2 - n = 0
 109 
  do // iterative formula to solve x^2 - n = 0
110 
        {
 110 
  {
111 
                s1 = s2;
 111 
    s1 = s2;
112 
                s2 = number / s1;
 112 
    s2 = number / s1;
113 
                s2 += s1;
 113 
    s2 += s1;