| 10 |
10 |
|
| 11 |
11 |
#define TABLE_LENGTH 64
|
| 12 |
12 |
#define TABLE_STEP 1621//.160757
|
|
13 |
#define EXP_MAGICONSTANT
|
| 13 |
14 |
|
| 14 |
|
static inline int32_t trig_lookup(int32_t theta, int32_t* table);
|
| 15 |
|
|
| 16 |
15 |
static int32_t linspace[TABLE_LENGTH] PROGMEM = {
|
| 17 |
16 |
0, 1621, 3242, 4863, 6485, 8106, 9727, 11348,
|
| 18 |
17 |
12969, 14590, 16212, 17833, 19454, 21075, 22696, 24317,
|
| ... | ... | |
| 44 |
43 |
return fp_cos(theta + FP_PI_OVER_TWO);
|
| 45 |
44 |
}
|
| 46 |
45 |
|
|
46 |
int32_t fp_exp(int32_t x) {
|
|
47 |
//Try with partial fractions for now.
|
|
48 |
int32_t xsquare = fp_mult(x,x);
|
|
49 |
int32_t result, i;
|
|
50 |
|
|
51 |
if(theta > EXP_MAGICONSTANT) {
|
|
52 |
usb_puts("Output value is out of range.\n\r");
|
|
53 |
return -1;
|
|
54 |
}
|
|
55 |
|
|
56 |
//This is again, massive amounts of lazyness.
|
|
57 |
//The approximation fails outside this range (approximately).
|
|
58 |
if(theta < -17)
|
|
59 |
return 0;
|
|
60 |
|
|
61 |
result = xsquare;
|
|
62 |
for(i= (22 << 16); i > (2 << 16); i-= (4 << 16)) {
|
|
63 |
result += i;
|
|
64 |
result = fp_div(xsquare, result);
|
|
65 |
}
|
|
66 |
|
|
67 |
result += (2 << 16) - x;
|
|
68 |
return (1 << 16) + fp_div(fp_mult((2 << 16), x), result);
|
|
69 |
|
|
70 |
}
|
|
71 |
|
| 47 |
72 |
//Quadratic interpolation.
|
| 48 |
73 |
int32_t fp_cos(int32_t theta) {
|
| 49 |
74 |
uint8_t n;
|
| ... | ... | |
| 69 |
94 |
n++;
|
| 70 |
95 |
|
| 71 |
96 |
//theta is between x_{n} and x_{n+1}
|
| 72 |
|
|
| 73 |
97 |
if(n == TABLE_LENGTH - 1) {
|
| 74 |
98 |
//Perform linear interpolation, since we're close to zero anyway.
|
| 75 |
99 |
x_n = pgm_read_dword(&linspace[TABLE_LENGTH - 1]);
|