root / trunk / code / projects / fp_math / fp_math.c @ 1586
History | View | Annotate | Download (4.02 KB)
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 |
#define FP_PI_OVER_TWO 102944 |
9 |
#define FP_TWO_PI 411775 |
10 |
|
11 |
#define TABLE_LENGTH 64 |
12 |
#define TABLE_STEP 1621//.160757 |
13 |
#define EXP_MAGICONSTANT
|
14 |
|
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 |
90785, 92406, 94027, 95648, 97270, 98891, 100512, 102133 |
24 |
}; |
25 |
|
26 |
static int32_t fp_cos_table[TABLE_LENGTH] PROGMEM = {
|
27 |
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 |
12089, 10492, 8889, 7280, 5667, 4050, 2431, 811 |
35 |
}; |
36 |
|
37 |
//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 |
|
42 |
int32_t fp_sin(int32_t theta) { |
43 |
return fp_cos(theta + FP_PI_OVER_TWO);
|
44 |
} |
45 |
|
46 |
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 |
//Quadratic interpolation.
|
73 |
int32_t fp_cos(int32_t theta) { |
74 |
uint8_t n; |
75 |
int8_t negative; |
76 |
int32_t x_n, x_np1, x_np2; |
77 |
int32_t y_n, y_np1, y_np2; |
78 |
int32_t dd_n, dd_np1, second_dd, result; |
79 |
|
80 |
//Move theta into [0, pi/2] w/ appropriate sign.
|
81 |
theta = ABS(theta) % FP_TWO_PI; |
82 |
|
83 |
if(theta > FP_PI)
|
84 |
theta = FP_TWO_PI - theta; |
85 |
|
86 |
if(theta > FP_PI_OVER_TWO) {
|
87 |
theta = FP_PI - theta; |
88 |
negative = 1;
|
89 |
} |
90 |
|
91 |
//Find the nearest table values. FIXME
|
92 |
n = theta / TABLE_STEP; |
93 |
while( n < TABLE_LENGTH - 1 && (x_np1 = pgm_read_dword(&linspace[n+1]))) |
94 |
n++; |
95 |
|
96 |
//theta is between x_{n} and x_{n+1}
|
97 |
if(n == TABLE_LENGTH - 1) { |
98 |
//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 |
} |
105 |
|
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 |
|
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 |
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 |
|
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 |
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 |
} |
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 |
|
145 |
|