root / trunk / code / projects / fp_math / fp_math.c @ 1579
History | View | Annotate | Download (4.58 KB)
1 | 1579 | justin | #include "fp_math.h" |
---|---|---|---|
2 | |||
3 | #include <avr/pgmspace.h> |
||
4 | #include <math.h> |
||
5 | #include <serial.h> |
||
6 | |||
7 | #define ABS(x) (x > 0 ? x : -x) |
||
8 | |||
9 | #define TABLE_LENGTH 128 |
||
10 | #define TABLE_STEP 1621//.160757 |
||
11 | |||
12 | static int32_t linspace[TABLE_LENGTH] PROGMEM = {
|
||
13 | 0, 1621, 3242, 4863, 6485, 8106, 9727, 11348, |
||
14 | 12969, 14590, 16212, 17833, 19454, 21075, 22696, 24317, |
||
15 | 25939, 27560, 29181, 30802, 32423, 34044, 35666, 37287, |
||
16 | 38908, 40529, 42150, 43771, 45393, 47014, 48635, 50256, |
||
17 | 51877, 53498, 55119, 56741, 58362, 59983, 61604, 63225, |
||
18 | 64846, 66468, 68089, 69710, 71331, 72952, 74573, 76195, |
||
19 | 77816, 79437, 81058, 82679, 84300, 85922, 87543, 89164, |
||
20 | 90785, 92406, 94027, 95648, 97270, 98891, 100512, 102133, |
||
21 | 103754, 105375, 106997, 108618, 110239, 111860, 113481, 115102, |
||
22 | 116724, 118345, 119966, 121587, 123208, 124829, 126451, 128072, |
||
23 | 129693, 131314, 132935, 134556, 136178, 137799, 139420, 141041, |
||
24 | 142662, 144283, 145904, 147526, 149147, 150768, 152389, 154010, |
||
25 | 155631, 157253, 158874, 160495, 162116, 163737, 165358, 166980, |
||
26 | 168601, 170222, 171843, 173464, 175085, 176707, 178328, 179949, |
||
27 | 181570, 183191, 184812, 186433, 188055, 189676, 191297, 192918, |
||
28 | 194539, 196160, 197782, 199403, 201024, 202645, 204266, 205887 |
||
29 | }; |
||
30 | |||
31 | static int32_t cos_table[TABLE_LENGTH] PROGMEM = {
|
||
32 | 65536, 65516, 65456, 65356, 65215, 65035, 64815, 64556, |
||
33 | 64257, 63919, 63541, 63125, 62670, 62176, 61645, 61076, |
||
34 | 60470, 59826, 59146, 58430, 57678, 56890, 56068, 55212, |
||
35 | 54322, 53398, 52442, 51454, 50434, 49384, 48303, 47193, |
||
36 | 46053, 44886, 43691, 42470, 41222, 39949, 38652, 37331, |
||
37 | 35988, 34622, 33235, 31828, 30401, 28956, 27492, 26013, |
||
38 | 24517, 23006, 21481, 19943, 18393, 16831, 15260, 13679, |
||
39 | 12089, 10492, 8889, 7280, 5667, 4050, 2431, 811, |
||
40 | -811, -2431, -4050, -5667, -7280, -8889, -10492, -12089, |
||
41 | -13679, -15260, -16831, -18393, -19943, -21481, -23006, -24517, |
||
42 | -26013, -27492, -28956, -30401, -31828, -33235, -34622, -35988, |
||
43 | -37331, -38652, -39949, -41222, -42470, -43691, -44886, -46053, |
||
44 | -47193, -48303, -49384, -50434, -51454, -52442, -53398, -54322, |
||
45 | -55212, -56068, -56890, -57678, -58430, -59146, -59826, -60470, |
||
46 | -61076, -61645, -62176, -62670, -63125, -63541, -63919, -64257, |
||
47 | -64556, -64815, -65035, -65215, -65356, -65456, -65516, -65536 |
||
48 | }; |
||
49 | |||
50 | static int32_t sin_table[TABLE_LENGTH] PROGMEM = {
|
||
51 | 0, 1621, 3241, 4859, 6474, 8085, 9691, 11292, |
||
52 | 12885, 14470, 16047, 17614, 19169, 20714, 22245, 23763, |
||
53 | 25267, 26755, 28226, 29680, 31117, 32534, 33931, 35307, |
||
54 | 36662, 37995, 39304, 40589, 41849, 43084, 44292, 45473, |
||
55 | 46627, 47751, 48847, 49913, 50948, 51952, 52924, 53864, |
||
56 | 54771, 55644, 56484, 57288, 58058, 58792, 59491, 60152, |
||
57 | 60777, 61365, 61915, 62428, 62902, 63338, 63735, 64093, |
||
58 | 64411, 64691, 64930, 65130, 65291, 65411, 65491, 65531, |
||
59 | 65531, 65491, 65411, 65291, 65130, 64930, 64691, 64411, |
||
60 | 64093, 63735, 63338, 62902, 62428, 61915, 61365, 60777, |
||
61 | 60152, 59491, 58792, 58058, 57288, 56484, 55644, 54771, |
||
62 | 53864, 52924, 51952, 50948, 49913, 48847, 47751, 46627, |
||
63 | 45473, 44292, 43084, 41849, 40589, 39304, 37995, 36662, |
||
64 | 35307, 33931, 32534, 31117, 29680, 28226, 26755, 25267, |
||
65 | 23763, 22245, 20714, 19169, 17614, 16047, 14470, 12885, |
||
66 | 11292, 9691, 8085, 6474, 4859, 3241, 1621, 0 |
||
67 | }; |
||
68 | |||
69 | //Quadratic interpolation.
|
||
70 | int32_t fp_cos(int32_t theta) { |
||
71 | uint8_t n; |
||
72 | int32_t x_n, x_np1, x_np2; |
||
73 | int32_t y_n, y_np1, y_np2; |
||
74 | int32_t dd_n, dd_np1, second_dd; |
||
75 | |||
76 | //Move theta into [0, pi]
|
||
77 | theta = ABS(theta) % FP_TWO_PI; |
||
78 | if(theta > FP_PI)
|
||
79 | theta = FP_TWO_PI - theta; |
||
80 | |||
81 | //Find the nearest table values. FIXME
|
||
82 | n = theta / TABLE_STEP; |
||
83 | while((x_np1 = pgm_read_dword(&linspace[n+1])) < theta) n++; |
||
84 | usb_puti(n); |
||
85 | |||
86 | //theta is between x_{n} and x_{n+1}
|
||
87 | |||
88 | //Address edge cases.
|
||
89 | if(n == 0) { |
||
90 | //TODO
|
||
91 | } |
||
92 | |||
93 | if(n == TABLE_LENGTH - 1) { |
||
94 | //TODO
|
||
95 | } |
||
96 | |||
97 | //Address the general case. Quadratic interpolation.
|
||
98 | //Load in the necessary values.
|
||
99 | x_n = pgm_read_dword(&linspace[n]); |
||
100 | x_np1 = pgm_read_dword(&linspace[n + 1]);
|
||
101 | x_np2 = pgm_read_dword(&linspace[n + 2]);
|
||
102 | |||
103 | y_n = pgm_read_dword(&cos_table[n]); |
||
104 | y_np1 = pgm_read_dword(&cos_table[n + 1]);
|
||
105 | y_np2 = pgm_read_dword(&cos_table[n + 2]);
|
||
106 | |||
107 | //Calculate first divided differences.
|
||
108 | dd_n = fp_div(y_np1 - y_n, x_np1 - x_n); |
||
109 | dd_np1 = fp_div(y_np2 - y_np1, x_np2 - x_np1); |
||
110 | |||
111 | //Calculate the second divided difference.
|
||
112 | second_dd = fp_div(dd_np1 - dd_n, x_np2 - x_n); |
||
113 | |||
114 | return fp_mult(fp_mult(second_dd, theta - x_n), theta - x_np1)
|
||
115 | + fp_mult(dd_n, theta - x_n) + y_n; |
||
116 | } |
||
117 | |||
118 | //FIXME I didn't write these functions very carefully...
|
||
119 | int32_t fp_mult(int32_t a, int32_t b) { |
||
120 | return (int32_t) (((int64_t)a) * ((int64_t)b)) >> 16; |
||
121 | } |
||
122 | |||
123 | int32_t fp_div(int32_t a, int32_t b) { |
||
124 | return (int32_t) ((int64_t)a << 16) / (int64_t)b; |
||
125 | } |
||
126 | |||
127 |