Statistics
| Revision:

root / trunk / code / projects / fp_math / fp_math.c @ 1586

History | View | Annotate | Download (4.02 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
#define EXP_MAGICONSTANT
14

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

    
26
static int32_t fp_cos_table[TABLE_LENGTH] PROGMEM = {
27
        65536, 65516, 65456, 65356, 65215, 65035, 64815, 64556, 
28
        64257, 63919, 63541, 63125, 62670, 62176, 61645, 61076, 
29
        60470, 59826, 59146, 58430, 57678, 56890, 56068, 55212, 
30
        54322, 53398, 52442, 51454, 50434, 49384, 48303, 47193, 
31
        46053, 44886, 43691, 42470, 41222, 39949, 38652, 37331, 
32
        35988, 34622, 33235, 31828, 30401, 28956, 27492, 26013, 
33
        24517, 23006, 21481, 19943, 18393, 16831, 15260, 13679, 
34
        12089, 10492, 8889, 7280, 5667, 4050, 2431, 811
35
};
36

    
37
//FIXME Lazy implementations of tangent and sine.
38
int32_t fp_tan(int32_t theta) { 
39
        return fp_div(fp_sin(theta), fp_cos(theta));
40
}
41

    
42
int32_t fp_sin(int32_t theta) { 
43
        return fp_cos(theta + FP_PI_OVER_TWO);
44
}
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

    
72
//Quadratic interpolation.
73
int32_t fp_cos(int32_t theta) { 
74
        uint8_t n;        
75
        int8_t negative;
76
        int32_t x_n, x_np1, x_np2;
77
        int32_t y_n, y_np1, y_np2;
78
        int32_t dd_n, dd_np1, second_dd, result;
79
        
80
        //Move theta into [0, pi/2] w/ appropriate sign.
81
        theta = ABS(theta) % FP_TWO_PI; 
82

    
83
        if(theta > FP_PI) 
84
                theta = FP_TWO_PI - theta;
85
        
86
        if(theta > FP_PI_OVER_TWO) {
87
                theta = FP_PI - theta;
88
                negative = 1;
89
        }
90

    
91
        //Find the nearest table values. FIXME
92
        n = theta / TABLE_STEP;
93
        while( n < TABLE_LENGTH - 1 && (x_np1 = pgm_read_dword(&linspace[n+1]))) 
94
                n++;
95
        
96
        //theta is between x_{n} and x_{n+1}
97
        if(n == TABLE_LENGTH - 1) { 
98
                //Perform linear interpolation, since we're close to zero anyway.
99
                x_n = pgm_read_dword(&linspace[TABLE_LENGTH - 1]);
100
                y_n = pgm_read_dword(&fp_cos_table[TABLE_LENGTH - 1]);
101

    
102
                result = fp_mult(fp_div(FP_PI_OVER_TWO - x_n, 0 - y_n), theta - x_n) + y_n;
103
                return negative ? -result : result;
104
        }
105

    
106
        if(n == TABLE_LENGTH) { 
107
                //We didn't find a value! Oh no!
108
                usb_puts("fp_math: We couldn't find surrounding table values! \n\r
109
                                  This should never happen!!!\n\r\n\r");
110
                return 0;
111
        }
112
        
113
        //Address the general case. Quadratic interpolation.
114
        //Load in the necessary values.
115
        x_n = pgm_read_dword(&linspace[n]);
116
        x_np2  = pgm_read_dword(&linspace[n + 2]);
117
        
118
        y_n = pgm_read_dword(&fp_cos_table[n]);
119
        y_np1 = pgm_read_dword(&fp_cos_table[n + 1]);
120
        y_np2 = pgm_read_dword(&fp_cos_table[n + 2]);
121

    
122
        //Calculate first divided differences.
123
        dd_n = fp_div(y_np1 - y_n, x_np1 - x_n);
124
        dd_np1 = fp_div(y_np2 - y_np1, x_np2 - x_np1);
125

    
126
        //Calculate the second divided difference.
127
        second_dd = fp_div(dd_np1 - dd_n, x_np2 - x_n);
128
        
129
        result = fp_mult(fp_mult(second_dd, theta - x_n), theta - x_np1)  
130
                           + fp_mult(dd_n, theta - x_n) + y_n;
131

    
132
        return negative ? -result : result;
133
}
134

    
135
//FIXME I didn't write these functions very carefully...
136
int32_t fp_mult(int32_t a, int32_t b) { 
137
        return (int32_t) (((int64_t)a) * ((int64_t)b)) >>  16;
138
}
139

    
140
int32_t fp_div(int32_t a, int32_t b) { 
141
        return (int32_t) ((int64_t)a << 16) / (int64_t)b;
142
}
143

    
144

    
145