root / trunk / code / projects / fp_math / fp_math.c @ 1603
History  View  Annotate  Download (4.18 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 1635//.160757 
13 
#define EXP_MAGICONSTANT 726817 //ln(2^16) * 2^16 
14  
15 
//#define DEBUG

16  
17 
//TODO This can probably be removed at some point.

18 
static int32_t linspace[TABLE_LENGTH] PROGMEM = {

19 
0, 1634, 3268, 4902, 6536, 8170, 9804, 11438, 
20 
13072, 14706, 16340, 17974, 19608, 21242, 22876, 24510, 
21 
26144, 27778, 29412, 31047, 32681, 34315, 35949, 37583, 
22 
39217, 40851, 42485, 44119, 45753, 47387, 49021, 50655, 
23 
52289, 53923, 55557, 57191, 58825, 60459, 62093, 63727, 
24 
65361, 66995, 68629, 70263, 71897, 73531, 75165, 76799, 
25 
78433, 80067, 81701, 83335, 84969, 86603, 88237, 89871, 
26 
91506, 93140, 94774, 96408, 98042, 99676, 101310, 102944, 
27 
}; 
28  
29 
static int32_t fp_cos_table[TABLE_LENGTH] PROGMEM = {

30 
65536, 65516, 65455, 65353, 65210, 65027, 64804, 64540, 
31 
64237, 63893, 63509, 63087, 62624, 62123, 61584, 61006, 
32 
60390, 59736, 59046, 58319, 57555, 56756, 55921, 55052, 
33 
54148, 53211, 52241, 51238, 50203, 49138, 48041, 46915, 
34 
45760, 44576, 43364, 42126, 40861, 39571, 38256, 36918, 
35 
35556, 34173, 32768, 31343, 29898, 28435, 26954, 25456, 
36 
23943, 22415, 20872, 19317, 17750, 16171, 14583, 12986, 
37 
11380, 9768, 8149, 6525, 4898, 3267, 1634, 0, 
38 
}; 
39  
40 
//FIXME Lazy implementations of tangent and sine.

41 
int32_t fp_tan(int32_t theta) { 
42 
return fp_div(fp_sin(theta), fp_cos(theta));

43 
} 
44  
45 
int32_t fp_sin(int32_t theta) { 
46 
return fp_cos(theta + FP_PI_OVER_TWO);

47 
} 
48  
49 
int32_t fp_exp(int32_t x) { 
50 
//Try with partial fractions for now.

51 
int32_t xsquare = fp_mult(x,x); 
52 
int32_t result, i; 
53 

54 
if(x > EXP_MAGICONSTANT) {

55 
usb_puts("Output value is out of range.\n\r");

56 
return 1; 
57 
} 
58 

59 
//This is again, massive amounts of lazyness.

60 
//The approximation fails outside this range (approximately).

61 
if(x < (17l << 16)) 
62 
return 0; 
63 

64 
result = xsquare; 
65 
for(i= (22l << 16); i > (2l << 16); i= (4l << 16)) { 
66 
result += i; 
67 
result = fp_div(xsquare, result); 
68 
} 
69 

70 
result += (2l << 16)  x; 
71 
return (1l << 16) + fp_div(fp_mult((2l << 16), x), result); 
72  
73 
} 
74  
75 
//Quadratic interpolation.

76 
int32_t fp_cos(int32_t theta) { 
77 
uint8_t n; 
78 
int8_t negative; 
79 
int32_t x_n, x_np1, x_np2; 
80 
int32_t y_n, y_np1, y_np2; 
81 
int32_t dd_n, dd_np1, second_dd, result; 
82  
83 
#ifdef DEBUG

84 
char buf[128]; 
85 
#endif

86  
87 
//Move theta into [0, pi/2] w/ appropriate sign.

88 
theta = ABS(theta) % FP_TWO_PI; 
89 

90 
//Reflecting into [0, pi] doesn't change sign.

91 
if(theta > FP_PI)

92 
theta = FP_TWO_PI  theta; 
93 

94 
//Reflecting into [0, pi/2] does.

95 
if(theta > FP_PI_OVER_TWO) {

96 
theta = FP_PI  theta; 
97 
negative = 1;

98 
} 
99 

100 
//Find n such that theta is between x_{n} and x_{n+1}

101 
n = theta / TABLE_STEP; 
102 
while( n < TABLE_LENGTH  1 
103 
&& pgm_read_dword(&linspace[n+1]) < theta)

104 
n++; 
105 

106 
//Edge case

107 
if(n == TABLE_LENGTH  1) { 
108 
//Perform linear interpolation, since we're close to zero anyway.

109 
x_n = pgm_read_dword(&linspace[TABLE_LENGTH  1]);

110 
y_n = pgm_read_dword(&fp_cos_table[TABLE_LENGTH  1]);

111  
112 
result = fp_mult( 
113 
fp_div(FP_PI_OVER_TWO  x_n, 0  y_n), theta  x_n) + y_n;

114 

115 
return negative ? result : result;

116 
} 
117  
118 
//Perform quadratic interpolation.

119 

120 
//Load in the necessary values.

121 
x_n = pgm_read_dword(&linspace[n]); 
122 
x_np1 = pgm_read_dword(&linspace[n + 1]);

123 
x_np2 = pgm_read_dword(&linspace[n + 2]);

124 

125 
y_n = pgm_read_dword(&fp_cos_table[n]); 
126 
y_np1 = pgm_read_dword(&fp_cos_table[n + 1]);

127 
y_np2 = pgm_read_dword(&fp_cos_table[n + 2]);

128  
129 
#ifdef DEBUG

130 
sprintf(buf, "x_n = %ld, theta = %ld, x_{n+1} = %ld", x_n, theta, x_np1);

131 
usb_puts(buf); 
132 
#endif

133  
134 
//Calculate first divided differences.

135 
dd_n = fp_div(y_np1  y_n, x_np1  x_n); 
136 
dd_np1 = fp_div(y_np2  y_np1, x_np2  x_np1); 
137  
138 
//Calculate the second divided difference.

139 
second_dd = fp_div(dd_np1  dd_n, x_np2  x_n); 
140 

141 
result = fp_mult(fp_mult(second_dd, theta  x_n), theta  x_np1) 
142 
+ fp_mult(dd_n, theta  x_n) + y_n; 
143  
144 
return negative ? result : result;

145 
} 
146  
147 
//FIXME I didn't write these functions very carefully...

148 
int32_t fp_mult(int32_t a, int32_t b) { 
149 
return (int32_t) (((int64_t)a) * ((int64_t)b)) >> 16; 
150 
} 
151  
152 
int32_t fp_div(int32_t a, int32_t b) { 
153 
return (int32_t) ((int64_t)a << 16) / (int64_t)b; 
154 
} 
155  
156  
157 