Subversion Repositories FlightCtrl

Rev

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

Rev 1994 Rev 1995
Line 1... Line 1...
1
#include <stdlib.h>
1
#include <stdlib.h>
2
#include <avr/pgmspace.h>
2
#include <avr/pgmspace.h>
3
#include "mymath.h"
3
#include "mymath.h"
4
 
4
 
5
// discrete mathematics
5
// discrete mathematics
6
 
6
 
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};
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);
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
{
40
        return (c_sin_8192(90 - angle));
40
        return (c_sin_8192(90 - angle));
41
}
41
}