Subversion Repositories FlightCtrl

Rev

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

Rev 1612 Rev 1821
Line 1... Line 1...
1
#include "dongfangMath.h"
1
#include "dongfangMath.h"
2
#include <inttypes.h>
2
#include <inttypes.h>
3
#include <avr/pgmspace.h>
3
#include <avr/pgmspace.h>
Line 4... Line -...
4
 
-
 
5
const int16_t SIN_TABLE[] PROGMEM = {
4
 
6
  (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5),
5
const int16_t SIN_TABLE[] PROGMEM = { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5),
7
  (int16_t) (0.01745240643728351 * MATH_UNIT_FACTOR + 0.5),
6
                (int16_t) (0.01745240643728351 * MATH_UNIT_FACTOR + 0.5),
8
  (int16_t) (0.03489949670250097 * MATH_UNIT_FACTOR + 0.5),
7
                (int16_t) (0.03489949670250097 * MATH_UNIT_FACTOR + 0.5),
9
  (int16_t) (0.05233595624294383 * MATH_UNIT_FACTOR + 0.5),
8
                (int16_t) (0.05233595624294383 * MATH_UNIT_FACTOR + 0.5),
10
  (int16_t) (0.0697564737441253 * MATH_UNIT_FACTOR + 0.5),
9
                (int16_t) (0.0697564737441253 * MATH_UNIT_FACTOR + 0.5),
11
  (int16_t) (0.08715574274765817 * MATH_UNIT_FACTOR + 0.5),
10
                (int16_t) (0.08715574274765817 * MATH_UNIT_FACTOR + 0.5),
12
  (int16_t) (0.10452846326765346 * MATH_UNIT_FACTOR + 0.5),
11
                (int16_t) (0.10452846326765346 * MATH_UNIT_FACTOR + 0.5),
13
  (int16_t) (0.12186934340514748 * MATH_UNIT_FACTOR + 0.5),
12
                (int16_t) (0.12186934340514748 * MATH_UNIT_FACTOR + 0.5),
14
  (int16_t) (0.13917310096006544 * MATH_UNIT_FACTOR + 0.5),
13
                (int16_t) (0.13917310096006544 * MATH_UNIT_FACTOR + 0.5),
15
  (int16_t) (0.15643446504023087 * MATH_UNIT_FACTOR + 0.5),
14
                (int16_t) (0.15643446504023087 * MATH_UNIT_FACTOR + 0.5),
16
  (int16_t) (0.17364817766693033 * MATH_UNIT_FACTOR + 0.5),
15
                (int16_t) (0.17364817766693033 * MATH_UNIT_FACTOR + 0.5),
17
  (int16_t) (0.1908089953765448 * MATH_UNIT_FACTOR + 0.5),
16
                (int16_t) (0.1908089953765448 * MATH_UNIT_FACTOR + 0.5),
18
  (int16_t) (0.20791169081775931 * MATH_UNIT_FACTOR + 0.5),
17
                (int16_t) (0.20791169081775931 * MATH_UNIT_FACTOR + 0.5),
19
  (int16_t) (0.224951054343865 * MATH_UNIT_FACTOR + 0.5),
18
                (int16_t) (0.224951054343865 * MATH_UNIT_FACTOR + 0.5),
20
  (int16_t) (0.24192189559966773 * MATH_UNIT_FACTOR + 0.5),
19
                (int16_t) (0.24192189559966773 * MATH_UNIT_FACTOR + 0.5),
21
  (int16_t) (0.25881904510252074 * MATH_UNIT_FACTOR + 0.5),
20
                (int16_t) (0.25881904510252074 * MATH_UNIT_FACTOR + 0.5),
22
  (int16_t) (0.27563735581699916 * MATH_UNIT_FACTOR + 0.5),
21
                (int16_t) (0.27563735581699916 * MATH_UNIT_FACTOR + 0.5),
23
  (int16_t) (0.29237170472273677 * MATH_UNIT_FACTOR + 0.5),
22
                (int16_t) (0.29237170472273677 * MATH_UNIT_FACTOR + 0.5),
24
  (int16_t) (0.3090169943749474 * MATH_UNIT_FACTOR + 0.5),
23
                (int16_t) (0.3090169943749474 * MATH_UNIT_FACTOR + 0.5),
25
  (int16_t) (0.32556815445715664 * MATH_UNIT_FACTOR + 0.5),
24
                (int16_t) (0.32556815445715664 * MATH_UNIT_FACTOR + 0.5),
26
  (int16_t) (0.3420201433256687 * MATH_UNIT_FACTOR + 0.5),
25
                (int16_t) (0.3420201433256687 * MATH_UNIT_FACTOR + 0.5),
27
  (int16_t) (0.35836794954530027 * MATH_UNIT_FACTOR + 0.5),
26
                (int16_t) (0.35836794954530027 * MATH_UNIT_FACTOR + 0.5),
28
  (int16_t) (0.374606593415912 * MATH_UNIT_FACTOR + 0.5),
27
                (int16_t) (0.374606593415912 * MATH_UNIT_FACTOR + 0.5),
29
  (int16_t) (0.3907311284892737 * MATH_UNIT_FACTOR + 0.5),
28
                (int16_t) (0.3907311284892737 * MATH_UNIT_FACTOR + 0.5),
30
  (int16_t) (0.40673664307580015 * MATH_UNIT_FACTOR + 0.5),
29
                (int16_t) (0.40673664307580015 * MATH_UNIT_FACTOR + 0.5),
31
  (int16_t) (0.42261826174069944 * MATH_UNIT_FACTOR + 0.5),
30
                (int16_t) (0.42261826174069944 * MATH_UNIT_FACTOR + 0.5),
32
  (int16_t) (0.4383711467890774 * MATH_UNIT_FACTOR + 0.5),
31
                (int16_t) (0.4383711467890774 * MATH_UNIT_FACTOR + 0.5),
33
  (int16_t) (0.45399049973954675 * MATH_UNIT_FACTOR + 0.5),
32
                (int16_t) (0.45399049973954675 * MATH_UNIT_FACTOR + 0.5),
34
  (int16_t) (0.4694715627858908 * MATH_UNIT_FACTOR + 0.5),
33
                (int16_t) (0.4694715627858908 * MATH_UNIT_FACTOR + 0.5),
35
  (int16_t) (0.48480962024633706 * MATH_UNIT_FACTOR + 0.5),
34
                (int16_t) (0.48480962024633706 * MATH_UNIT_FACTOR + 0.5),
36
  (int16_t) (0.49999999999999994 * MATH_UNIT_FACTOR + 0.5),
35
                (int16_t) (0.49999999999999994 * MATH_UNIT_FACTOR + 0.5),
37
  (int16_t) (0.5150380749100542 * MATH_UNIT_FACTOR + 0.5),
36
                (int16_t) (0.5150380749100542 * MATH_UNIT_FACTOR + 0.5),
38
  (int16_t) (0.5299192642332049 * MATH_UNIT_FACTOR + 0.5),
37
                (int16_t) (0.5299192642332049 * MATH_UNIT_FACTOR + 0.5),
39
  (int16_t) (0.5446390350150271 * MATH_UNIT_FACTOR + 0.5),
38
                (int16_t) (0.5446390350150271 * MATH_UNIT_FACTOR + 0.5),
40
  (int16_t) (0.5591929034707469 * MATH_UNIT_FACTOR + 0.5),
39
                (int16_t) (0.5591929034707469 * MATH_UNIT_FACTOR + 0.5),
41
  (int16_t) (0.573576436351046 * MATH_UNIT_FACTOR + 0.5),
40
                (int16_t) (0.573576436351046 * MATH_UNIT_FACTOR + 0.5),
42
  (int16_t) (0.5877852522924731 * MATH_UNIT_FACTOR + 0.5),
41
                (int16_t) (0.5877852522924731 * MATH_UNIT_FACTOR + 0.5),
43
  (int16_t) (0.6018150231520483 * MATH_UNIT_FACTOR + 0.5),
42
                (int16_t) (0.6018150231520483 * MATH_UNIT_FACTOR + 0.5),
44
  (int16_t) (0.6156614753256582 * MATH_UNIT_FACTOR + 0.5),
43
                (int16_t) (0.6156614753256582 * MATH_UNIT_FACTOR + 0.5),
45
  (int16_t) (0.6293203910498374 * MATH_UNIT_FACTOR + 0.5),
44
                (int16_t) (0.6293203910498374 * MATH_UNIT_FACTOR + 0.5),
46
  (int16_t) (0.6427876096865393 * MATH_UNIT_FACTOR + 0.5),
45
                (int16_t) (0.6427876096865393 * MATH_UNIT_FACTOR + 0.5),
47
  (int16_t) (0.6560590289905072 * MATH_UNIT_FACTOR + 0.5),
46
                (int16_t) (0.6560590289905072 * MATH_UNIT_FACTOR + 0.5),
48
  (int16_t) (0.6691306063588582 * MATH_UNIT_FACTOR + 0.5),
47
                (int16_t) (0.6691306063588582 * MATH_UNIT_FACTOR + 0.5),
49
  (int16_t) (0.6819983600624985 * MATH_UNIT_FACTOR + 0.5),
48
                (int16_t) (0.6819983600624985 * MATH_UNIT_FACTOR + 0.5),
50
  (int16_t) (0.6946583704589973 * MATH_UNIT_FACTOR + 0.5),
49
                (int16_t) (0.6946583704589973 * MATH_UNIT_FACTOR + 0.5),
51
  (int16_t) (0.7071067811865475 * MATH_UNIT_FACTOR + 0.5),
50
                (int16_t) (0.7071067811865475 * MATH_UNIT_FACTOR + 0.5),
52
  (int16_t) (0.7193398003386511 * MATH_UNIT_FACTOR + 0.5),
51
                (int16_t) (0.7193398003386511 * MATH_UNIT_FACTOR + 0.5),
53
  (int16_t) (0.7313537016191705 * MATH_UNIT_FACTOR + 0.5),
52
                (int16_t) (0.7313537016191705 * MATH_UNIT_FACTOR + 0.5),
54
  (int16_t) (0.7431448254773942 * MATH_UNIT_FACTOR + 0.5),
53
                (int16_t) (0.7431448254773942 * MATH_UNIT_FACTOR + 0.5),
55
  (int16_t) (0.754709580222772 * MATH_UNIT_FACTOR + 0.5),
54
                (int16_t) (0.754709580222772 * MATH_UNIT_FACTOR + 0.5),
56
  (int16_t) (0.766044443118978 * MATH_UNIT_FACTOR + 0.5),
55
                (int16_t) (0.766044443118978 * MATH_UNIT_FACTOR + 0.5),
57
  (int16_t) (0.7771459614569708 * MATH_UNIT_FACTOR + 0.5),
56
                (int16_t) (0.7771459614569708 * MATH_UNIT_FACTOR + 0.5),
58
  (int16_t) (0.788010753606722 * MATH_UNIT_FACTOR + 0.5),
57
                (int16_t) (0.788010753606722 * MATH_UNIT_FACTOR + 0.5),
59
  (int16_t) (0.7986355100472928 * MATH_UNIT_FACTOR + 0.5),
58
                (int16_t) (0.7986355100472928 * MATH_UNIT_FACTOR + 0.5),
60
  (int16_t) (0.8090169943749475 * MATH_UNIT_FACTOR + 0.5),
59
                (int16_t) (0.8090169943749475 * MATH_UNIT_FACTOR + 0.5),
61
  (int16_t) (0.8191520442889918 * MATH_UNIT_FACTOR + 0.5),
60
                (int16_t) (0.8191520442889918 * MATH_UNIT_FACTOR + 0.5),
62
  (int16_t) (0.8290375725550417 * MATH_UNIT_FACTOR + 0.5),
61
                (int16_t) (0.8290375725550417 * MATH_UNIT_FACTOR + 0.5),
63
  (int16_t) (0.8386705679454239 * MATH_UNIT_FACTOR + 0.5),
62
                (int16_t) (0.8386705679454239 * MATH_UNIT_FACTOR + 0.5),
64
  (int16_t) (0.8480480961564261 * MATH_UNIT_FACTOR + 0.5),
63
                (int16_t) (0.8480480961564261 * MATH_UNIT_FACTOR + 0.5),
65
  (int16_t) (0.8571673007021122 * MATH_UNIT_FACTOR + 0.5),
64
                (int16_t) (0.8571673007021122 * MATH_UNIT_FACTOR + 0.5),
66
  (int16_t) (0.8660254037844386 * MATH_UNIT_FACTOR + 0.5),
65
                (int16_t) (0.8660254037844386 * MATH_UNIT_FACTOR + 0.5),
67
  (int16_t) (0.8746197071393957 * MATH_UNIT_FACTOR + 0.5),
66
                (int16_t) (0.8746197071393957 * MATH_UNIT_FACTOR + 0.5),
68
  (int16_t) (0.8829475928589269 * MATH_UNIT_FACTOR + 0.5),
67
                (int16_t) (0.8829475928589269 * MATH_UNIT_FACTOR + 0.5),
69
  (int16_t) (0.8910065241883678 * MATH_UNIT_FACTOR + 0.5),
68
                (int16_t) (0.8910065241883678 * MATH_UNIT_FACTOR + 0.5),
70
  (int16_t) (0.898794046299167 * MATH_UNIT_FACTOR + 0.5),
69
                (int16_t) (0.898794046299167 * MATH_UNIT_FACTOR + 0.5),
71
  (int16_t) (0.9063077870366499 * MATH_UNIT_FACTOR + 0.5),
70
                (int16_t) (0.9063077870366499 * MATH_UNIT_FACTOR + 0.5),
72
  (int16_t) (0.9135454576426009 * MATH_UNIT_FACTOR + 0.5),
71
                (int16_t) (0.9135454576426009 * MATH_UNIT_FACTOR + 0.5),
73
  (int16_t) (0.9205048534524403 * MATH_UNIT_FACTOR + 0.5),
72
                (int16_t) (0.9205048534524403 * MATH_UNIT_FACTOR + 0.5),
74
  (int16_t) (0.9271838545667874 * MATH_UNIT_FACTOR + 0.5),
73
                (int16_t) (0.9271838545667874 * MATH_UNIT_FACTOR + 0.5),
75
  (int16_t) (0.9335804264972017 * MATH_UNIT_FACTOR + 0.5),
74
                (int16_t) (0.9335804264972017 * MATH_UNIT_FACTOR + 0.5),
76
  (int16_t) (0.9396926207859083 * MATH_UNIT_FACTOR + 0.5),
75
                (int16_t) (0.9396926207859083 * MATH_UNIT_FACTOR + 0.5),
77
  (int16_t) (0.9455185755993167 * MATH_UNIT_FACTOR + 0.5),
76
                (int16_t) (0.9455185755993167 * MATH_UNIT_FACTOR + 0.5),
78
  (int16_t) (0.9510565162951535 * MATH_UNIT_FACTOR + 0.5),
77
                (int16_t) (0.9510565162951535 * MATH_UNIT_FACTOR + 0.5),
79
  (int16_t) (0.9563047559630354 * MATH_UNIT_FACTOR + 0.5),
78
                (int16_t) (0.9563047559630354 * MATH_UNIT_FACTOR + 0.5),
80
  (int16_t) (0.9612616959383189 * MATH_UNIT_FACTOR + 0.5),
79
                (int16_t) (0.9612616959383189 * MATH_UNIT_FACTOR + 0.5),
81
  (int16_t) (0.9659258262890683 * MATH_UNIT_FACTOR + 0.5),
80
                (int16_t) (0.9659258262890683 * MATH_UNIT_FACTOR + 0.5),
82
  (int16_t) (0.9702957262759965 * MATH_UNIT_FACTOR + 0.5),
81
                (int16_t) (0.9702957262759965 * MATH_UNIT_FACTOR + 0.5),
83
  (int16_t) (0.9743700647852352 * MATH_UNIT_FACTOR + 0.5),
82
                (int16_t) (0.9743700647852352 * MATH_UNIT_FACTOR + 0.5),
84
  (int16_t) (0.9781476007338056 * MATH_UNIT_FACTOR + 0.5),
83
                (int16_t) (0.9781476007338056 * MATH_UNIT_FACTOR + 0.5),
85
  (int16_t) (0.981627183447664 * MATH_UNIT_FACTOR + 0.5),
84
                (int16_t) (0.981627183447664 * MATH_UNIT_FACTOR + 0.5),
86
  (int16_t) (0.984807753012208 * MATH_UNIT_FACTOR + 0.5),
85
                (int16_t) (0.984807753012208 * MATH_UNIT_FACTOR + 0.5),
87
  (int16_t) (0.9876883405951378 * MATH_UNIT_FACTOR + 0.5),
86
                (int16_t) (0.9876883405951378 * MATH_UNIT_FACTOR + 0.5),
88
  (int16_t) (0.9902680687415703 * MATH_UNIT_FACTOR + 0.5),
87
                (int16_t) (0.9902680687415703 * MATH_UNIT_FACTOR + 0.5),
89
  (int16_t) (0.992546151641322 * MATH_UNIT_FACTOR + 0.5),
88
                (int16_t) (0.992546151641322 * MATH_UNIT_FACTOR + 0.5),
90
  (int16_t) (0.9945218953682733 * MATH_UNIT_FACTOR + 0.5),
89
                (int16_t) (0.9945218953682733 * MATH_UNIT_FACTOR + 0.5),
91
  (int16_t) (0.9961946980917455 * MATH_UNIT_FACTOR + 0.5),
90
                (int16_t) (0.9961946980917455 * MATH_UNIT_FACTOR + 0.5),
92
  (int16_t) (0.9975640502598242 * MATH_UNIT_FACTOR + 0.5),
91
                (int16_t) (0.9975640502598242 * MATH_UNIT_FACTOR + 0.5),
93
  (int16_t) (0.9986295347545738 * MATH_UNIT_FACTOR + 0.5),
92
                (int16_t) (0.9986295347545738 * MATH_UNIT_FACTOR + 0.5),
94
  (int16_t) (0.9993908270190958 * MATH_UNIT_FACTOR + 0.5),
93
                (int16_t) (0.9993908270190958 * MATH_UNIT_FACTOR + 0.5),
95
  (int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5),
94
                (int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5), (int16_t) (1.0
Line 96... Line 95...
96
  (int16_t) (1.0 * MATH_UNIT_FACTOR) };
95
                                * MATH_UNIT_FACTOR) };
97
 
96
 
98
const int16_t TAN_TABLE[] PROGMEM = {
97
const int16_t TAN_TABLE[] PROGMEM
99
  (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5),
98
                = { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5),
100
  (int16_t) (0.017455064928217585 * MATH_UNIT_FACTOR + 0.5),
99
                                (int16_t) (0.017455064928217585 * MATH_UNIT_FACTOR + 0.5),
101
  (int16_t) (0.03492076949174773 * MATH_UNIT_FACTOR + 0.5),
100
                                (int16_t) (0.03492076949174773 * MATH_UNIT_FACTOR + 0.5),
102
  (int16_t) (0.0524077792830412 * MATH_UNIT_FACTOR + 0.5),
101
                                (int16_t) (0.0524077792830412 * MATH_UNIT_FACTOR + 0.5),
103
  (int16_t) (0.06992681194351041 * MATH_UNIT_FACTOR + 0.5),
102
                                (int16_t) (0.06992681194351041 * MATH_UNIT_FACTOR + 0.5),
104
  (int16_t) (0.08748866352592401 * MATH_UNIT_FACTOR + 0.5),
103
                                (int16_t) (0.08748866352592401 * MATH_UNIT_FACTOR + 0.5),
105
  (int16_t) (0.10510423526567646 * MATH_UNIT_FACTOR + 0.5),
104
                                (int16_t) (0.10510423526567646 * MATH_UNIT_FACTOR + 0.5),
106
  (int16_t) (0.1227845609029046 * MATH_UNIT_FACTOR + 0.5),
105
                                (int16_t) (0.1227845609029046 * MATH_UNIT_FACTOR + 0.5),
107
  (int16_t) (0.14054083470239145 * MATH_UNIT_FACTOR + 0.5),
106
                                (int16_t) (0.14054083470239145 * MATH_UNIT_FACTOR + 0.5),
108
  (int16_t) (0.15838444032453627 * MATH_UNIT_FACTOR + 0.5),
107
                                (int16_t) (0.15838444032453627 * MATH_UNIT_FACTOR + 0.5),
109
  (int16_t) (0.17632698070846498 * MATH_UNIT_FACTOR + 0.5),
108
                                (int16_t) (0.17632698070846498 * MATH_UNIT_FACTOR + 0.5),
110
  (int16_t) (0.19438030913771848 * MATH_UNIT_FACTOR + 0.5),
109
                                (int16_t) (0.19438030913771848 * MATH_UNIT_FACTOR + 0.5),
111
  (int16_t) (0.2125565616700221 * MATH_UNIT_FACTOR + 0.5),
110
                                (int16_t) (0.2125565616700221 * MATH_UNIT_FACTOR + 0.5),
112
  (int16_t) (0.23086819112556312 * MATH_UNIT_FACTOR + 0.5),
111
                                (int16_t) (0.23086819112556312 * MATH_UNIT_FACTOR + 0.5),
113
  (int16_t) (0.24932800284318068 * MATH_UNIT_FACTOR + 0.5),
112
                                (int16_t) (0.24932800284318068 * MATH_UNIT_FACTOR + 0.5),
114
  (int16_t) (0.2679491924311227 * MATH_UNIT_FACTOR + 0.5),
113
                                (int16_t) (0.2679491924311227 * MATH_UNIT_FACTOR + 0.5),
115
  (int16_t) (0.2867453857588079 * MATH_UNIT_FACTOR + 0.5),
114
                                (int16_t) (0.2867453857588079 * MATH_UNIT_FACTOR + 0.5),
116
  (int16_t) (0.3057306814586604 * MATH_UNIT_FACTOR + 0.5),
115
                                (int16_t) (0.3057306814586604 * MATH_UNIT_FACTOR + 0.5),
117
  (int16_t) (0.3249196962329063 * MATH_UNIT_FACTOR + 0.5),
116
                                (int16_t) (0.3249196962329063 * MATH_UNIT_FACTOR + 0.5),
118
  (int16_t) (0.3443276132896652 * MATH_UNIT_FACTOR + 0.5),
117
                                (int16_t) (0.3443276132896652 * MATH_UNIT_FACTOR + 0.5),
119
  (int16_t) (0.36397023426620234 * MATH_UNIT_FACTOR + 0.5),
118
                                (int16_t) (0.36397023426620234 * MATH_UNIT_FACTOR + 0.5),
120
  (int16_t) (0.3838640350354158 * MATH_UNIT_FACTOR + 0.5),
119
                                (int16_t) (0.3838640350354158 * MATH_UNIT_FACTOR + 0.5),
121
  (int16_t) (0.4040262258351568 * MATH_UNIT_FACTOR + 0.5),
120
                                (int16_t) (0.4040262258351568 * MATH_UNIT_FACTOR + 0.5),
122
  (int16_t) (0.4244748162096047 * MATH_UNIT_FACTOR + 0.5),
121
                                (int16_t) (0.4244748162096047 * MATH_UNIT_FACTOR + 0.5),
123
  (int16_t) (0.4452286853085361 * MATH_UNIT_FACTOR + 0.5),
122
                                (int16_t) (0.4452286853085361 * MATH_UNIT_FACTOR + 0.5),
124
  (int16_t) (0.4663076581549986 * MATH_UNIT_FACTOR + 0.5),
123
                                (int16_t) (0.4663076581549986 * MATH_UNIT_FACTOR + 0.5),
125
  (int16_t) (0.48773258856586144 * MATH_UNIT_FACTOR + 0.5),
124
                                (int16_t) (0.48773258856586144 * MATH_UNIT_FACTOR + 0.5),
126
  (int16_t) (0.5095254494944288 * MATH_UNIT_FACTOR + 0.5),
125
                                (int16_t) (0.5095254494944288 * MATH_UNIT_FACTOR + 0.5),
127
  (int16_t) (0.5317094316614788 * MATH_UNIT_FACTOR + 0.5),
126
                                (int16_t) (0.5317094316614788 * MATH_UNIT_FACTOR + 0.5),
128
  (int16_t) (0.554309051452769 * MATH_UNIT_FACTOR + 0.5),
127
                                (int16_t) (0.554309051452769 * MATH_UNIT_FACTOR + 0.5),
129
  (int16_t) (0.5773502691896257 * MATH_UNIT_FACTOR + 0.5),
128
                                (int16_t) (0.5773502691896257 * MATH_UNIT_FACTOR + 0.5),
130
  (int16_t) (0.6008606190275604 * MATH_UNIT_FACTOR + 0.5),
129
                                (int16_t) (0.6008606190275604 * MATH_UNIT_FACTOR + 0.5),
131
  (int16_t) (0.6248693519093275 * MATH_UNIT_FACTOR + 0.5),
130
                                (int16_t) (0.6248693519093275 * MATH_UNIT_FACTOR + 0.5),
132
  (int16_t) (0.6494075931975106 * MATH_UNIT_FACTOR + 0.5),
131
                                (int16_t) (0.6494075931975106 * MATH_UNIT_FACTOR + 0.5),
133
  (int16_t) (0.6745085168424267 * MATH_UNIT_FACTOR + 0.5),
132
                                (int16_t) (0.6745085168424267 * MATH_UNIT_FACTOR + 0.5),
134
  (int16_t) (0.7002075382097097 * MATH_UNIT_FACTOR + 0.5),
133
                                (int16_t) (0.7002075382097097 * MATH_UNIT_FACTOR + 0.5),
135
  (int16_t) (0.7265425280053608 * MATH_UNIT_FACTOR + 0.5),
134
                                (int16_t) (0.7265425280053608 * MATH_UNIT_FACTOR + 0.5),
136
  (int16_t) (0.7535540501027942 * MATH_UNIT_FACTOR + 0.5),
135
                                (int16_t) (0.7535540501027942 * MATH_UNIT_FACTOR + 0.5),
137
  (int16_t) (0.7812856265067173 * MATH_UNIT_FACTOR + 0.5),
136
                                (int16_t) (0.7812856265067173 * MATH_UNIT_FACTOR + 0.5),
138
  (int16_t) (0.809784033195007 * MATH_UNIT_FACTOR + 0.5),
137
                                (int16_t) (0.809784033195007 * MATH_UNIT_FACTOR + 0.5),
139
  (int16_t) (0.8390996311772799 * MATH_UNIT_FACTOR + 0.5),
138
                                (int16_t) (0.8390996311772799 * MATH_UNIT_FACTOR + 0.5),
140
  (int16_t) (0.8692867378162265 * MATH_UNIT_FACTOR + 0.5),
139
                                (int16_t) (0.8692867378162265 * MATH_UNIT_FACTOR + 0.5),
141
  (int16_t) (0.9004040442978399 * MATH_UNIT_FACTOR + 0.5),
140
                                (int16_t) (0.9004040442978399 * MATH_UNIT_FACTOR + 0.5),
142
  (int16_t) (0.9325150861376615 * MATH_UNIT_FACTOR + 0.5),
141
                                (int16_t) (0.9325150861376615 * MATH_UNIT_FACTOR + 0.5),
143
  (int16_t) (0.9656887748070739 * MATH_UNIT_FACTOR + 0.5),
142
                                (int16_t) (0.9656887748070739 * MATH_UNIT_FACTOR + 0.5),
144
  (int16_t) (0.9999999999999999 * MATH_UNIT_FACTOR + 0.5),
143
                                (int16_t) (0.9999999999999999 * MATH_UNIT_FACTOR + 0.5),
145
  (int16_t) (1.0355303137905694 * MATH_UNIT_FACTOR + 0.5),
144
                                (int16_t) (1.0355303137905694 * MATH_UNIT_FACTOR + 0.5),
146
  (int16_t) (1.0723687100246826 * MATH_UNIT_FACTOR + 0.5),
145
                                (int16_t) (1.0723687100246826 * MATH_UNIT_FACTOR + 0.5),
147
  (int16_t) (1.1106125148291928 * MATH_UNIT_FACTOR + 0.5),
146
                                (int16_t) (1.1106125148291928 * MATH_UNIT_FACTOR + 0.5),
148
  (int16_t) (1.1503684072210094 * MATH_UNIT_FACTOR + 0.5),
147
                                (int16_t) (1.1503684072210094 * MATH_UNIT_FACTOR + 0.5),
149
  (int16_t) (1.19175359259421 * MATH_UNIT_FACTOR + 0.5),
148
                                (int16_t) (1.19175359259421 * MATH_UNIT_FACTOR + 0.5),
150
  (int16_t) (1.234897156535051 * MATH_UNIT_FACTOR + 0.5),
149
                                (int16_t) (1.234897156535051 * MATH_UNIT_FACTOR + 0.5),
151
  (int16_t) (1.2799416321930788 * MATH_UNIT_FACTOR + 0.5),
150
                                (int16_t) (1.2799416321930788 * MATH_UNIT_FACTOR + 0.5),
152
  (int16_t) (1.3270448216204098 * MATH_UNIT_FACTOR + 0.5),
151
                                (int16_t) (1.3270448216204098 * MATH_UNIT_FACTOR + 0.5),
153
  (int16_t) (1.3763819204711734 * MATH_UNIT_FACTOR + 0.5),
152
                                (int16_t) (1.3763819204711734 * MATH_UNIT_FACTOR + 0.5),
154
  (int16_t) (1.4281480067421144 * MATH_UNIT_FACTOR + 0.5),
153
                                (int16_t) (1.4281480067421144 * MATH_UNIT_FACTOR + 0.5),
155
  (int16_t) (1.4825609685127403 * MATH_UNIT_FACTOR + 0.5),
154
                                (int16_t) (1.4825609685127403 * MATH_UNIT_FACTOR + 0.5),
156
  (int16_t) (1.5398649638145825 * MATH_UNIT_FACTOR + 0.5),
155
                                (int16_t) (1.5398649638145825 * MATH_UNIT_FACTOR + 0.5),
157
  (int16_t) (1.6003345290410504 * MATH_UNIT_FACTOR + 0.5),
156
                                (int16_t) (1.6003345290410504 * MATH_UNIT_FACTOR + 0.5),
158
  (int16_t) (1.6642794823505174 * MATH_UNIT_FACTOR + 0.5),
157
                                (int16_t) (1.6642794823505174 * MATH_UNIT_FACTOR + 0.5),
159
  (int16_t) (1.7320508075688767 * MATH_UNIT_FACTOR + 0.5),
158
                                (int16_t) (1.7320508075688767 * MATH_UNIT_FACTOR + 0.5),
160
  (int16_t) (1.8040477552714236 * MATH_UNIT_FACTOR + 0.5),
159
                                (int16_t) (1.8040477552714236 * MATH_UNIT_FACTOR + 0.5),
161
  (int16_t) (1.8807264653463318 * MATH_UNIT_FACTOR + 0.5),
160
                                (int16_t) (1.8807264653463318 * MATH_UNIT_FACTOR + 0.5),
162
  (int16_t) (1.9626105055051504 * MATH_UNIT_FACTOR + 0.5),
161
                                (int16_t) (1.9626105055051504 * MATH_UNIT_FACTOR + 0.5),
163
  (int16_t) (2.050303841579296 * MATH_UNIT_FACTOR + 0.5),
162
                                (int16_t) (2.050303841579296 * MATH_UNIT_FACTOR + 0.5),
164
  (int16_t) (2.1445069205095586 * MATH_UNIT_FACTOR + 0.5),
163
                                (int16_t) (2.1445069205095586 * MATH_UNIT_FACTOR + 0.5),
165
  (int16_t) (2.2460367739042164 * MATH_UNIT_FACTOR + 0.5),
164
                                (int16_t) (2.2460367739042164 * MATH_UNIT_FACTOR + 0.5),
166
  (int16_t) (2.355852365823752 * MATH_UNIT_FACTOR + 0.5),
165
                                (int16_t) (2.355852365823752 * MATH_UNIT_FACTOR + 0.5),
167
  (int16_t) (2.4750868534162964 * MATH_UNIT_FACTOR + 0.5),
166
                                (int16_t) (2.4750868534162964 * MATH_UNIT_FACTOR + 0.5),
168
  (int16_t) (2.6050890646938005 * MATH_UNIT_FACTOR + 0.5),
167
                                (int16_t) (2.6050890646938005 * MATH_UNIT_FACTOR + 0.5),
169
  (int16_t) (2.7474774194546216 * MATH_UNIT_FACTOR + 0.5),
168
                                (int16_t) (2.7474774194546216 * MATH_UNIT_FACTOR + 0.5),
170
  (int16_t) (2.904210877675822 * MATH_UNIT_FACTOR + 0.5),
169
                                (int16_t) (2.904210877675822 * MATH_UNIT_FACTOR + 0.5),
171
  (int16_t) (3.0776835371752527 * MATH_UNIT_FACTOR + 0.5),
170
                                (int16_t) (3.0776835371752527 * MATH_UNIT_FACTOR + 0.5),
172
  (int16_t) (3.2708526184841404 * MATH_UNIT_FACTOR + 0.5),
171
                                (int16_t) (3.2708526184841404 * MATH_UNIT_FACTOR + 0.5),
173
  (int16_t) (3.4874144438409087 * MATH_UNIT_FACTOR + 0.5),
172
                                (int16_t) (3.4874144438409087 * MATH_UNIT_FACTOR + 0.5),
174
  (int16_t) (3.7320508075688776 * MATH_UNIT_FACTOR + 0.5),
173
                                (int16_t) (3.7320508075688776 * MATH_UNIT_FACTOR + 0.5),
175
  (int16_t) (4.010780933535842 * MATH_UNIT_FACTOR + 0.5),
174
                                (int16_t) (4.010780933535842 * MATH_UNIT_FACTOR + 0.5),
176
  (int16_t) (4.331475874284157 * MATH_UNIT_FACTOR + 0.5),
175
                                (int16_t) (4.331475874284157 * MATH_UNIT_FACTOR + 0.5),
177
  (int16_t) (4.704630109478451 * MATH_UNIT_FACTOR + 0.5),
176
                                (int16_t) (4.704630109478451 * MATH_UNIT_FACTOR + 0.5),
178
  (int16_t) (5.144554015970307 * MATH_UNIT_FACTOR + 0.5),
177
                                (int16_t) (5.144554015970307 * MATH_UNIT_FACTOR + 0.5),
179
  (int16_t) (5.671281819617707 * MATH_UNIT_FACTOR + 0.5),
178
                                (int16_t) (5.671281819617707 * MATH_UNIT_FACTOR + 0.5),
180
  (int16_t) (6.313751514675041 * MATH_UNIT_FACTOR + 0.5),
179
                                (int16_t) (6.313751514675041 * MATH_UNIT_FACTOR + 0.5),
181
  (int16_t) (7.115369722384195 * MATH_UNIT_FACTOR + 0.5),
180
                                (int16_t) (7.115369722384195 * MATH_UNIT_FACTOR + 0.5),
182
  (int16_t) (8.144346427974593 * MATH_UNIT_FACTOR + 0.5),
181
                                (int16_t) (8.144346427974593 * MATH_UNIT_FACTOR + 0.5),
183
  (int16_t) (9.514364454222587 * MATH_UNIT_FACTOR + 0.5),
182
                                (int16_t) (9.514364454222587 * MATH_UNIT_FACTOR + 0.5),
184
  (int16_t) (11.430052302761348 * MATH_UNIT_FACTOR + 0.5),
183
                                (int16_t) (11.430052302761348 * MATH_UNIT_FACTOR + 0.5),
185
  (int16_t) (14.300666256711896 * MATH_UNIT_FACTOR + 0.5),
184
                                (int16_t) (14.300666256711896 * MATH_UNIT_FACTOR + 0.5),
186
  (int16_t) (19.08113668772816 * MATH_UNIT_FACTOR + 0.5),
185
                                (int16_t) (19.08113668772816 * MATH_UNIT_FACTOR + 0.5),
187
  (int16_t) (28.636253282915515 * MATH_UNIT_FACTOR + 0.5),
186
                                (int16_t) (28.636253282915515 * MATH_UNIT_FACTOR + 0.5),
188
  (int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5),
-
 
Line 189... Line 187...
189
  (int16_t) (32767)
187
                                (int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5),
190
};
188
                                (int16_t) (32767) };
191
 
189
 
192
int16_t int_sin(int32_t arg) {
190
int16_t int_sin(int32_t arg) {
193
  int8_t sign;
191
        int8_t sign;
194
  int16_t result;
192
        int16_t result;
195
  arg /= MATH_DRG_FACTOR;
193
        arg /= MATH_DRG_FACTOR;
196
  arg %= 360;
194
        arg %= 360;
197
  if (arg < 0) {
195
        if (arg < 0) {
198
    arg = -arg;
196
                arg = -arg;
199
    sign = -1;
197
                sign = -1;
200
  } else {
198
        } else {
201
    sign = 1;
199
                sign = 1;
202
  }
200
        }
203
  if (arg >= 90) {
201
        if (arg >= 90) {
204
    arg = 180 - arg;
202
                arg = 180 - arg;
205
  }
203
        }
Line 206... Line 204...
206
  result = pgm_read_word(&SIN_TABLE[(uint8_t) arg]);
204
        result = pgm_read_word(&SIN_TABLE[(uint8_t) arg]);
207
  return (sign == 1) ? result : -result;
205
        return (sign == 1) ? result : -result;
208
}
206
}
209
 
207
 
210
int16_t int_cos(int32_t arg) {
208
int16_t int_cos(int32_t arg) {
Line 211... Line 209...
211
  if (arg > 90L * MATH_DRG_FACTOR)
209
        if (arg > 90L * MATH_DRG_FACTOR)
212
    return int_sin(arg + (90L - 360L) * MATH_DRG_FACTOR);
210
                return int_sin(arg + (90L - 360L) * MATH_DRG_FACTOR);
213
  return int_sin(arg + 90L * MATH_DRG_FACTOR);
211
        return int_sin(arg + 90L * MATH_DRG_FACTOR);
214
}
212
}
215
 
213
 
216
int16_t int_tan(int32_t arg) {
214
int16_t int_tan(int32_t arg) {
217
  int8_t sign = 1;
215
        int8_t sign = 1;
218
  int16_t result;
216
        int16_t result;
219
  arg /= MATH_DRG_FACTOR;
217
        arg /= MATH_DRG_FACTOR;
220
  if (arg >= 90) {
218
        if (arg >= 90) {
221
    arg = 180 - arg;
219
                arg = 180 - arg;
222
    sign = -1;
220
                sign = -1;
223
  } else if (arg < -90) {
221
        } else if (arg < -90) {
224
    arg += 180;
222
                arg += 180;
225
  } else if (arg < 0) {
223
        } else if (arg < 0) {
226
    arg =- arg;
224
                arg = -arg;