Project

General

Profile

Revision 1579

Initial pass at a 32-bit fixed point library.

- Generated cosine / sine tables, put them in PROGMEM

- Wrote initial quadratic interpolation cosine function.
... not safe to use yet, but it seems to work for non-edge
cases. Need to write a better way to test it.

View differences:

trunk/code/projects/fp_math/fp_math.c
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

  
trunk/code/projects/fp_math/gentables.m
1

  
2
space = int32(round(linspace(0,pi,128) * 2^16))
3
costable = int32(round(cos(linspace(0,pi,128)) * 2^16));
4
sintable = int32(round(sin(linspace(0,pi,128)) * 2^16));
5

  
6
space_file = fopen('space_table.txt', 'w');
7
cos_file = fopen('cos_table.txt', 'w');
8
sin_file = fopen('sin_table.txt', 'w');
9

  
10
for i=1:length(space)
11
	fprintf(space_file, '%d, ', space(i));
12
	if ~mod(i,8)
13
		fprintf(space_file, '\n');
14
	end
15
end
16

  
17
for i=1:length(costable)
18
	fprintf(cos_file, '%d, ', costable(i));
19
	if ~mod(i,8)
20
		fprintf(cos_file, '\n');
21
	end
22
end
23

  
24
for i=1:length(sintable)
25
	fprintf(sin_file, '%d, ', sintable(i));
26
	if ~mod(i,8)
27
		fprintf(sin_file, '\n');
28
	end
29
end
30

  
trunk/code/projects/fp_math/fp_math.h
1

  
2
#ifndef _FP_MATH_H_
3
#define _FP_MATH_H_
4

  
5
#define FP_PI 205887
6
#define FP_TWO_PI 411775
7

  
8
#include <stdint.h>
9

  
10
int32_t fp_cos(int32_t theta);
11
int32_t fp_sin(int32_t theta);
12

  
13
int32_t fp_mult(int32_t a, int32_t b);
14
int32_t fp_div(int32_t a, int32_t b);
15

  
16
#endif
17

  
trunk/code/projects/fp_math/main.c
1
#include <serial.h>
2
#include <stdint.h>
3
#include "fp_math.h"
4

  
5
int main(int argc, char** argv) { 
6
	int32_t i,j;
7
	char buf[128];
8
	
9
	usb_init();
10
	usb_puts("USB Initialized\n\r");
11
		
12
	for(i=10000; i < 11000; i+=5) { 
13
		sprintf(buf, "fp_cos(%ld) = %ld\n\r", i,  fp_cos(i));
14
		usb_puts(buf);
15
	}
16
}
17

  
trunk/code/projects/fp_math/Makefile
1
# this is a local makefile
2

  
3
# Relative path to the root directory (containing lib directory)
4
ifndef COLONYROOT
5
COLONYROOT := ..
6

  
7
# Target file name (without extension).
8
TARGET=main
9

  
10
# Uncomment this to use the wireless library
11
USE_WIRELESS = 1
12

  
13
# com1 = serial port. Use lpt1 to connect to parallel port.
14
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
15
AVRDUDE_PORT = /dev/tty.usbserial-A4001gUV
16

  
17

  
18
else
19
COLONYROOT := ../$(COLONYROOT)
20
endif
21

  
22
include $(COLONYROOT)/Makefile

Also available in: Unified diff