root / quad1 / AeroQuad / Accel.h @ 9240aaa3
History | View | Annotate | Download (15.3 KB)
1 | 9240aaa3 | Alex | /*
|
---|---|---|---|
2 | AeroQuad v2.1 - October 2010
|
||
3 | www.AeroQuad.com
|
||
4 | Copyright (c) 2010 Ted Carancho. All rights reserved.
|
||
5 | An Open Source Arduino based multicopter.
|
||
6 |
|
||
7 | This program is free software: you can redistribute it and/or modify
|
||
8 | it under the terms of the GNU General Public License as published by
|
||
9 | the Free Software Foundation, either version 3 of the License, or
|
||
10 | (at your option) any later version.
|
||
11 | |||
12 | This program is distributed in the hope that it will be useful,
|
||
13 | but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
15 | GNU General Public License for more details.
|
||
16 | |||
17 | You should have received a copy of the GNU General Public License
|
||
18 | along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
19 | */
|
||
20 | |||
21 | class Accel { |
||
22 | public:
|
||
23 | float accelScaleFactor;
|
||
24 | float smoothFactor;
|
||
25 | float rawAltitude;
|
||
26 | int accelChannel[3]; |
||
27 | int accelZero[3]; |
||
28 | int accelData[3]; |
||
29 | int accelADC[3]; |
||
30 | int sign[3]; |
||
31 | int accelOneG, zAxis;
|
||
32 | byte rollChannel, pitchChannel, zAxisChannel; |
||
33 | unsigned long currentTime, previousTime; |
||
34 | Accel(void) {
|
||
35 | sign[ROLL] = 1;
|
||
36 | sign[PITCH] = 1;
|
||
37 | sign[YAW] = 1;
|
||
38 | zAxis = 0;
|
||
39 | } |
||
40 | |||
41 | // ******************************************************************
|
||
42 | // The following function calls must be defined in any new subclasses
|
||
43 | // ******************************************************************
|
||
44 | virtual void initialize(void) { |
||
45 | this->_initialize(rollChannel, pitchChannel, zAxisChannel); |
||
46 | smoothFactor = readFloat(ACCSMOOTH_ADR); |
||
47 | } |
||
48 | virtual void measure(void); |
||
49 | virtual void calibrate(void); |
||
50 | virtual const int getFlightData(byte); |
||
51 | |||
52 | // **************************************************************
|
||
53 | // The following functions are common between all Gyro subclasses
|
||
54 | // **************************************************************
|
||
55 | void _initialize(byte rollChannel, byte pitchChannel, byte zAxisChannel) {
|
||
56 | accelChannel[ROLL] = rollChannel; |
||
57 | accelChannel[PITCH] = pitchChannel; |
||
58 | accelChannel[ZAXIS] = zAxisChannel; |
||
59 | |||
60 | accelZero[ROLL] = readFloat(LEVELROLLCAL_ADR); |
||
61 | accelZero[PITCH] = readFloat(LEVELPITCHCAL_ADR); |
||
62 | accelZero[ZAXIS] = readFloat(LEVELZCAL_ADR); |
||
63 | accelOneG = readFloat(ACCEL1G_ADR); |
||
64 | } |
||
65 | |||
66 | const int getRaw(byte axis) { |
||
67 | return accelADC[axis] * sign[axis];
|
||
68 | } |
||
69 | |||
70 | const int getData(byte axis) { |
||
71 | return accelData[axis] * sign[axis];
|
||
72 | } |
||
73 | |||
74 | const int invert(byte axis) { |
||
75 | sign[axis] = -sign[axis]; |
||
76 | return sign[axis];
|
||
77 | } |
||
78 | |||
79 | const int getZero(byte axis) { |
||
80 | return accelZero[axis];
|
||
81 | } |
||
82 | |||
83 | void setZero(byte axis, int value) { |
||
84 | accelZero[axis] = value; |
||
85 | } |
||
86 | |||
87 | const float getScaleFactor(void) { |
||
88 | return accelScaleFactor;
|
||
89 | } |
||
90 | |||
91 | const float getSmoothFactor() { |
||
92 | return smoothFactor;
|
||
93 | } |
||
94 | |||
95 | void setSmoothFactor(float value) { |
||
96 | smoothFactor = value; |
||
97 | } |
||
98 | |||
99 | const float angleRad(byte axis) { |
||
100 | if (axis == PITCH) return arctan2(accelData[PITCH] * sign[PITCH], sqrt((long(accelData[ROLL]) * accelData[ROLL]) + (long(accelData[ZAXIS]) * accelData[ZAXIS]))); |
||
101 | if (axis == ROLL) return arctan2(accelData[ROLL] * sign[ROLL], sqrt((long(accelData[PITCH]) * accelData[PITCH]) + (long(accelData[ZAXIS]) * accelData[ZAXIS]))); |
||
102 | } |
||
103 | |||
104 | const float angleDeg(byte axis) { |
||
105 | return degrees(angleRad(axis));
|
||
106 | } |
||
107 | |||
108 | void setOneG(int value) { |
||
109 | accelOneG = value; |
||
110 | } |
||
111 | |||
112 | const int getOneG(void) { |
||
113 | return accelOneG;
|
||
114 | } |
||
115 | |||
116 | const int getZaxis() { |
||
117 | zAxis = smooth(getFlightData(ZAXIS) - accelOneG, zAxis, 0.25); |
||
118 | return zAxis;
|
||
119 | } |
||
120 | |||
121 | void calculateAltitude() {
|
||
122 | currentTime = millis(); |
||
123 | if ((abs(getData(ROLL)) < 1800) || (abs(getData(PITCH)) < 1800)) |
||
124 | rawAltitude += (getData(ZAXIS) - accelOneG) * accelScaleFactor * ((currentTime - previousTime) / 1000.0); |
||
125 | previousTime = currentTime; |
||
126 | } |
||
127 | |||
128 | const float getAltitude(void) { |
||
129 | return rawAltitude;
|
||
130 | } |
||
131 | }; |
||
132 | |||
133 | /******************************************************/
|
||
134 | /************ AeroQuad v1 Accelerometer ***************/
|
||
135 | /******************************************************/
|
||
136 | class Accel_AeroQuad_v1 : public Accel { |
||
137 | private:
|
||
138 | int findZero[FINDZERO];
|
||
139 | |||
140 | public:
|
||
141 | Accel_AeroQuad_v1() : Accel(){ |
||
142 | // Accelerometer Values
|
||
143 | // Update these variables if using a different accel
|
||
144 | // Output is ratiometric for ADXL 335
|
||
145 | // Note: Vs is not AREF voltage
|
||
146 | // If Vs = 3.6V, then output sensitivity is 360mV/g
|
||
147 | // If Vs = 2V, then it's 195 mV/g
|
||
148 | // Then if Vs = 3.3V, then it's 329.062 mV/g
|
||
149 | accelScaleFactor = 0.000329062; |
||
150 | } |
||
151 | |||
152 | void initialize(void) { |
||
153 | // rollChannel = 1
|
||
154 | // pitchChannel = 0
|
||
155 | // zAxisChannel = 2
|
||
156 | this->_initialize(1, 0, 2); |
||
157 | smoothFactor = readFloat(ACCSMOOTH_ADR); |
||
158 | } |
||
159 | |||
160 | void measure(void) { |
||
161 | for (axis = ROLL; axis < LASTAXIS; axis++) {
|
||
162 | accelADC[axis] = analogRead(accelChannel[axis]) - accelZero[axis]; |
||
163 | accelData[axis] = smooth(accelADC[axis], accelData[axis], smoothFactor); |
||
164 | } |
||
165 | } |
||
166 | |||
167 | const int getFlightData(byte axis) { |
||
168 | return getRaw(axis);
|
||
169 | } |
||
170 | |||
171 | // Allows user to zero accelerometers on command
|
||
172 | void calibrate(void) { |
||
173 | for (byte calAxis = ROLL; calAxis < LASTAXIS; calAxis++) {
|
||
174 | for (int i=0; i<FINDZERO; i++) |
||
175 | findZero[i] = analogRead(accelChannel[calAxis]); |
||
176 | accelZero[calAxis] = findMode(findZero, FINDZERO); |
||
177 | } |
||
178 | |||
179 | // store accel value that represents 1g
|
||
180 | accelOneG = accelZero[ZAXIS]; |
||
181 | // replace with estimated Z axis 0g value
|
||
182 | accelZero[ZAXIS] = (accelZero[ROLL] + accelZero[PITCH]) / 2;
|
||
183 | |||
184 | writeFloat(accelOneG, ACCEL1G_ADR); |
||
185 | writeFloat(accelZero[ROLL], LEVELROLLCAL_ADR); |
||
186 | writeFloat(accelZero[PITCH], LEVELPITCHCAL_ADR); |
||
187 | writeFloat(accelZero[ZAXIS], LEVELZCAL_ADR); |
||
188 | } |
||
189 | }; |
||
190 | |||
191 | /******************************************************/
|
||
192 | /********* AeroQuad Mega v2 Accelerometer *************/
|
||
193 | /******************************************************/
|
||
194 | class Accel_AeroQuadMega_v2 : public Accel { |
||
195 | private:
|
||
196 | int findZero[FINDZERO];
|
||
197 | int accelAddress;
|
||
198 | int data[2]; |
||
199 | int rawData[3]; |
||
200 | byte select; // use to select which axis is being read
|
||
201 | |||
202 | public:
|
||
203 | Accel_AeroQuadMega_v2() : Accel(){ |
||
204 | accelAddress = 0x40; // page 54 and 61 of datasheet |
||
205 | // Accelerometer value if BMA180 setup for 1.5G
|
||
206 | // Page 27 of datasheet = 0.00013g/LSB
|
||
207 | accelScaleFactor = 0.00019; |
||
208 | } |
||
209 | |||
210 | void initialize(void) { |
||
211 | accelZero[ROLL] = readFloat(LEVELROLLCAL_ADR); |
||
212 | accelZero[PITCH] = readFloat(LEVELPITCHCAL_ADR); |
||
213 | accelZero[ZAXIS] = readFloat(LEVELZCAL_ADR); |
||
214 | accelOneG = readFloat(ACCEL1G_ADR); |
||
215 | smoothFactor = readFloat(ACCSMOOTH_ADR); |
||
216 | select = PITCH; |
||
217 | |||
218 | // Check if accel is connected
|
||
219 | if (readWhoI2C(accelAddress) != 0x03) // page 52 of datasheet |
||
220 | Serial.println("Accelerometer not found!");
|
||
221 | |||
222 | // Thanks to SwiftingSpeed for updates on these settings
|
||
223 | // http://aeroquad.com/showthread.php?991-AeroQuad-Flight-Software-v2.0&p=11207&viewfull=1#post11207
|
||
224 | updateRegisterI2C(accelAddress, 0x10, 0xB6); //reset device |
||
225 | delay(10); //sleep 10 ms after reset (page 25) |
||
226 | |||
227 | // In datasheet, summary register map is page 21
|
||
228 | // Low pass filter settings is page 27
|
||
229 | // Range settings is page 28
|
||
230 | updateRegisterI2C(accelAddress, 0x0D, 0x10); //enable writing to control registers |
||
231 | sendByteI2C(accelAddress, 0x20); // register bw_tcs (bits 4-7) |
||
232 | data[0] = readByteI2C(accelAddress); // get current register value |
||
233 | updateRegisterI2C(accelAddress, 0x20, data[0] & 0x0F); // set low pass filter to 10Hz (value = 0000xxxx) |
||
234 | |||
235 | // From page 27 of BMA180 Datasheet
|
||
236 | // 1.0g = 0.13 mg/LSB
|
||
237 | // 1.5g = 0.19 mg/LSB
|
||
238 | // 2.0g = 0.25 mg/LSB
|
||
239 | // 3.0g = 0.38 mg/LSB
|
||
240 | // 4.0g = 0.50 mg/LSB
|
||
241 | // 8.0g = 0.99 mg/LSB
|
||
242 | // 16.0g = 1.98 mg/LSB
|
||
243 | sendByteI2C(accelAddress, 0x35); // register offset_lsb1 (bits 1-3) |
||
244 | data[0] = readByteI2C(accelAddress);
|
||
245 | //Serial.println(data[0], HEX);
|
||
246 | data[0] &= 0xF1; |
||
247 | data[0] |= 1<<1; |
||
248 | updateRegisterI2C(accelAddress, 0x35, data[0]); // set range to +/-1.5g (value = xxxx001x) |
||
249 | //sendByteI2C(accelAddress, 0x35); // register offset_lsb1 (bits 1-3)
|
||
250 | //data[0] = readByteI2C(accelAddress);
|
||
251 | //Serial.println(data[0], HEX);
|
||
252 | } |
||
253 | |||
254 | void measure(void) { |
||
255 | // round robin between each axis so that I2C blocking time is low
|
||
256 | if (select == ROLL) sendByteI2C(accelAddress, 0x04); |
||
257 | if (select == PITCH) sendByteI2C(accelAddress, 0x02); |
||
258 | if (select == ZAXIS) sendByteI2C(accelAddress, 0x06); |
||
259 | rawData[select] = readReverseWordI2C(accelAddress) >> 2; // last 2 bits are not part of measurement |
||
260 | accelADC[select] = rawData[select] - accelZero[select]; // center accel data around zero
|
||
261 | accelData[select] = smooth(accelADC[select], accelData[select], smoothFactor); |
||
262 | if (select == ZAXIS) calculateAltitude();
|
||
263 | if (++select == LASTAXIS) select = ROLL; // go to next axis, reset to ROLL if past ZAXIS |
||
264 | } |
||
265 | |||
266 | const int getFlightData(byte axis) { |
||
267 | return getRaw(axis) >> 4; |
||
268 | } |
||
269 | |||
270 | // Allows user to zero accelerometers on command
|
||
271 | void calibrate(void) { |
||
272 | int dataAddress;
|
||
273 | |||
274 | for (byte calAxis = ROLL; calAxis < ZAXIS; calAxis++) {
|
||
275 | if (calAxis == ROLL) dataAddress = 0x04; |
||
276 | if (calAxis == PITCH) dataAddress = 0x02; |
||
277 | if (calAxis == ZAXIS) dataAddress = 0x06; |
||
278 | for (int i=0; i<FINDZERO; i++) { |
||
279 | sendByteI2C(accelAddress, dataAddress); |
||
280 | findZero[i] = readReverseWordI2C(accelAddress) >> 2; // last two bits are not part of measurement |
||
281 | delay(1);
|
||
282 | } |
||
283 | accelZero[calAxis] = findMode(findZero, FINDZERO); |
||
284 | } |
||
285 | |||
286 | // replace with estimated Z axis 0g value
|
||
287 | accelZero[ZAXIS] = (accelZero[ROLL] + accelZero[PITCH]) / 2;
|
||
288 | // store accel value that represents 1g
|
||
289 | measure(); |
||
290 | accelOneG = getRaw(ZAXIS); |
||
291 | |||
292 | writeFloat(accelOneG, ACCEL1G_ADR); |
||
293 | writeFloat(accelZero[ROLL], LEVELROLLCAL_ADR); |
||
294 | writeFloat(accelZero[PITCH], LEVELPITCHCAL_ADR); |
||
295 | writeFloat(accelZero[ZAXIS], LEVELZCAL_ADR); |
||
296 | } |
||
297 | }; |
||
298 | |||
299 | /******************************************************/
|
||
300 | /*********** ArduCopter ADC Accelerometer *************/
|
||
301 | /******************************************************/
|
||
302 | #ifdef ArduCopter
|
||
303 | class Accel_ArduCopter : public Accel { |
||
304 | private:
|
||
305 | int findZero[FINDZERO];
|
||
306 | int rawADC;
|
||
307 | |||
308 | public:
|
||
309 | Accel_ArduCopter() : Accel(){ |
||
310 | // ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step
|
||
311 | // ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412
|
||
312 | // Tested value : 414
|
||
313 | // #define GRAVITY 414 //this equivalent to 1G in the raw data coming from the accelerometer
|
||
314 | // #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
|
||
315 | accelScaleFactor = 414.0 / 9.81; |
||
316 | } |
||
317 | |||
318 | void initialize(void) { |
||
319 | // rollChannel = 5
|
||
320 | // pitchChannel = 4
|
||
321 | // zAxisChannel = 6
|
||
322 | this->_initialize(5, 4, 6); |
||
323 | } |
||
324 | |||
325 | void measure(void) { |
||
326 | for (axis = ROLL; axis < LASTAXIS; axis++) {
|
||
327 | rawADC = analogRead_ArduCopter_ADC(accelChannel[axis]); |
||
328 | if (rawADC > 500) // Check if measurement good |
||
329 | accelADC[axis] = rawADC - accelZero[axis]; |
||
330 | accelData[axis] = accelADC[axis]; // no smoothing needed
|
||
331 | } |
||
332 | } |
||
333 | |||
334 | const int getFlightData(byte axis) { |
||
335 | return getRaw(axis);
|
||
336 | } |
||
337 | |||
338 | // Allows user to zero accelerometers on command
|
||
339 | void calibrate(void) { |
||
340 | for(byte calAxis = 0; calAxis < LASTAXIS; calAxis++) { |
||
341 | for (int i=0; i<FINDZERO; i++) { |
||
342 | findZero[i] = analogRead_ArduCopter_ADC(accelChannel[calAxis]); |
||
343 | delay(1);
|
||
344 | } |
||
345 | accelZero[calAxis] = findMode(findZero, FINDZERO); |
||
346 | } |
||
347 | |||
348 | // store accel value that represents 1g
|
||
349 | accelOneG = accelZero[ZAXIS]; |
||
350 | // replace with estimated Z axis 0g value
|
||
351 | accelZero[ZAXIS] = (accelZero[ROLL] + accelZero[PITCH]) / 2;
|
||
352 | |||
353 | writeFloat(accelOneG, ACCEL1G_ADR); |
||
354 | writeFloat(accelZero[ROLL], LEVELROLLCAL_ADR); |
||
355 | writeFloat(accelZero[PITCH], LEVELPITCHCAL_ADR); |
||
356 | writeFloat(accelZero[ZAXIS], LEVELZCAL_ADR); |
||
357 | } |
||
358 | }; |
||
359 | #endif
|
||
360 | |||
361 | /******************************************************/
|
||
362 | /****************** Wii Accelerometer *****************/
|
||
363 | /******************************************************/
|
||
364 | class Accel_Wii : public Accel { |
||
365 | private:
|
||
366 | int findZero[FINDZERO];
|
||
367 | |||
368 | public:
|
||
369 | Accel_Wii() : Accel(){ |
||
370 | accelScaleFactor = 0;
|
||
371 | } |
||
372 | |||
373 | void initialize(void) { |
||
374 | smoothFactor = readFloat(ACCSMOOTH_ADR); |
||
375 | accelZero[ROLL] = readFloat(LEVELROLLCAL_ADR); |
||
376 | accelZero[PITCH] = readFloat(LEVELPITCHCAL_ADR); |
||
377 | accelZero[ZAXIS] = readFloat(LEVELZCAL_ADR); |
||
378 | accelOneG = readFloat(ACCEL1G_ADR); |
||
379 | } |
||
380 | |||
381 | void measure(void) { |
||
382 | // Actual measurement performed in gyro class
|
||
383 | // We just update the appropriate variables here
|
||
384 | for (axis = ROLL; axis < LASTAXIS; axis++) {
|
||
385 | accelADC[axis] = NWMP_acc[axis] - accelZero[axis]; |
||
386 | accelData[axis] = smooth(accelADC[axis], accelData[axis], smoothFactor); |
||
387 | } |
||
388 | } |
||
389 | |||
390 | const int getFlightData(byte axis) { |
||
391 | return getRaw(axis);
|
||
392 | } |
||
393 | |||
394 | // Allows user to zero accelerometers on command
|
||
395 | void calibrate(void) { |
||
396 | for(byte calAxis = ROLL; calAxis < LASTAXIS; calAxis++) {
|
||
397 | for (int i=0; i<FINDZERO; i++) { |
||
398 | updateControls(); |
||
399 | findZero[i] = NWMP_acc[calAxis]; |
||
400 | } |
||
401 | accelZero[calAxis] = findMode(findZero, FINDZERO); |
||
402 | } |
||
403 | |||
404 | // store accel value that represents 1g
|
||
405 | accelOneG = accelZero[ZAXIS]; |
||
406 | // replace with estimated Z axis 0g value
|
||
407 | accelZero[ZAXIS] = (accelZero[ROLL] + accelZero[PITCH]) / 2;
|
||
408 | |||
409 | writeFloat(accelOneG, ACCEL1G_ADR); |
||
410 | writeFloat(accelZero[ROLL], LEVELROLLCAL_ADR); |
||
411 | writeFloat(accelZero[PITCH], LEVELPITCHCAL_ADR); |
||
412 | writeFloat(accelZero[ZAXIS], LEVELZCAL_ADR); |
||
413 | } |
||
414 | }; |
||
415 | |||
416 | /******************************************************/
|
||
417 | /************* MultiPilot Accelerometer ***************/
|
||
418 | /******************************************************/
|
||
419 | class Accel_Multipilot : public Accel { |
||
420 | private:
|
||
421 | int findZero[FINDZERO];
|
||
422 | |||
423 | public:
|
||
424 | Accel_Multipilot() : Accel(){ |
||
425 | // Accelerometer Values
|
||
426 | // Update these variables if using a different accel
|
||
427 | // Output is ratiometric for ADXL 335
|
||
428 | // Note: Vs is not AREF voltage
|
||
429 | // If Vs = 3.6V, then output sensitivity is 360mV/g
|
||
430 | // If Vs = 2V, then it's 195 mV/g
|
||
431 | // Then if Vs = 3.3V, then it's 329.062 mV/g
|
||
432 | // Accelerometer Values for LIS344ALH set fs to +- 2G
|
||
433 | // Vdd = 3.3 Volt
|
||
434 | // Zero = Vdd / 2
|
||
435 | // 3300 mV / 5 (+-2G ) = 660
|
||
436 | accelScaleFactor = 0.000660; |
||
437 | } |
||
438 | |||
439 | void initialize(void) { |
||
440 | // rollChannel = 6
|
||
441 | // pitchChannel = 7
|
||
442 | // zAxisChannel = 5
|
||
443 | this->_initialize(6, 7, 5); |
||
444 | smoothFactor = readFloat(ACCSMOOTH_ADR); |
||
445 | } |
||
446 | |||
447 | void measure(void) { |
||
448 | for (axis = ROLL; axis < LASTAXIS; axis++) {
|
||
449 | accelADC[axis] = analogRead(accelChannel[axis]) - accelZero[axis]; |
||
450 | accelData[axis] = smooth(accelADC[axis], accelData[axis], smoothFactor); |
||
451 | } |
||
452 | } |
||
453 | |||
454 | const int getFlightData(byte axis) { |
||
455 | return getRaw(axis);
|
||
456 | } |
||
457 | |||
458 | // Allows user to zero accelerometers on command
|
||
459 | void calibrate(void) { |
||
460 | for (byte calAxis = ROLL; calAxis < LASTAXIS; calAxis++) {
|
||
461 | for (int i=0; i<FINDZERO; i++) |
||
462 | findZero[i] = analogRead(accelChannel[calAxis]); |
||
463 | accelZero[calAxis] = findMode(findZero, FINDZERO); |
||
464 | } |
||
465 | |||
466 | // store accel value that represents 1g
|
||
467 | accelOneG = accelZero[ZAXIS]; |
||
468 | // replace with estimated Z axis 0g value
|
||
469 | accelZero[ZAXIS] = (accelZero[ROLL] + accelZero[PITCH]) / 2;
|
||
470 | |||
471 | writeFloat(accelOneG, ACCEL1G_ADR); |
||
472 | writeFloat(accelZero[ROLL], LEVELROLLCAL_ADR); |
||
473 | writeFloat(accelZero[PITCH], LEVELPITCHCAL_ADR); |
||
474 | writeFloat(accelZero[ZAXIS], LEVELZCAL_ADR); |
||
475 | } |
||
476 | }; |