Revision 1602 trunk/code/projects/fp_math/fp_math.c
fp_math.c  

9  9 
#define FP_TWO_PI 411775 
10  10  
11  11 
#define TABLE_LENGTH 64 
12 
#define TABLE_STEP 1621//.160757


13 
#define EXP_MAGICONSTANT 

12 
#define TABLE_STEP 1635//.160757


13 
#define EXP_MAGICONSTANT 726817 //ln(2^16) * 2^16


14  14  
15 
//#define DEBUG 

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

15  18 
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


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,


24  27 
}; 
25  28  
26  29 
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


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,


35  38 
}; 
36  39  
37  40 
//FIXME Lazy implementations of tangent and sine. 
...  ...  
48  51 
int32_t xsquare = fp_mult(x,x); 
49  52 
int32_t result, i; 
50  53 

51 
if(theta > EXP_MAGICONSTANT) {


54 
if(x > EXP_MAGICONSTANT) {


52  55 
usb_puts("Output value is out of range.\n\r"); 
53  56 
return 1; 
54  57 
} 
55  58 

56  59 
//This is again, massive amounts of lazyness. 
57  60 
//The approximation fails outside this range (approximately). 
58 
if(theta < 17)


61 
if(x < (17l << 16))


59  62 
return 0; 
60  63 

61  64 
result = xsquare; 
62 
for(i= (22 << 16); i > (2 << 16); i= (4 << 16)) {


65 
for(i= (22l << 16); i > (2l << 16); i= (4l << 16)) {


63  66 
result += i; 
64  67 
result = fp_div(xsquare, result); 
65  68 
} 
66  69 

67 
result += (2 << 16)  x; 

68 
return (1 << 16) + fp_div(fp_mult((2 << 16), x), result);


70 
result += (2l << 16)  x;


71 
return (1l << 16) + fp_div(fp_mult((2l << 16), x), result);


69  72  
70  73 
} 
71  74  
...  ...  
76  79 
int32_t x_n, x_np1, x_np2; 
77  80 
int32_t y_n, y_np1, y_np2; 
78  81 
int32_t dd_n, dd_np1, second_dd, result; 
79 


82  
83 
#ifdef DEBUG 

84 
char buf[128]; 

85 
#endif 

86  
80  87 
//Move theta into [0, pi/2] w/ appropriate sign. 
81  88 
theta = ABS(theta) % FP_TWO_PI; 
82  89  
...  ...  
90  97  
91  98 
//Find the nearest table values. FIXME 
92  99 
n = theta / TABLE_STEP; 
93 
while( n < TABLE_LENGTH  1 && (x_np1 = pgm_read_dword(&linspace[n+1]))) 

100 
while( n < TABLE_LENGTH  1 

101 
&& (x_np1 = pgm_read_dword(&linspace[n+1]) < theta)) 

94  102 
n++; 
95  103 

96  104 
//theta is between x_{n} and x_{n+1} 
105  
97  106 
if(n == TABLE_LENGTH  1) { 
98  107 
//Perform linear interpolation, since we're close to zero anyway. 
99  108 
x_n = pgm_read_dword(&linspace[TABLE_LENGTH  1]); 
100  109 
y_n = pgm_read_dword(&fp_cos_table[TABLE_LENGTH  1]); 
101  110  
102 
result = fp_mult(fp_div(FP_PI_OVER_TWO  x_n, 0  y_n), theta  x_n) + y_n; 

111 
result = fp_mult( 

112 
fp_div(FP_PI_OVER_TWO  x_n, 0  y_n), theta  x_n) + y_n; 

113 


103  114 
return negative ? result : result; 
104  115 
} 
105  116  
106  117 
if(n == TABLE_LENGTH) { 
107  118 
//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"); 

119 
usb_puts("fp_math: Fatal! We couldn't find surrounding table values! \n\r");


120 


110  121 
return 0; 
111  122 
} 
112 


123  
113  124 
//Address the general case. Quadratic interpolation. 
114  125 
//Load in the necessary values. 
115  126 
x_n = pgm_read_dword(&linspace[n]); 
127 
x_np1 = pgm_read_dword(&linspace[n + 1]); 

116  128 
x_np2 = pgm_read_dword(&linspace[n + 2]); 
117  129 

118  130 
y_n = pgm_read_dword(&fp_cos_table[n]); 
119  131 
y_np1 = pgm_read_dword(&fp_cos_table[n + 1]); 
120  132 
y_np2 = pgm_read_dword(&fp_cos_table[n + 2]); 
121  133  
134 
#ifdef DEBUG 

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

136 
usb_puts(buf); 

137 
#endif 

138  
122  139 
//Calculate first divided differences. 
123  140 
dd_n = fp_div(y_np1  y_n, x_np1  x_n); 
124  141 
dd_np1 = fp_div(y_np2  y_np1, x_np2  x_np1); 
Also available in: Unified diff