root / trunk / code / projects / fp_math / fp_math.c @ 1585
History  View  Annotate  Download (3.49 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  
14 
static inline int32_t trig_lookup(int32_t theta, int32_t* table); 
15  
16 
static int32_t linspace[TABLE_LENGTH] PROGMEM = {

17 
0, 1621, 3242, 4863, 6485, 8106, 9727, 11348, 
18 
12969, 14590, 16212, 17833, 19454, 21075, 22696, 24317, 
19 
25939, 27560, 29181, 30802, 32423, 34044, 35666, 37287, 
20 
38908, 40529, 42150, 43771, 45393, 47014, 48635, 50256, 
21 
51877, 53498, 55119, 56741, 58362, 59983, 61604, 63225, 
22 
64846, 66468, 68089, 69710, 71331, 72952, 74573, 76195, 
23 
77816, 79437, 81058, 82679, 84300, 85922, 87543, 89164, 
24 
90785, 92406, 94027, 95648, 97270, 98891, 100512, 102133 
25 
}; 
26  
27 
static int32_t fp_cos_table[TABLE_LENGTH] PROGMEM = {

28 
65536, 65516, 65456, 65356, 65215, 65035, 64815, 64556, 
29 
64257, 63919, 63541, 63125, 62670, 62176, 61645, 61076, 
30 
60470, 59826, 59146, 58430, 57678, 56890, 56068, 55212, 
31 
54322, 53398, 52442, 51454, 50434, 49384, 48303, 47193, 
32 
46053, 44886, 43691, 42470, 41222, 39949, 38652, 37331, 
33 
35988, 34622, 33235, 31828, 30401, 28956, 27492, 26013, 
34 
24517, 23006, 21481, 19943, 18393, 16831, 15260, 13679, 
35 
12089, 10492, 8889, 7280, 5667, 4050, 2431, 811 
36 
}; 
37  
38 
//FIXME Lazy implementations of tangent and sine.

39 
int32_t fp_tan(int32_t theta) { 
40 
return fp_div(fp_sin(theta), fp_cos(theta));

41 
} 
42  
43 
int32_t fp_sin(int32_t theta) { 
44 
return fp_cos(theta + FP_PI_OVER_TWO);

45 
} 
46  
47 
//Quadratic interpolation.

48 
int32_t fp_cos(int32_t theta) { 
49 
uint8_t n; 
50 
int8_t negative; 
51 
int32_t x_n, x_np1, x_np2; 
52 
int32_t y_n, y_np1, y_np2; 
53 
int32_t dd_n, dd_np1, second_dd, result; 
54 

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

56 
theta = ABS(theta) % FP_TWO_PI; 
57  
58 
if(theta > FP_PI)

59 
theta = FP_TWO_PI  theta; 
60 

61 
if(theta > FP_PI_OVER_TWO) {

62 
theta = FP_PI  theta; 
63 
negative = 1;

64 
} 
65  
66 
//Find the nearest table values. FIXME

67 
n = theta / TABLE_STEP; 
68 
while( n < TABLE_LENGTH  1 && (x_np1 = pgm_read_dword(&linspace[n+1]))) 
69 
n++; 
70 

71 
//theta is between x_{n} and x_{n+1}

72 

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

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

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

77  
78 
result = fp_mult(fp_div(FP_PI_OVER_TWO  x_n, 0  y_n), theta  x_n) + y_n;

79 
return negative ? result : result;

80 
} 
81  
82 
if(n == TABLE_LENGTH) {

83 
//We didn't find a value! Oh no!

84 
usb_puts("fp_math: We couldn't find surrounding table values! \n\r

85 
This should never happen!!!\n\r\n\r"); 
86 
return 0; 
87 
} 
88 

89 
//Address the general case. Quadratic interpolation.

90 
//Load in the necessary values.

91 
x_n = pgm_read_dword(&linspace[n]); 
92 
x_np2 = pgm_read_dword(&linspace[n + 2]);

93 

94 
y_n = pgm_read_dword(&fp_cos_table[n]); 
95 
y_np1 = pgm_read_dword(&fp_cos_table[n + 1]);

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

97  
98 
//Calculate first divided differences.

99 
dd_n = fp_div(y_np1  y_n, x_np1  x_n); 
100 
dd_np1 = fp_div(y_np2  y_np1, x_np2  x_np1); 
101  
102 
//Calculate the second divided difference.

103 
second_dd = fp_div(dd_np1  dd_n, x_np2  x_n); 
104 

105 
result = fp_mult(fp_mult(second_dd, theta  x_n), theta  x_np1) 
106 
+ fp_mult(dd_n, theta  x_n) + y_n; 
107  
108 
return negative ? result : result;

109 
} 
110  
111 
//FIXME I didn't write these functions very carefully...

112 
int32_t fp_mult(int32_t a, int32_t b) { 
113 
return (int32_t) (((int64_t)a) * ((int64_t)b)) >> 16; 
114 
} 
115  
116 
int32_t fp_div(int32_t a, int32_t b) { 
117 
return (int32_t) ((int64_t)a << 16) / (int64_t)b; 
118 
} 
119  
120  
121 