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