root / trunk / code / projects / fp_math / fp_math.c @ 1586
History | View | Annotate | Download (4.02 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 | 1585 | justin | #define FP_PI_OVER_TWO 102944 |
9 | #define FP_TWO_PI 411775 |
||
10 | 1579 | justin | |
11 | 1585 | justin | #define TABLE_LENGTH 64 |
12 | 1579 | justin | #define TABLE_STEP 1621//.160757 |
13 | 1586 | justin | #define EXP_MAGICONSTANT
|
14 | 1579 | justin | |
15 | static int32_t linspace[TABLE_LENGTH] PROGMEM = {
|
||
16 | 0, 1621, 3242, 4863, 6485, 8106, 9727, 11348, |
||
17 | 12969, 14590, 16212, 17833, 19454, 21075, 22696, 24317, |
||
18 | 25939, 27560, 29181, 30802, 32423, 34044, 35666, 37287, |
||
19 | 38908, 40529, 42150, 43771, 45393, 47014, 48635, 50256, |
||
20 | 51877, 53498, 55119, 56741, 58362, 59983, 61604, 63225, |
||
21 | 64846, 66468, 68089, 69710, 71331, 72952, 74573, 76195, |
||
22 | 77816, 79437, 81058, 82679, 84300, 85922, 87543, 89164, |
||
23 | 1585 | justin | 90785, 92406, 94027, 95648, 97270, 98891, 100512, 102133 |
24 | 1579 | justin | }; |
25 | |||
26 | 1585 | justin | static int32_t fp_cos_table[TABLE_LENGTH] PROGMEM = {
|
27 | 1579 | justin | 65536, 65516, 65456, 65356, 65215, 65035, 64815, 64556, |
28 | 64257, 63919, 63541, 63125, 62670, 62176, 61645, 61076, |
||
29 | 60470, 59826, 59146, 58430, 57678, 56890, 56068, 55212, |
||
30 | 54322, 53398, 52442, 51454, 50434, 49384, 48303, 47193, |
||
31 | 46053, 44886, 43691, 42470, 41222, 39949, 38652, 37331, |
||
32 | 35988, 34622, 33235, 31828, 30401, 28956, 27492, 26013, |
||
33 | 24517, 23006, 21481, 19943, 18393, 16831, 15260, 13679, |
||
34 | 1585 | justin | 12089, 10492, 8889, 7280, 5667, 4050, 2431, 811 |
35 | 1579 | justin | }; |
36 | |||
37 | 1585 | justin | //FIXME Lazy implementations of tangent and sine.
|
38 | int32_t fp_tan(int32_t theta) { |
||
39 | return fp_div(fp_sin(theta), fp_cos(theta));
|
||
40 | } |
||
41 | 1579 | justin | |
42 | 1585 | justin | int32_t fp_sin(int32_t theta) { |
43 | return fp_cos(theta + FP_PI_OVER_TWO);
|
||
44 | } |
||
45 | |||
46 | 1586 | justin | int32_t fp_exp(int32_t x) { |
47 | //Try with partial fractions for now.
|
||
48 | int32_t xsquare = fp_mult(x,x); |
||
49 | int32_t result, i; |
||
50 | |||
51 | if(theta > EXP_MAGICONSTANT) {
|
||
52 | usb_puts("Output value is out of range.\n\r");
|
||
53 | return -1; |
||
54 | } |
||
55 | |||
56 | //This is again, massive amounts of lazyness.
|
||
57 | //The approximation fails outside this range (approximately).
|
||
58 | if(theta < -17) |
||
59 | return 0; |
||
60 | |||
61 | result = xsquare; |
||
62 | for(i= (22 << 16); i > (2 << 16); i-= (4 << 16)) { |
||
63 | result += i; |
||
64 | result = fp_div(xsquare, result); |
||
65 | } |
||
66 | |||
67 | result += (2 << 16) - x; |
||
68 | return (1 << 16) + fp_div(fp_mult((2 << 16), x), result); |
||
69 | |||
70 | } |
||
71 | |||
72 | 1579 | justin | //Quadratic interpolation.
|
73 | int32_t fp_cos(int32_t theta) { |
||
74 | uint8_t n; |
||
75 | 1585 | justin | int8_t negative; |
76 | 1579 | justin | int32_t x_n, x_np1, x_np2; |
77 | int32_t y_n, y_np1, y_np2; |
||
78 | 1585 | justin | int32_t dd_n, dd_np1, second_dd, result; |
79 | 1579 | justin | |
80 | 1585 | justin | //Move theta into [0, pi/2] w/ appropriate sign.
|
81 | theta = ABS(theta) % FP_TWO_PI; |
||
82 | |||
83 | 1579 | justin | if(theta > FP_PI)
|
84 | theta = FP_TWO_PI - theta; |
||
85 | 1585 | justin | |
86 | if(theta > FP_PI_OVER_TWO) {
|
||
87 | theta = FP_PI - theta; |
||
88 | negative = 1;
|
||
89 | } |
||
90 | 1579 | justin | |
91 | //Find the nearest table values. FIXME
|
||
92 | n = theta / TABLE_STEP; |
||
93 | 1585 | justin | while( n < TABLE_LENGTH - 1 && (x_np1 = pgm_read_dword(&linspace[n+1]))) |
94 | n++; |
||
95 | 1579 | justin | |
96 | //theta is between x_{n} and x_{n+1}
|
||
97 | if(n == TABLE_LENGTH - 1) { |
||
98 | 1585 | justin | //Perform linear interpolation, since we're close to zero anyway.
|
99 | x_n = pgm_read_dword(&linspace[TABLE_LENGTH - 1]);
|
||
100 | y_n = pgm_read_dword(&fp_cos_table[TABLE_LENGTH - 1]);
|
||
101 | |||
102 | result = fp_mult(fp_div(FP_PI_OVER_TWO - x_n, 0 - y_n), theta - x_n) + y_n;
|
||
103 | return negative ? -result : result;
|
||
104 | 1579 | justin | } |
105 | 1585 | justin | |
106 | if(n == TABLE_LENGTH) {
|
||
107 | //We didn't find a value! Oh no!
|
||
108 | usb_puts("fp_math: We couldn't find surrounding table values! \n\r
|
||
109 | This should never happen!!!\n\r\n\r"); |
||
110 | return 0; |
||
111 | } |
||
112 | 1579 | justin | |
113 | //Address the general case. Quadratic interpolation.
|
||
114 | //Load in the necessary values.
|
||
115 | x_n = pgm_read_dword(&linspace[n]); |
||
116 | x_np2 = pgm_read_dword(&linspace[n + 2]);
|
||
117 | |||
118 | 1585 | justin | y_n = pgm_read_dword(&fp_cos_table[n]); |
119 | y_np1 = pgm_read_dword(&fp_cos_table[n + 1]);
|
||
120 | y_np2 = pgm_read_dword(&fp_cos_table[n + 2]);
|
||
121 | 1579 | justin | |
122 | //Calculate first divided differences.
|
||
123 | dd_n = fp_div(y_np1 - y_n, x_np1 - x_n); |
||
124 | dd_np1 = fp_div(y_np2 - y_np1, x_np2 - x_np1); |
||
125 | |||
126 | //Calculate the second divided difference.
|
||
127 | second_dd = fp_div(dd_np1 - dd_n, x_np2 - x_n); |
||
128 | |||
129 | 1585 | justin | result = fp_mult(fp_mult(second_dd, theta - x_n), theta - x_np1) |
130 | + fp_mult(dd_n, theta - x_n) + y_n; |
||
131 | |||
132 | return negative ? -result : result;
|
||
133 | 1579 | justin | } |
134 | |||
135 | //FIXME I didn't write these functions very carefully...
|
||
136 | int32_t fp_mult(int32_t a, int32_t b) { |
||
137 | return (int32_t) (((int64_t)a) * ((int64_t)b)) >> 16; |
||
138 | } |
||
139 | |||
140 | int32_t fp_div(int32_t a, int32_t b) { |
||
141 | return (int32_t) ((int64_t)a << 16) / (int64_t)b; |
||
142 | } |
||
143 | |||
144 |