Revision 1602 trunk/code/projects/fp_math/fp_math.c

View differences:

fp_math.c
9 9
#define FP_TWO_PI 411775
10 10

  
11 11
#define TABLE_LENGTH 64
12
#define TABLE_STEP 1621//.160757
13
#define EXP_MAGICONSTANT
12
#define TABLE_STEP 1635//.160757
13
#define EXP_MAGICONSTANT 726817 //ln(2^16) * 2^16
14 14

  
15
//#define DEBUG
16

  
17
//TODO This can probably be removed at some point.
15 18
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
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, 
24 27
};
25 28

  
26 29
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
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, 
35 38
};
36 39

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

  
70 73
}
71 74

  
......
76 79
	int32_t x_n, x_np1, x_np2;
77 80
	int32_t y_n, y_np1, y_np2;
78 81
	int32_t dd_n, dd_np1, second_dd, result;
79
	
82

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

  
80 87
	//Move theta into [0, pi/2] w/ appropriate sign.
81 88
	theta = ABS(theta) % FP_TWO_PI; 
82 89

  
......
90 97

  
91 98
	//Find the nearest table values. FIXME
92 99
	n = theta / TABLE_STEP;
93
	while( n < TABLE_LENGTH - 1 && (x_np1 = pgm_read_dword(&linspace[n+1]))) 
100
	while( n < TABLE_LENGTH - 1 
101
		&& (x_np1 = pgm_read_dword(&linspace[n+1]) < theta)) 
94 102
		n++;
95 103
	
96 104
	//theta is between x_{n} and x_{n+1}
105

  
97 106
	if(n == TABLE_LENGTH - 1) { 
98 107
		//Perform linear interpolation, since we're close to zero anyway.
99 108
		x_n = pgm_read_dword(&linspace[TABLE_LENGTH - 1]);
100 109
		y_n = pgm_read_dword(&fp_cos_table[TABLE_LENGTH - 1]);
101 110

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

  
106 117
	if(n == TABLE_LENGTH) { 
107 118
		//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");
119
		usb_puts("fp_math: Fatal! We couldn't find surrounding table values! \n\r");
120
				  
110 121
		return 0;
111 122
	}
112
	
123

  
113 124
	//Address the general case. Quadratic interpolation.
114 125
	//Load in the necessary values.
115 126
	x_n = pgm_read_dword(&linspace[n]);
127
	x_np1 = pgm_read_dword(&linspace[n + 1]);
116 128
	x_np2  = pgm_read_dword(&linspace[n + 2]);
117 129
	
118 130
	y_n = pgm_read_dword(&fp_cos_table[n]);
119 131
	y_np1 = pgm_read_dword(&fp_cos_table[n + 1]);
120 132
	y_np2 = pgm_read_dword(&fp_cos_table[n + 2]);
121 133

  
134
#ifdef DEBUG
135
	sprintf(buf, "x_n = %ld, theta = %ld, x_{n+1} = %ld", x_n, theta, x_np1);
136
	usb_puts(buf);
137
#endif
138

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

Also available in: Unified diff