Rev 964 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 964 | Rev 967 | ||
---|---|---|---|
- | 1 | ////////////////////////////////////// |
|
- | 2 | // LocoHead - Mathias Kreider, 2011 // |
|
- | 3 | // // |
|
- | 4 | // headtracker.c // |
|
- | 5 | // I2C, angular and filter functions// |
|
- | 6 | ////////////////////////////////////// |
|
- | 7 | ||
1 | #include "vector.h" |
8 | #include "vector.h" |
2 | #include <math.h> |
9 | #include <math.h> |
3 | #include <inttypes.h> |
10 | #include <inttypes.h> |
4 | #include <avr/io.h> |
11 | #include <avr/io.h> |
5 | #include <stdlib.h> |
12 | #include <stdlib.h> |
6 | 13 | ||
7 | extern vector m_max; |
14 | extern vector m_max; |
8 | extern vector m_min; |
15 | extern vector m_min; |
9 | 16 | ||
10 | 17 | ||
11 | void i2c_start() { |
18 | void i2c_start() { |
12 | TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN); // send start condition |
19 | TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN); // send start condition |
13 | while (!(TWCR & (1 << TWINT))); |
20 | while (!(TWCR & (1 << TWINT))); |
14 | } |
21 | } |
15 | 22 | ||
16 | void i2c_write_byte(char byte) { |
23 | void i2c_write_byte(char byte) { |
17 | TWDR = byte; |
24 | TWDR = byte; |
18 | TWCR = (1 << TWINT) | (1 << TWEN); // start address transmission |
25 | TWCR = (1 << TWINT) | (1 << TWEN); // start address transmission |
19 | while (!(TWCR & (1 << TWINT))); |
26 | while (!(TWCR & (1 << TWINT))); |
20 | } |
27 | } |
21 | 28 | ||
22 | char i2c_read_byte() { |
29 | char i2c_read_byte() { |
23 | TWCR = (1 << TWINT) | (1 << TWEA) | (1 << TWEN); // start data reception, transmit ACK |
30 | TWCR = (1 << TWINT) | (1 << TWEA) | (1 << TWEN); // start data reception, transmit ACK |
24 | while (!(TWCR & (1 << TWINT))); |
31 | while (!(TWCR & (1 << TWINT))); |
25 | return TWDR; |
32 | return TWDR; |
26 | } |
33 | } |
27 | 34 | ||
28 | char i2c_read_last_byte() { |
35 | char i2c_read_last_byte() { |
29 | TWCR = (1 << TWINT) | (1 << TWEN); // start data reception |
36 | TWCR = (1 << TWINT) | (1 << TWEN); // start data reception |
30 | while (!(TWCR & (1 << TWINT))); |
37 | while (!(TWCR & (1 << TWINT))); |
31 | return TWDR; |
38 | return TWDR; |
32 | } |
39 | } |
33 | 40 | ||
34 | void i2c_stop() { |
41 | void i2c_stop() { |
35 | TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); // send stop condition |
42 | TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); // send stop condition |
36 | } |
43 | } |
37 | 44 | ||
38 | 45 | ||
39 | // Returns a set of acceleration and raw magnetic readings from the cmp01a. |
46 | // Returns a set of acceleration and raw magnetic readings from the cmp01a. |
40 | void read_data_raw(vector *a, vector *m) |
47 | void read_data_raw(vector *a, vector *m) |
41 | { |
48 | { |
42 | // read accelerometer values |
49 | // read accelerometer values |
43 | i2c_start(); |
50 | i2c_start(); |
44 | i2c_write_byte(0x30); // write acc |
51 | i2c_write_byte(0x30); // write acc |
45 | i2c_write_byte(0xa8); // OUT_X_L_A, MSB set to enable auto-increment |
52 | i2c_write_byte(0xa8); // OUT_X_L_A, MSB set to enable auto-increment |
46 | i2c_start(); // repeated start |
53 | i2c_start(); // repeated start |
47 | i2c_write_byte(0x31); // read acc |
54 | i2c_write_byte(0x31); // read acc |
48 | unsigned char axl = i2c_read_byte(); |
55 | unsigned char axl = i2c_read_byte(); |
49 | unsigned char axh = i2c_read_byte(); |
56 | unsigned char axh = i2c_read_byte(); |
50 | unsigned char ayl = i2c_read_byte(); |
57 | unsigned char ayl = i2c_read_byte(); |
51 | unsigned char ayh = i2c_read_byte(); |
58 | unsigned char ayh = i2c_read_byte(); |
52 | unsigned char azl = i2c_read_byte(); |
59 | unsigned char azl = i2c_read_byte(); |
53 | unsigned char azh = i2c_read_last_byte(); |
60 | unsigned char azh = i2c_read_last_byte(); |
54 | i2c_stop(); |
61 | i2c_stop(); |
55 | 62 | ||
56 | // read magnetometer values |
63 | // read magnetometer values |
57 | i2c_start(); |
64 | i2c_start(); |
58 | i2c_write_byte(0x3C); // write mag |
65 | i2c_write_byte(0x3C); // write mag |
59 | i2c_write_byte(0x03); // OUTXH_M |
66 | i2c_write_byte(0x03); // OUTXH_M |
60 | i2c_start(); // repeated start |
67 | i2c_start(); // repeated start |
61 | i2c_write_byte(0x3D); // read mag |
68 | i2c_write_byte(0x3D); // read mag |
62 | unsigned char mxh = i2c_read_byte(); |
69 | unsigned char mxh = i2c_read_byte(); |
63 | unsigned char mxl = i2c_read_byte(); |
70 | unsigned char mxl = i2c_read_byte(); |
64 | unsigned char myh = i2c_read_byte(); |
71 | unsigned char myh = i2c_read_byte(); |
65 | unsigned char myl = i2c_read_byte(); |
72 | unsigned char myl = i2c_read_byte(); |
66 | unsigned char mzh = i2c_read_byte(); |
73 | unsigned char mzh = i2c_read_byte(); |
67 | unsigned char mzl = i2c_read_last_byte(); |
74 | unsigned char mzl = i2c_read_last_byte(); |
68 | i2c_stop(); |
75 | i2c_stop(); |
69 | 76 | ||
70 | a->x = axh << 8 | axl; |
77 | a->x = axh << 8 | axl; |
71 | a->y = ayh << 8 | ayl; |
78 | a->y = ayh << 8 | ayl; |
72 | a->z = azh << 8 | azl; |
79 | a->z = azh << 8 | azl; |
73 | m->x = mxh << 8 | mxl; |
80 | m->x = mxh << 8 | mxl; |
74 | m->y = myh << 8 | myl; |
81 | m->y = myh << 8 | myl; |
75 | m->z = mzh << 8 | mzl; |
82 | m->z = mzh << 8 | mzl; |
76 | } |
83 | } |
77 | 84 | ||
78 | float IIR2(float x, float* z) |
85 | float IIR2(float x, float* z) |
79 | { |
86 | { |
80 | 87 | ||
81 | //const for butterworth lowpass fc 0.5Hz |
88 | //const for butterworth lowpass fc 0.5Hz |
82 | // const float a[3] = {1.0000, -1.8521, 0.8623}; |
89 | // const float a[3] = {1.0000, -1.8521, 0.8623}; |
83 | // const float b[3] = {0.0026, 0.0051, 0.0026}; |
90 | // const float b[3] = {0.0026, 0.0051, 0.0026}; |
84 | 91 | ||
85 | //const for butterworth lowpass fc 2Hz |
92 | //const for butterworth lowpass fc 2Hz |
86 | const float a[3] = {1.0000, -1.4190, 0.5533}; |
93 | const float a[3] = {1.0000, -1.4190, 0.5533}; |
87 | const float b[3] = {0.0336, 0.0671, 0.0336}; |
94 | const float b[3] = {0.0336, 0.0671, 0.0336}; |
88 | 95 | ||
89 | 96 | ||
90 | float y,r; |
97 | float y,r; |
91 | 98 | ||
92 | r = a[1]*z[0]+a[2]*z[1]; |
99 | r = a[1]*z[0]+a[2]*z[1]; |
93 | y = b[0]*(x-r)+b[1]*z[0]+b[2]*z[1]; |
100 | y = b[0]*(x-r)+b[1]*z[0]+b[2]*z[1]; |
94 | z[1]= z[0]; |
101 | z[1]= z[0]; |
95 | z[0]= x-r; |
102 | z[0]= x-r; |
96 | 103 | ||
97 | return y; |
104 | return y; |
98 | 105 | ||
99 | } |
106 | } |
100 | 107 | ||
101 | 108 | ||
102 | //cancels out movemt below threshold while using step sum to |
109 | //cancels out movemt below threshold while using step sum to |
103 | int thr_filter(int x, int * x_reg, int * y_reg) |
110 | int thr_filter(int x, int * x_reg, int * y_reg) |
104 | { |
111 | { |
105 | int y; |
112 | int y; |
106 | int diff; |
113 | int diff; |
107 | int sum = 0; |
114 | int sum = 0; |
108 | 115 | ||
109 | const int thr = 4; |
116 | const int thr = 4; |
110 | const int lmt = 5; |
117 | const int lmt = 5; |
111 | 118 | ||
112 | diff = x - *x_reg; |
119 | diff = x - *x_reg; |
113 | 120 | ||
114 | if(abs(diff) <= thr) |
121 | if(abs(diff) <= thr) |
115 | { |
122 | { |
116 | sum += diff; |
123 | sum += diff; |
117 | if(abs(sum) >= lmt) |
124 | if(abs(sum) >= lmt) |
118 | { |
125 | { |
119 | sum = 0; |
126 | sum = 0; |
120 | y = x; |
127 | y = x; |
121 | } |
128 | } |
122 | else y = *y_reg; |
129 | else y = *y_reg; |
123 | } |
130 | } |
124 | else |
131 | else |
125 | { |
132 | { |
126 | y = x; |
133 | y = x; |
127 | sum = 0; |
134 | sum = 0; |
128 | } |
135 | } |
129 | 136 | ||
130 | 137 | ||
131 | *x_reg = x; |
138 | *x_reg = x; |
132 | *y_reg = y; |
139 | *y_reg = y; |
133 | 140 | ||
134 | return y; |
141 | return y; |
135 | } |
142 | } |
136 | 143 | ||
137 | 144 | ||
138 | // Returns corrected and low-pass filtered magnetometer and accelerometer values |
145 | // Returns corrected and low-pass filtered magnetometer and accelerometer values |
139 | void read_data(vector *a, vector *m) |
146 | void read_data(vector *a, vector *m) |
140 | { |
147 | { |
141 | //interal state buffers for IIR axis filtering |
148 | //interal state buffers for IIR axis filtering |
142 | static float zm_x[2] = {0.0, 0.0}; |
149 | static float zm_x[2] = {0.0, 0.0}; |
143 | static float zm_y[2] = {0.0, 0.0}; |
150 | static float zm_y[2] = {0.0, 0.0}; |
144 | static float zm_z[2] = {0.0, 0.0}; |
151 | static float zm_z[2] = {0.0, 0.0}; |
145 | static float za_x[2] = {0.0, 0.0}; |
152 | static float za_x[2] = {0.0, 0.0}; |
146 | static float za_y[2] = {0.0, 0.0}; |
153 | static float za_y[2] = {0.0, 0.0}; |
147 | static float za_z[2] = {0.0, 0.0}; |
154 | static float za_z[2] = {0.0, 0.0}; |
148 | 155 | ||
149 | 156 | ||
150 | read_data_raw(a, m); |
157 | read_data_raw(a, m); |
151 | 158 | ||
152 | //low pass filter acc |
159 | //low pass filter acc |
153 | a->x = IIR2(a->x, za_x); |
160 | a->x = IIR2(a->x, za_x); |
154 | a->y = IIR2(a->y, za_y); |
161 | a->y = IIR2(a->y, za_y); |
155 | a->z = IIR2(a->z, za_z); |
162 | a->z = IIR2(a->z, za_z); |
156 | 163 | ||
157 | //compensate scale and offset, low pass filter mag |
164 | //compensate scale and offset, low pass filter mag |
158 | m->x = IIR2(((m->x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0), zm_x); |
165 | m->x = IIR2(((m->x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0), zm_x); |
159 | m->y = IIR2(((m->y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0), zm_y); |
166 | m->y = IIR2(((m->y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0), zm_y); |
160 | m->z = IIR2(((m->z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0), zm_z); |
167 | m->z = IIR2(((m->z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0), zm_z); |
161 | } |
168 | } |
162 | 169 | ||
163 | 170 | ||
164 | 171 | ||
165 | float get_heading(const vector *a, const vector *m, const vector *p) |
172 | float get_heading(const vector *a, const vector *m, const vector *p) |
166 | { |
173 | { |
167 | vector E; |
174 | vector E; |
168 | vector N; |
175 | vector N; |
169 | 176 | ||
170 | // cross magnetic vector (magnetic north + inclination) with "down" (acceleration vector) to produce "west" |
177 | // cross magnetic vector (magnetic north + inclination) with "down" (acceleration vector) to produce "west" |
171 | // -- right hand rule says |
178 | // -- right hand rule says |
172 | 179 | ||
173 | vector_cross(m, a, &E); |
180 | vector_cross(m, a, &E); |
174 | vector_normalize(&E); |
181 | vector_normalize(&E); |
175 | 182 | ||
176 | // cross "down" with "east" to produce "north" (parallel to the ground) |
183 | // cross "down" with "east" to produce "north" (parallel to the ground) |
177 | vector_cross(a, &E, &N); |
184 | vector_cross(a, &E, &N); |
178 | vector_normalize(&N); |
185 | vector_normalize(&N); |
179 | 186 | ||
180 | // compute heading |
187 | // compute heading |
181 | 188 | ||
182 | float heading = atan2(vector_dot(&E, p), vector_dot(&N, p)) * 180.0 / M_PI; |
189 | float heading = atan2(vector_dot(&E, p), vector_dot(&N, p)) * 180.0 / M_PI; |
183 | return heading; |
190 | return heading; |
184 | 191 | ||
185 | } |
192 | } |
186 | 193 | ||
187 | float get_perpendicular(const vector *a, const vector *d, const vector *q) |
194 | float get_perpendicular(const vector *a, const vector *d, const vector *q) |
188 | { |
195 | { |
189 | 196 | ||
190 | 197 | ||
191 | float sign = 0.0; |
198 | float sign = 0.0; |
192 | vector norma = *a; |
199 | vector norma = *a; |
193 | 200 | ||
194 | if (q->x == 0.0) {norma.x = 0.0; sign = norma.y;}// cancel out movement on undesired axis |
201 | if (q->x == 0.0) {norma.x = 0.0; sign = norma.y;}// cancel out movement on undesired axis |
195 | else if (q->y == 0.0) {norma.y = 0.0; sign = norma.x;} |
202 | else if (q->y == 0.0) {norma.y = 0.0; sign = norma.x;} |
196 | vector_normalize(&norma); |
203 | vector_normalize(&norma); |
197 | 204 | ||
198 | 205 | ||
199 | // compute angle |
206 | // compute angle |
200 | float angle = acos(vector_dot(&norma,d)) * 180.0/M_PI; |
207 | float angle = acos(vector_dot(&norma,d)) * 180.0/M_PI; |
201 | if(sign >= 0.0) angle *= -1; |
208 | if(sign >= 0.0) angle *= -1; |
202 | 209 | ||
203 | return angle; |
210 | return angle; |
204 | 211 | ||
205 | } |
212 | } |
206 | 213 | ||
207 | int get_us(float angle, float deg_min, float deg_max, int pwm_min,int pwm_max) |
214 | int get_us(float angle, float deg_min, float deg_max, int pwm_min,int pwm_max) |
208 | { |
215 | { |
209 | //adjust sign change of angular function to new zero offset |
216 | //adjust sign change of angular function to new zero offset |
210 | if(angle < -180.0) angle += 360.0; |
217 | if(angle < -180.0) angle += 360.0; |
211 | if(angle >= 180.0) angle -= 360.0; |
218 | if(angle >= 180.0) angle -= 360.0; |
212 | 219 | ||
213 | //crop |
220 | //crop |
214 | if(angle < deg_min) angle = deg_min; |
221 | if(angle < deg_min) angle = deg_min; |
215 | else if (angle > deg_max) angle = deg_max; |
222 | else if (angle > deg_max) angle = deg_max; |
216 | 223 | ||
217 | //scale to pwm |
224 | //scale to pwm |
218 | float ratio = ((float)(pwm_max - pwm_min)) / (deg_max - deg_min); |
225 | float ratio = ((float)(pwm_max - pwm_min)) / (deg_max - deg_min); |
219 | int diff = ((int)((angle-deg_min) * ratio)); |
226 | int diff = ((int)((angle-deg_min) * ratio)); |
220 | 227 | ||
221 | return pwm_min + diff; |
228 | return pwm_min + diff; |
222 | } |
229 | } |
223 | 230 |