/
tof_table_gen.c
159 lines (120 loc) · 3.79 KB
/
tof_table_gen.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
/*
Generates arctan table for optimized 3DTOF calculation and verifies the table, bruteforcing every input condition to find maximum error
compared to double precision floating point reference design.
Unit: mm
*/
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#ifndef M_PI
#define M_PI 3.141592653589793238
#endif
#define TOF_TBL_LEN 256
int16_t tof_tbl[TOF_TBL_LEN]; // spans pi/4 (45 degs) of the full atan2 circle
int TOF_TBL_PERIOD, TOF_TBL_HALF_PERIOD, TOF_TBL_QUART_PERIOD; // will be #defines in the generated .h file - used here for verification
#define SENSOR_MAX 2048
int16_t get_tof_tbl(int16_t y, int16_t x)
{
int yy, xx;
if(y<0)
yy = -y;
else
yy = y;
if(x<0)
xx = -x;
else
xx = x;
int swapped = 0;
if(xx<yy)
{
swapped = 1;
int16_t tmp = xx;
xx = yy;
yy = tmp;
}
if(xx == 0) return 0;
int idx = (yy*(TOF_TBL_LEN-1))/xx;
if(idx < 0 || idx > TOF_TBL_LEN-1)
{
printf("Err: OOR idx, y=%d x=%d, yy=%d, xx=%d, idx=%d\n", y,x,yy,xx,idx);
return 0;
}
int16_t res = tof_tbl[idx];
if(swapped) res = TOF_TBL_QUART_PERIOD - res;
if(x<0) res = TOF_TBL_HALF_PERIOD - res;
if(y<0) res = -res;
return res;
}
int main(int argc, char** argv)
{
double fled = 20000000.0/3.0;
double mult = 299792458.0/2.0*1.0/(2.0*M_PI*fled);
double unamb = 299792458.0/2.0*1.0/(fled);
TOF_TBL_PERIOD = unamb*1000;
TOF_TBL_HALF_PERIOD = unamb*1000.0/2.0;
TOF_TBL_QUART_PERIOD = unamb*1000.0/4.0;
FILE* tblh = fopen("tof_table.h", "w");
if(!tblh)
{
printf("Error opening tof_table.h for write\n");
return 1;
}
FILE* tblc = fopen("tof_table.c", "w");
if(!tblc)
{
printf("Error opening tof_table.c for write\n");
return 1;
}
fprintf(tblh, "//AUTOGENERATED FOR Fled = %.3f\n", fled);
fprintf(tblh, "#ifndef TOF_TABLE_H\n");
fprintf(tblh, "#define TOF_TABLE_H\n\n");
fprintf(tblh, "#include <stdint.h>\n\n");
fprintf(tblh, "#define TOF_TBL_LEN %u\n\n", TOF_TBL_LEN);
fprintf(tblh, "#define TOF_TBL_PERIOD %u\n\n", TOF_TBL_PERIOD);
fprintf(tblh, "#define TOF_TBL_HALF_PERIOD %u\n\n", TOF_TBL_HALF_PERIOD);
fprintf(tblh, "#define TOF_TBL_QUART_PERIOD %u\n\n", TOF_TBL_QUART_PERIOD);
fprintf(tblh, "extern const int16_t tof_tbl[TOF_TBL_LEN] __attribute__((section(\".tof_tbl\")));\n\n");
fprintf(tblh, "#endif\n");
fclose(tblh);
fprintf(tblc, "#include <stdint.h>\n");
fprintf(tblc, "#include \"tof_table.h\"\n\n");
fprintf(tblc, "const int16_t tof_tbl[TOF_TBL_LEN] __attribute__((section(\".tof_tbl\"))) =\n");
fprintf(tblc, "{\n");
for(int i=0; i<TOF_TBL_LEN; i++)
{
tof_tbl[i] = (unamb*1000.0*(atan((double)i/(double)TOF_TBL_LEN))/(2*M_PI))+0.5;
fprintf(tblc, "/*i=%4u*/ %5d%s\n", i, tof_tbl[i], (i==TOF_TBL_LEN-1)?"":",");
}
fprintf(tblc, "};\n");
// Check all possible inputs to the "real" formula to find max error:
int max_err = -9999999;
int min_err = 9999999;
for(int y=-SENSOR_MAX; y<SENSOR_MAX; y++)
{
for(int x=-SENSOR_MAX; x<SENSOR_MAX; x++)
{
double actual = (mult * (/*M_PI +*/ atan2(y,x)));
if(actual < 0.0) actual += unamb;
else if(actual > unamb) actual -= unamb;
int actual_i = 1000.0*actual;
int from_table = get_tof_tbl(y, x);
if(from_table < 0) from_table += TOF_TBL_PERIOD;
int err = actual_i - from_table;
if(err > TOF_TBL_PERIOD/2) err -= TOF_TBL_PERIOD;
if(err < -TOF_TBL_PERIOD/2) err += TOF_TBL_PERIOD;
//printf("y=%5d x=%5d actual=%5u table=%5u err=%5d\n", y, x, actual_i, from_table, err);
if(err > max_err)
{
max_err = err;
printf("max_err at y=%5d x=%5d actual=%5u table=%5u err=%5d\n", y, x, actual_i, from_table, err);
}
if(err < min_err)
{
min_err = err;
printf("min_err at y=%5d x=%5d actual=%5u table=%5u err=%5d\n", y, x, actual_i, from_table, err);
}
}
}
printf("Memory required for the table: %lu bytes\n", sizeof(tof_tbl));
return 0;
}