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