Revision 1579
Initial pass at a 32-bit fixed point library.
- Generated cosine / sine tables, put them in PROGMEM
- Wrote initial quadratic interpolation cosine function.
... not safe to use yet, but it seems to work for non-edge
cases. Need to write a better way to test it.
trunk/code/projects/fp_math/fp_math.c | ||
---|---|---|
1 |
#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 |
|
|
128 |
|
trunk/code/projects/fp_math/gentables.m | ||
---|---|---|
1 |
|
|
2 |
space = int32(round(linspace(0,pi,128) * 2^16)) |
|
3 |
costable = int32(round(cos(linspace(0,pi,128)) * 2^16)); |
|
4 |
sintable = int32(round(sin(linspace(0,pi,128)) * 2^16)); |
|
5 |
|
|
6 |
space_file = fopen('space_table.txt', 'w'); |
|
7 |
cos_file = fopen('cos_table.txt', 'w'); |
|
8 |
sin_file = fopen('sin_table.txt', 'w'); |
|
9 |
|
|
10 |
for i=1:length(space) |
|
11 |
fprintf(space_file, '%d, ', space(i)); |
|
12 |
if ~mod(i,8) |
|
13 |
fprintf(space_file, '\n'); |
|
14 |
end |
|
15 |
end |
|
16 |
|
|
17 |
for i=1:length(costable) |
|
18 |
fprintf(cos_file, '%d, ', costable(i)); |
|
19 |
if ~mod(i,8) |
|
20 |
fprintf(cos_file, '\n'); |
|
21 |
end |
|
22 |
end |
|
23 |
|
|
24 |
for i=1:length(sintable) |
|
25 |
fprintf(sin_file, '%d, ', sintable(i)); |
|
26 |
if ~mod(i,8) |
|
27 |
fprintf(sin_file, '\n'); |
|
28 |
end |
|
29 |
end |
|
30 |
|
trunk/code/projects/fp_math/fp_math.h | ||
---|---|---|
1 |
|
|
2 |
#ifndef _FP_MATH_H_ |
|
3 |
#define _FP_MATH_H_ |
|
4 |
|
|
5 |
#define FP_PI 205887 |
|
6 |
#define FP_TWO_PI 411775 |
|
7 |
|
|
8 |
#include <stdint.h> |
|
9 |
|
|
10 |
int32_t fp_cos(int32_t theta); |
|
11 |
int32_t fp_sin(int32_t theta); |
|
12 |
|
|
13 |
int32_t fp_mult(int32_t a, int32_t b); |
|
14 |
int32_t fp_div(int32_t a, int32_t b); |
|
15 |
|
|
16 |
#endif |
|
17 |
|
trunk/code/projects/fp_math/main.c | ||
---|---|---|
1 |
#include <serial.h> |
|
2 |
#include <stdint.h> |
|
3 |
#include "fp_math.h" |
|
4 |
|
|
5 |
int main(int argc, char** argv) { |
|
6 |
int32_t i,j; |
|
7 |
char buf[128]; |
|
8 |
|
|
9 |
usb_init(); |
|
10 |
usb_puts("USB Initialized\n\r"); |
|
11 |
|
|
12 |
for(i=10000; i < 11000; i+=5) { |
|
13 |
sprintf(buf, "fp_cos(%ld) = %ld\n\r", i, fp_cos(i)); |
|
14 |
usb_puts(buf); |
|
15 |
} |
|
16 |
} |
|
17 |
|
trunk/code/projects/fp_math/Makefile | ||
---|---|---|
1 |
# this is a local makefile |
|
2 |
|
|
3 |
# Relative path to the root directory (containing lib directory) |
|
4 |
ifndef COLONYROOT |
|
5 |
COLONYROOT := .. |
|
6 |
|
|
7 |
# Target file name (without extension). |
|
8 |
TARGET=main |
|
9 |
|
|
10 |
# Uncomment this to use the wireless library |
|
11 |
USE_WIRELESS = 1 |
|
12 |
|
|
13 |
# com1 = serial port. Use lpt1 to connect to parallel port. |
|
14 |
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi) |
|
15 |
AVRDUDE_PORT = /dev/tty.usbserial-A4001gUV |
|
16 |
|
|
17 |
|
|
18 |
else |
|
19 |
COLONYROOT := ../$(COLONYROOT) |
|
20 |
endif |
|
21 |
|
|
22 |
include $(COLONYROOT)/Makefile |
Also available in: Unified diff