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...
