root / quad1 / AeroQuad / Accel.h @ 9240aaa3
History | View | Annotate | Download (15.3 KB)
1 |
/*
|
---|---|
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 |
}; |