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 