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

5  5 
#include <serial.h> 
6  6  
7  7 
#define ABS(x) (x > 0 ? x : x) 
8 
#define FP_PI_OVER_TWO 102944 

9 
#define FP_TWO_PI 411775 

8  10  
9 
#define TABLE_LENGTH 128


11 
#define TABLE_LENGTH 64


10  12 
#define TABLE_STEP 1621//.160757 
11  13  
14 
static inline int32_t trig_lookup(int32_t theta, int32_t* table); 

15  
12  16 
static int32_t linspace[TABLE_LENGTH] PROGMEM = { 
13  17 
0, 1621, 3242, 4863, 6485, 8106, 9727, 11348, 
14  18 
12969, 14590, 16212, 17833, 19454, 21075, 22696, 24317, 
...  ...  
17  21 
51877, 53498, 55119, 56741, 58362, 59983, 61604, 63225, 
18  22 
64846, 66468, 68089, 69710, 71331, 72952, 74573, 76195, 
19  23 
77816, 79437, 81058, 82679, 84300, 85922, 87543, 89164, 
20 
90785, 92406, 94027, 95648, 97270, 98891, 100512, 102133, 

21 
103754, 105375, 106997, 108618, 110239, 111860, 113481, 115102, 

22 
116724, 118345, 119966, 121587, 123208, 124829, 126451, 128072, 

23 
129693, 131314, 132935, 134556, 136178, 137799, 139420, 141041, 

24 
142662, 144283, 145904, 147526, 149147, 150768, 152389, 154010, 

25 
155631, 157253, 158874, 160495, 162116, 163737, 165358, 166980, 

26 
168601, 170222, 171843, 173464, 175085, 176707, 178328, 179949, 

27 
181570, 183191, 184812, 186433, 188055, 189676, 191297, 192918, 

28 
194539, 196160, 197782, 199403, 201024, 202645, 204266, 205887 

24 
90785, 92406, 94027, 95648, 97270, 98891, 100512, 102133 

29  25 
}; 
30  26  
31 
static int32_t cos_table[TABLE_LENGTH] PROGMEM = { 

27 
static int32_t fp_cos_table[TABLE_LENGTH] PROGMEM = {


32  28 
65536, 65516, 65456, 65356, 65215, 65035, 64815, 64556, 
33  29 
64257, 63919, 63541, 63125, 62670, 62176, 61645, 61076, 
34  30 
60470, 59826, 59146, 58430, 57678, 56890, 56068, 55212, 
...  ...  
36  32 
46053, 44886, 43691, 42470, 41222, 39949, 38652, 37331, 
37  33 
35988, 34622, 33235, 31828, 30401, 28956, 27492, 26013, 
38  34 
24517, 23006, 21481, 19943, 18393, 16831, 15260, 13679, 
39 
12089, 10492, 8889, 7280, 5667, 4050, 2431, 811, 

40 
811, 2431, 4050, 5667, 7280, 8889, 10492, 12089, 

41 
13679, 15260, 16831, 18393, 19943, 21481, 23006, 24517, 

42 
26013, 27492, 28956, 30401, 31828, 33235, 34622, 35988, 

43 
37331, 38652, 39949, 41222, 42470, 43691, 44886, 46053, 

44 
47193, 48303, 49384, 50434, 51454, 52442, 53398, 54322, 

45 
55212, 56068, 56890, 57678, 58430, 59146, 59826, 60470, 

46 
61076, 61645, 62176, 62670, 63125, 63541, 63919, 64257, 

47 
64556, 64815, 65035, 65215, 65356, 65456, 65516, 65536 

35 
12089, 10492, 8889, 7280, 5667, 4050, 2431, 811 

48  36 
}; 
49  37  
50 
static int32_t sin_table[TABLE_LENGTH] PROGMEM = { 

51 
0, 1621, 3241, 4859, 6474, 8085, 9691, 11292, 

52 
12885, 14470, 16047, 17614, 19169, 20714, 22245, 23763, 

53 
25267, 26755, 28226, 29680, 31117, 32534, 33931, 35307, 

54 
36662, 37995, 39304, 40589, 41849, 43084, 44292, 45473, 

55 
46627, 47751, 48847, 49913, 50948, 51952, 52924, 53864, 

56 
54771, 55644, 56484, 57288, 58058, 58792, 59491, 60152, 

57 
60777, 61365, 61915, 62428, 62902, 63338, 63735, 64093, 

58 
64411, 64691, 64930, 65130, 65291, 65411, 65491, 65531, 

59 
65531, 65491, 65411, 65291, 65130, 64930, 64691, 64411, 

60 
64093, 63735, 63338, 62902, 62428, 61915, 61365, 60777, 

61 
60152, 59491, 58792, 58058, 57288, 56484, 55644, 54771, 

62 
53864, 52924, 51952, 50948, 49913, 48847, 47751, 46627, 

63 
45473, 44292, 43084, 41849, 40589, 39304, 37995, 36662, 

64 
35307, 33931, 32534, 31117, 29680, 28226, 26755, 25267, 

65 
23763, 22245, 20714, 19169, 17614, 16047, 14470, 12885, 

66 
11292, 9691, 8085, 6474, 4859, 3241, 1621, 0 

67 
}; 

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 
} 

68  42  
43 
int32_t fp_sin(int32_t theta) { 

44 
return fp_cos(theta + FP_PI_OVER_TWO); 

45 
} 

46  
69  47 
//Quadratic interpolation. 
70  48 
int32_t fp_cos(int32_t theta) { 
71  49 
uint8_t n; 
50 
int8_t negative; 

72  51 
int32_t x_n, x_np1, x_np2; 
73  52 
int32_t y_n, y_np1, y_np2; 
74 
int32_t dd_n, dd_np1, second_dd; 

53 
int32_t dd_n, dd_np1, second_dd, result;


75  54 

76 
//Move theta into [0, pi] 

77 
theta = ABS(theta) % FP_TWO_PI; 

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

56 
theta = ABS(theta) % FP_TWO_PI; 

57  
78  58 
if(theta > FP_PI) 
79  59 
theta = FP_TWO_PI  theta; 
60 


61 
if(theta > FP_PI_OVER_TWO) { 

62 
theta = FP_PI  theta; 

63 
negative = 1; 

64 
} 

80  65  
81  66 
//Find the nearest table values. FIXME 
82  67 
n = theta / TABLE_STEP; 
83 
while((x_np1 = pgm_read_dword(&linspace[n+1])) < theta) n++;


84 
usb_puti(n);


68 
while( n < TABLE_LENGTH  1 && (x_np1 = pgm_read_dword(&linspace[n+1])))


69 
n++;


85  70 

86  71 
//theta is between x_{n} and x_{n+1} 
87  
88 
//Address edge cases. 

89 
if(n == 0) { 

90 
//TODO 

91 
} 

92  72 

93  73 
if(n == TABLE_LENGTH  1) { 
94 
//TODO 

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; 

95  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 
} 

96  88 

97  89 
//Address the general case. Quadratic interpolation. 
98  90 
//Load in the necessary values. 
99  91 
x_n = pgm_read_dword(&linspace[n]); 
100 
x_np1 = pgm_read_dword(&linspace[n + 1]); 

101  92 
x_np2 = pgm_read_dword(&linspace[n + 2]); 
102  93 

103 
y_n = pgm_read_dword(&cos_table[n]); 

104 
y_np1 = pgm_read_dword(&cos_table[n + 1]); 

105 
y_np2 = pgm_read_dword(&cos_table[n + 2]); 

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]);


106  97  
107  98 
//Calculate first divided differences. 
108  99 
dd_n = fp_div(y_np1  y_n, x_np1  x_n); 
...  ...  
111  102 
//Calculate the second divided difference. 
112  103 
second_dd = fp_div(dd_np1  dd_n, x_np2  x_n); 
113  104 

114 
return fp_mult(fp_mult(second_dd, theta  x_n), theta  x_np1) 

115 
+ fp_mult(dd_n, theta  x_n) + y_n; 

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; 

116  109 
} 
117  110  
118  111 
//FIXME I didn't write these functions very carefully... 
Also available in: Unified diff