-
Notifications
You must be signed in to change notification settings - Fork 6
/
Copy pathtrig.c
113 lines (105 loc) · 4 KB
/
trig.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
/*
* Fixed point sine & cosine.
*
* Licensed under AGPLv3.
*/
#include <avr/pgmspace.h>
#include "ahrs.h"
/* 7th century formula that gives only about 0.0015 maximum error */
int16_t sin_16_bhaskara(int16_t angle) {
uint32_t w;
if (!(angle & 0x3fff))
return angle << 1;
if (angle >= 0) {
w = (uint32_t) angle * (0x8000 - angle);
return (w << 4) / ((5L << 15) - (w >> 13));
}
angle = (uint16_t) angle & 0x7fff;
w = (uint32_t) angle * (0x8000 - angle);
return -(int16_t) ((w << 4) / ((5L << 15) - (w >> 13)));
}
/* 15-bit sine LUT indexed by angle. */
const uint16_t sin_lut[130] PROGMEM = {
0, 402, 804, 1206, 1608, 2009, 2411, 2811, 3212, 3612, 4011, 4410,
4808, 5205, 5602, 5998, 6393, 6787, 7180, 7571, 7962, 8351, 8740,
9127, 9512, 9896, 10279, 10660, 11039, 11417, 11793, 12167, 12540,
12910, 13279, 13646, 14010, 14373, 14733, 15091, 15447, 15800, 16151,
16500, 16846, 17190, 17531, 17869, 18205, 18538, 18868, 19195, 19520,
19841, 20160, 20475, 20788, 21097, 21403, 21706, 22006, 22302, 22595,
22884, 23170, 23453, 23732, 24008, 24279, 24548, 24812, 25073, 25330,
25583, 25833, 26078, 26320, 26557, 26791, 27020, 27246, 27467, 27684,
27897, 28106, 28311, 28511, 28707, 28899, 29086, 29269, 29448, 29622,
29792, 29957, 30118, 30274, 30425, 30572, 30715, 30853, 30986, 31114,
31238, 31357, 31471, 31581, 31686, 31786, 31881, 31972, 32058, 32138,
32214, 32286, 32352, 32413, 32470, 32522, 32568, 32610, 32647, 32679,
32706, 32729, 32746, 32758, 32766, 32767, 32767,
};
/* About 0.012 maximum error, fast (about 40 AVR cycles) */
int16_t sin_16(int16_t x) {
if (x >= 0) {
if (x > 0x4000)
x = 0x8000 - x;
return pgm_read_word(sin_lut + (x >> 7));
}
x = (uint16_t) x & 0x7fff;
if (x > 0x4000)
x = 0x8000 - x;
return -pgm_read_word(sin_lut + (x >> 7));
}
/* About 0.0001 maximum error, relatively fast (about 80 AVR cycles) */
int16_t sin_16_l(int16_t x) {
int16_t b;
if (x >= 0) {
if (x > 0x4000)
x = 0x8000 - x;
b = pgm_read_word(sin_lut + (x >> 7));
return b +
(((uint16_t) (pgm_read_word(sin_lut + (x >> 7) + 1) -
b) * (x & 127) + 13) >> 7);
}
x = (uint16_t) x & 0x7fff;
if (x > 0x4000)
x = 0x8000 - x;
b = pgm_read_word(sin_lut + (x >> 7));
return -b -
(((uint16_t) (pgm_read_word(sin_lut + (x >> 7) + 1) -
b) * (x & 127) + 13) >> 7);
}
/* 15-bit sine LUT indexed by angle. */
const uint16_t sin_lut32[169] PROGMEM = {
0, 307, 614, 921, 1228, 1535, 1841, 2148, 2454, 2760, 3066, 3371,
3677, 3982, 4286, 4590, 4894, 5198, 5501, 5803, 6105, 6406, 6707,
7007, 7307, 7606, 7904, 8202, 8499, 8795, 9090, 9385, 9679, 9972,
10264, 10555, 10845, 11134, 11423, 11710, 11996, 12281, 12565, 12848,
13130, 13411, 13691, 13969, 14246, 14522, 14797, 15070, 15342, 15612,
15882, 16150, 16416, 16681, 16945, 17207, 17467, 17726, 17984, 18240,
18494, 18746, 18997, 19247, 19494, 19740, 19985, 20227, 20468, 20707,
20944, 21179, 21412, 21644, 21873, 22101, 22327, 22550, 22772, 22992,
23210, 23425, 23639, 23851, 24060, 24268, 24473, 24676, 24877, 25076,
25272, 25467, 25659, 25848, 26036, 26221, 26404, 26585, 26763, 26939,
27113, 27284, 27453, 27619, 27783, 27945, 28104, 28261, 28415, 28567,
28716, 28862, 29006, 29148, 29287, 29423, 29557, 29689, 29817, 29943,
30067, 30187, 30305, 30421, 30534, 30644, 30751, 30856, 30958, 31057,
31154, 31248, 31339, 31427, 31512, 31595, 31675, 31753, 31827, 31899,
31967, 32034, 32097, 32157, 32215, 32270, 32321, 32371, 32417, 32460,
32501, 32539, 32573, 32605, 32634, 32661, 32684, 32705, 32722, 32737,
32749, 32758, 32764, 32767, 32767,
};
int16_t sin_32_l(int32_t x) {
int16_t b;
if (x >= 0) {
if (x > (ROLL_PITCH_180DEG >> 1))
x = ROLL_PITCH_180DEG - x;
b = pgm_read_word(sin_lut32 + (x >> 22));
return b +
(((pgm_read_word(sin_lut32 + (x >> 22) + 1) - b) *
(x & 0x3fffff) + 0x1fffff) >> 22);
}
x += ROLL_PITCH_180DEG;
if (x > (ROLL_PITCH_180DEG >> 1))
x = ROLL_PITCH_180DEG - x;
b = pgm_read_word(sin_lut32 + (x >> 22));
return -b -
(((pgm_read_word(sin_lut32 + (x >> 22) + 1) - b) *
(x & 0x3fffff) + 0x1fffff) >> 22);
}