Subversion Repositories FlightCtrl

Rev

Rev 2248 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
2248 - 1
/**************************************************************************************************************************************
2
* File:                 mymath.c
3
*
4
* Purpose:              mathematic functions
5
*
6
* Functions:    int16_t c_sin_8192(int16_t angle)
7
*                               int16_t c_cos_8192(int16_t angle)
8
*                               int16_t c_atan2(int16_t y, int16_t x)
9
*                               uint32_t c_sqrt(uint32_t a)
10
*
11
* hardware:             Flight Ctrl V1.3
12
*
13
* Created:              Feb 2013
14
*
15
*************************************************************************************************************************************/
16
#include "mymath.h"
17
 
18
#include <stdlib.h>
19
#include <avr/pgmspace.h>
20
 
21
 
22
 
23
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};
24
 
25
//----------------------------------------------------------------------------------------------------------------------------------
26
// Sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
27
//----------------------------------------------------------------------------------------------------------------------------------
28
int16_t c_sin_8192(int16_t angle)
29
{
30
        int8_t m,n;
31
        int16_t sinus;
32
 
33
        // avoid negative angles
34
        if (angle < 0)
35
        {
36
                m = -1;
37
                angle = abs(angle);
38
        }
39
        else m = +1;
40
 
41
        // fold angle to intervall 0 to 359
42
        angle %= 360;
43
 
44
        // check quadrant
45
        if (angle <= 90) n=1; // first quadrant
46
        else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant
47
        else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant
48
        else {angle = 360 - angle; n = -1;}     //fourth quadrant
49
        // get lookup value
50
        sinus = pgm_read_word(&pgm_sinlookup[angle]);
51
        // calculate sinus value
52
        return (sinus * m * n);
53
}
54
// ----------------------------------------------------------------------------------------------------------------------------------
55
 
56
 
57
//----------------------------------------------------------------------------------------------------------------------------------
58
// Cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit.
59
//----------------------------------------------------------------------------------------------------------------------------------
60
int16_t c_cos_8192(int16_t angle)
61
{
62
        return (c_sin_8192(90 - angle));
63
}
64
// ----------------------------------------------------------------------------------------------------------------------------------
65
 
66
 
67
 
68
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};
69
 
70
//----------------------------------------------------------------------------------------------------------------------------------
71
// arcustangens returns degree in a range of +/. 180 deg
72
//----------------------------------------------------------------------------------------------------------------------------------
73
int16_t c_atan2(int16_t y, int16_t x)
74
{
75
        int16_t index, angle;
76
        int8_t m;
77
 
78
        if (!x && !y) return 0;         //atan2(0, 0) is undefined
79
 
80
        if (y < 0) m = -1;
81
        else m = 1;
82
 
83
        if (!x) return (90 * m);                // atan2(y,0) = +/- 90 deg
84
 
85
        index = (int16_t)(((int32_t)y * 64) / x);// calculate index for lookup table
86
        if (index < 0) index = -index;
87
 
88
        if (index < 346) angle = pgm_read_byte(&pgm_atanlookup[index]); // lookup for 0 deg to 79 deg
89
        else if (index > 7334) angle = 90;                                              // limit is 90 deg
90
        else if (index > 2444) angle = 89;                                              // 89 deg to 80 deg is mapped via intervalls
91
        else if (index > 1465) angle = 88;
92
        else if (index > 1046) angle = 87;
93
        else if (index > 813) angle = 86;
94
        else if (index > 664) angle = 85;
95
        else if (index > 561) angle = 84;
96
        else if (index > 486) angle = 83;
97
        else if (index > 428) angle = 82;
98
        else if (index > 382) angle = 81;
99
        else angle = 80; // (index>345)
100
 
101
        if (x > 0) return (angle * m);  // 1st and 4th quadrant
102
        else if ((x < 0) && (m > 0)) return (180 - angle);      // 2nd quadrant
103
        else return (angle - 180); // ( (x < 0) && (y < 0))     3rd quadrant
104
}
105
// ----------------------------------------------------------------------------------------------------------------------------------
106
 
107
 
108
//----------------------------------------------------------------------------------------------------------------------------------
109
// Integer square root
110
//----------------------------------------------------------------------------------------------------------------------------------
111
uint32_t c_sqrt(uint32_t a)
112
{
113
        uint32_t rem = 0;
114
        uint32_t root = 0;
115
        uint8_t i;
116
 
117
        for(i = 0; i < 16; i++)
118
        {
119
                root <<= 1;
120
                rem = ((rem << 2) + (a >> 30));
121
                a <<= 2;
122
                root++;
123
                if(root <= rem)
124
                {
125
                        rem -= root;
126
                        root++;
127
                }
128
                else root--;
129
        }
130
        return (root >> 1);
131
}
132
// ----------------------------------------------------------------------------------------------------------------------------------
133
 
134
// *** EOF: ************************************************************************************************************************