Subversion Repositories FlightCtrl

Rev

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

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