
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 


9 
#define TABLE_LENGTH 128


10 
#define TABLE_STEP 1621//.160757


11 


12 
static int32_t linspace[TABLE_LENGTH] PROGMEM = {


13 
0, 1621, 3242, 4863, 6485, 8106, 9727, 11348,


14 
12969, 14590, 16212, 17833, 19454, 21075, 22696, 24317,


15 
25939, 27560, 29181, 30802, 32423, 34044, 35666, 37287,


16 
38908, 40529, 42150, 43771, 45393, 47014, 48635, 50256,


17 
51877, 53498, 55119, 56741, 58362, 59983, 61604, 63225,


18 
64846, 66468, 68089, 69710, 71331, 72952, 74573, 76195,


19 
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


29 
};


30 


31 
static int32_t cos_table[TABLE_LENGTH] PROGMEM = {


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


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


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


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


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


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


38 
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


48 
};


49 


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


68 


69 
//Quadratic interpolation.


70 
int32_t fp_cos(int32_t theta) {


71 
uint8_t n;


72 
int32_t x_n, x_np1, x_np2;


73 
int32_t y_n, y_np1, y_np2;


74 
int32_t dd_n, dd_np1, second_dd;


75 


76 
//Move theta into [0, pi]


77 
theta = ABS(theta) % FP_TWO_PI;


78 
if(theta > FP_PI)


79 
theta = FP_TWO_PI  theta;


80 


81 
//Find the nearest table values. FIXME


82 
n = theta / TABLE_STEP;


83 
while((x_np1 = pgm_read_dword(&linspace[n+1])) < theta) n++;


84 
usb_puti(n);


85 


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


87 


88 
//Address edge cases.


89 
if(n == 0) {


90 
//TODO


91 
}


92 


93 
if(n == TABLE_LENGTH  1) {


94 
//TODO


95 
}


96 


97 
//Address the general case. Quadratic interpolation.


98 
//Load in the necessary values.


99 
x_n = pgm_read_dword(&linspace[n]);


100 
x_np1 = pgm_read_dword(&linspace[n + 1]);


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


102 


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


106 


107 
//Calculate first divided differences.


108 
dd_n = fp_div(y_np1  y_n, x_np1  x_n);


109 
dd_np1 = fp_div(y_np2  y_np1, x_np2  x_np1);


110 


111 
//Calculate the second divided difference.


112 
second_dd = fp_div(dd_np1  dd_n, x_np2  x_n);


113 


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


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


116 
}


117 


118 
//FIXME I didn't write these functions very carefully...


119 
int32_t fp_mult(int32_t a, int32_t b) {


120 
return (int32_t) (((int64_t)a) * ((int64_t)b)) >> 16;


121 
}


122 


123 
int32_t fp_div(int32_t a, int32_t b) {


124 
return (int32_t) ((int64_t)a << 16) / (int64_t)b;


125 
}


126 


127 


128 
