Statistics
| Revision:

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

History | View | Annotate | Download (4.18 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 1635//.160757
13
#define EXP_MAGICONSTANT 726817 //ln(2^16) * 2^16
14

    
15
//#define DEBUG
16

    
17
//TODO This can probably be removed at some point.
18
static int32_t linspace[TABLE_LENGTH] PROGMEM = { 
19
        0, 1634, 3268, 4902, 6536, 8170, 9804, 11438, 
20
        13072, 14706, 16340, 17974, 19608, 21242, 22876, 24510, 
21
        26144, 27778, 29412, 31047, 32681, 34315, 35949, 37583, 
22
        39217, 40851, 42485, 44119, 45753, 47387, 49021, 50655, 
23
        52289, 53923, 55557, 57191, 58825, 60459, 62093, 63727, 
24
        65361, 66995, 68629, 70263, 71897, 73531, 75165, 76799, 
25
        78433, 80067, 81701, 83335, 84969, 86603, 88237, 89871, 
26
        91506, 93140, 94774, 96408, 98042, 99676, 101310, 102944, 
27
};
28

    
29
static int32_t fp_cos_table[TABLE_LENGTH] PROGMEM = {
30
        65536, 65516, 65455, 65353, 65210, 65027, 64804, 64540, 
31
        64237, 63893, 63509, 63087, 62624, 62123, 61584, 61006, 
32
        60390, 59736, 59046, 58319, 57555, 56756, 55921, 55052, 
33
        54148, 53211, 52241, 51238, 50203, 49138, 48041, 46915, 
34
        45760, 44576, 43364, 42126, 40861, 39571, 38256, 36918, 
35
        35556, 34173, 32768, 31343, 29898, 28435, 26954, 25456, 
36
        23943, 22415, 20872, 19317, 17750, 16171, 14583, 12986, 
37
        11380, 9768, 8149, 6525, 4898, 3267, 1634, 0, 
38
};
39

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

    
45
int32_t fp_sin(int32_t theta) { 
46
        return fp_cos(theta + FP_PI_OVER_TWO);
47
}
48

    
49
int32_t fp_exp(int32_t x) { 
50
        //Try with partial fractions for now.
51
        int32_t xsquare = fp_mult(x,x);
52
        int32_t result, i;
53
        
54
        if(x > EXP_MAGICONSTANT) { 
55
                usb_puts("Output value is out of range.\n\r");
56
                return -1;
57
        }
58
        
59
        //This is again, massive amounts of lazyness.
60
        //The approximation fails outside this range (approximately).
61
        if(x < (-17l << 16))
62
                return 0;
63
        
64
        result = xsquare;
65
        for(i= (22l << 16); i > (2l << 16); i-= (4l << 16)) { 
66
                result += i;
67
                result = fp_div(xsquare, result);
68
        }
69
        
70
        result += (2l << 16) - x;
71
        return (1l << 16) + fp_div(fp_mult((2l << 16), x), result);
72

    
73
}
74

    
75
//Quadratic interpolation.
76
int32_t fp_cos(int32_t theta) { 
77
        uint8_t n;        
78
        int8_t negative;
79
        int32_t x_n, x_np1, x_np2;
80
        int32_t y_n, y_np1, y_np2;
81
        int32_t dd_n, dd_np1, second_dd, result;
82

    
83
#ifdef DEBUG
84
        char buf[128];
85
#endif
86

    
87
        //Move theta into [0, pi/2] w/ appropriate sign.
88
        theta = ABS(theta) % FP_TWO_PI; 
89
        
90
        //Reflecting into [0, pi] doesn't change sign.
91
        if(theta > FP_PI) 
92
                theta = FP_TWO_PI - theta;
93
        
94
        //Reflecting into [0, pi/2] does.
95
        if(theta > FP_PI_OVER_TWO) {
96
                theta = FP_PI - theta;
97
                negative = 1;
98
        }
99
        
100
        //Find n such that theta is between x_{n} and x_{n+1}
101
        n = theta / TABLE_STEP;
102
        while( n < TABLE_LENGTH - 1 
103
                && pgm_read_dword(&linspace[n+1]) < theta) 
104
                n++;
105
        
106
        //Edge case
107
        if(n == TABLE_LENGTH - 1) { 
108
                //Perform linear interpolation, since we're close to zero anyway.
109
                x_n = pgm_read_dword(&linspace[TABLE_LENGTH - 1]);
110
                y_n = pgm_read_dword(&fp_cos_table[TABLE_LENGTH - 1]);
111

    
112
                result = fp_mult(
113
                        fp_div(FP_PI_OVER_TWO - x_n, 0 - y_n), theta - x_n) + y_n;
114
                
115
                return negative ? -result : result;
116
        }
117

    
118
        //Perform quadratic interpolation.
119
        
120
        //Load in the necessary values.
121
        x_n = pgm_read_dword(&linspace[n]);
122
        x_np1 = pgm_read_dword(&linspace[n + 1]);
123
        x_np2  = pgm_read_dword(&linspace[n + 2]);
124
        
125
        y_n = pgm_read_dword(&fp_cos_table[n]);
126
        y_np1 = pgm_read_dword(&fp_cos_table[n + 1]);
127
        y_np2 = pgm_read_dword(&fp_cos_table[n + 2]);
128

    
129
#ifdef DEBUG
130
        sprintf(buf, "x_n = %ld, theta = %ld, x_{n+1} = %ld", x_n, theta, x_np1);
131
        usb_puts(buf);
132
#endif
133

    
134
        //Calculate first divided differences.
135
        dd_n = fp_div(y_np1 - y_n, x_np1 - x_n);
136
        dd_np1 = fp_div(y_np2 - y_np1, x_np2 - x_np1);
137

    
138
        //Calculate the second divided difference.
139
        second_dd = fp_div(dd_np1 - dd_n, x_np2 - x_n);
140
        
141
        result = fp_mult(fp_mult(second_dd, theta - x_n), theta - x_np1)  
142
                           + fp_mult(dd_n, theta - x_n) + y_n;
143

    
144
        return negative ? -result : result;
145
}
146

    
147
//FIXME I didn't write these functions very carefully...
148
int32_t fp_mult(int32_t a, int32_t b) { 
149
        return (int32_t) (((int64_t)a) * ((int64_t)b)) >>  16;
150
}
151

    
152
int32_t fp_div(int32_t a, int32_t b) { 
153
        return (int32_t) ((int64_t)a << 16) / (int64_t)b;
154
}
155

    
156

    
157