| 5 |
5 |
#include <serial.h>
|
| 6 |
6 |
|
| 7 |
7 |
#define ABS(x) (x > 0 ? x : -x)
|
|
8 |
#define FP_PI_OVER_TWO 102944
|
|
9 |
#define FP_TWO_PI 411775
|
| 8 |
10 |
|
| 9 |
|
#define TABLE_LENGTH 128
|
|
11 |
#define TABLE_LENGTH 64
|
| 10 |
12 |
#define TABLE_STEP 1621//.160757
|
| 11 |
13 |
|
|
14 |
static inline int32_t trig_lookup(int32_t theta, int32_t* table);
|
|
15 |
|
| 12 |
16 |
static int32_t linspace[TABLE_LENGTH] PROGMEM = {
|
| 13 |
17 |
0, 1621, 3242, 4863, 6485, 8106, 9727, 11348,
|
| 14 |
18 |
12969, 14590, 16212, 17833, 19454, 21075, 22696, 24317,
|
| ... | ... | |
| 17 |
21 |
51877, 53498, 55119, 56741, 58362, 59983, 61604, 63225,
|
| 18 |
22 |
64846, 66468, 68089, 69710, 71331, 72952, 74573, 76195,
|
| 19 |
23 |
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
|
|
24 |
90785, 92406, 94027, 95648, 97270, 98891, 100512, 102133
|
| 29 |
25 |
};
|
| 30 |
26 |
|
| 31 |
|
static int32_t cos_table[TABLE_LENGTH] PROGMEM = {
|
|
27 |
static int32_t fp_cos_table[TABLE_LENGTH] PROGMEM = {
|
| 32 |
28 |
65536, 65516, 65456, 65356, 65215, 65035, 64815, 64556,
|
| 33 |
29 |
64257, 63919, 63541, 63125, 62670, 62176, 61645, 61076,
|
| 34 |
30 |
60470, 59826, 59146, 58430, 57678, 56890, 56068, 55212,
|
| ... | ... | |
| 36 |
32 |
46053, 44886, 43691, 42470, 41222, 39949, 38652, 37331,
|
| 37 |
33 |
35988, 34622, 33235, 31828, 30401, 28956, 27492, 26013,
|
| 38 |
34 |
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
|
|
35 |
12089, 10492, 8889, 7280, 5667, 4050, 2431, 811
|
| 48 |
36 |
};
|
| 49 |
37 |
|
| 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 |
|
};
|
|
38 |
//FIXME Lazy implementations of tangent and sine.
|
|
39 |
int32_t fp_tan(int32_t theta) {
|
|
40 |
return fp_div(fp_sin(theta), fp_cos(theta));
|
|
41 |
}
|
| 68 |
42 |
|
|
43 |
int32_t fp_sin(int32_t theta) {
|
|
44 |
return fp_cos(theta + FP_PI_OVER_TWO);
|
|
45 |
}
|
|
46 |
|
| 69 |
47 |
//Quadratic interpolation.
|
| 70 |
48 |
int32_t fp_cos(int32_t theta) {
|
| 71 |
49 |
uint8_t n;
|
|
50 |
int8_t negative;
|
| 72 |
51 |
int32_t x_n, x_np1, x_np2;
|
| 73 |
52 |
int32_t y_n, y_np1, y_np2;
|
| 74 |
|
int32_t dd_n, dd_np1, second_dd;
|
|
53 |
int32_t dd_n, dd_np1, second_dd, result;
|
| 75 |
54 |
|
| 76 |
|
//Move theta into [0, pi]
|
| 77 |
|
theta = ABS(theta) % FP_TWO_PI;
|
|
55 |
//Move theta into [0, pi/2] w/ appropriate sign.
|
|
56 |
theta = ABS(theta) % FP_TWO_PI;
|
|
57 |
|
| 78 |
58 |
if(theta > FP_PI)
|
| 79 |
59 |
theta = FP_TWO_PI - theta;
|
|
60 |
|
|
61 |
if(theta > FP_PI_OVER_TWO) {
|
|
62 |
theta = FP_PI - theta;
|
|
63 |
negative = 1;
|
|
64 |
}
|
| 80 |
65 |
|
| 81 |
66 |
//Find the nearest table values. FIXME
|
| 82 |
67 |
n = theta / TABLE_STEP;
|
| 83 |
|
while((x_np1 = pgm_read_dword(&linspace[n+1])) < theta) n++;
|
| 84 |
|
usb_puti(n);
|
|
68 |
while( n < TABLE_LENGTH - 1 && (x_np1 = pgm_read_dword(&linspace[n+1])))
|
|
69 |
n++;
|
| 85 |
70 |
|
| 86 |
71 |
//theta is between x_{n} and x_{n+1}
|
| 87 |
|
|
| 88 |
|
//Address edge cases.
|
| 89 |
|
if(n == 0) {
|
| 90 |
|
//TODO
|
| 91 |
|
}
|
| 92 |
72 |
|
| 93 |
73 |
if(n == TABLE_LENGTH - 1) {
|
| 94 |
|
//TODO
|
|
74 |
//Perform linear interpolation, since we're close to zero anyway.
|
|
75 |
x_n = pgm_read_dword(&linspace[TABLE_LENGTH - 1]);
|
|
76 |
y_n = pgm_read_dword(&fp_cos_table[TABLE_LENGTH - 1]);
|
|
77 |
|
|
78 |
result = fp_mult(fp_div(FP_PI_OVER_TWO - x_n, 0 - y_n), theta - x_n) + y_n;
|
|
79 |
return negative ? -result : result;
|
| 95 |
80 |
}
|
|
81 |
|
|
82 |
if(n == TABLE_LENGTH) {
|
|
83 |
//We didn't find a value! Oh no!
|
|
84 |
usb_puts("fp_math: We couldn't find surrounding table values! \n\r
|
|
85 |
This should never happen!!!\n\r\n\r");
|
|
86 |
return 0;
|
|
87 |
}
|
| 96 |
88 |
|
| 97 |
89 |
//Address the general case. Quadratic interpolation.
|
| 98 |
90 |
//Load in the necessary values.
|
| 99 |
91 |
x_n = pgm_read_dword(&linspace[n]);
|
| 100 |
|
x_np1 = pgm_read_dword(&linspace[n + 1]);
|
| 101 |
92 |
x_np2 = pgm_read_dword(&linspace[n + 2]);
|
| 102 |
93 |
|
| 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]);
|
|
94 |
y_n = pgm_read_dword(&fp_cos_table[n]);
|
|
95 |
y_np1 = pgm_read_dword(&fp_cos_table[n + 1]);
|
|
96 |
y_np2 = pgm_read_dword(&fp_cos_table[n + 2]);
|
| 106 |
97 |
|
| 107 |
98 |
//Calculate first divided differences.
|
| 108 |
99 |
dd_n = fp_div(y_np1 - y_n, x_np1 - x_n);
|
| ... | ... | |
| 111 |
102 |
//Calculate the second divided difference.
|
| 112 |
103 |
second_dd = fp_div(dd_np1 - dd_n, x_np2 - x_n);
|
| 113 |
104 |
|
| 114 |
|
return fp_mult(fp_mult(second_dd, theta - x_n), theta - x_np1)
|
| 115 |
|
+ fp_mult(dd_n, theta - x_n) + y_n;
|
|
105 |
result = fp_mult(fp_mult(second_dd, theta - x_n), theta - x_np1)
|
|
106 |
+ fp_mult(dd_n, theta - x_n) + y_n;
|
|
107 |
|
|
108 |
return negative ? -result : result;
|
| 116 |
109 |
}
|
| 117 |
110 |
|
| 118 |
111 |
//FIXME I didn't write these functions very carefully...
|