root / quad1 / AeroQuad / Gyro.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 Gyro { |
22 |
public:
|
23 |
float gyroFullScaleOutput;
|
24 |
float gyroScaleFactor;
|
25 |
float smoothFactor;
|
26 |
int gyroChannel[3]; |
27 |
int gyroData[3]; |
28 |
int gyroZero[3]; |
29 |
int gyroADC[3]; |
30 |
byte rollChannel, pitchChannel, yawChannel; |
31 |
int sign[3]; |
32 |
float rawHeading, gyroHeading;
|
33 |
unsigned long currentTime, previousTime; |
34 |
|
35 |
// ************ Correct for gyro drift by FabQuad **************
|
36 |
// ************ http://aeroquad.com/entry.php?4- **************
|
37 |
int lastReceiverYaw, receiverYaw;
|
38 |
long yawAge;
|
39 |
int positiveGyroYawCount;
|
40 |
int negativeGyroYawCount;
|
41 |
int zeroGyroYawCount;
|
42 |
|
43 |
Gyro(void){
|
44 |
sign[ROLL] = 1;
|
45 |
sign[PITCH] = 1;
|
46 |
sign[YAW] = 1;
|
47 |
} |
48 |
|
49 |
// The following function calls must be defined in any new subclasses
|
50 |
virtual void initialize(byte rollChannel, byte pitchChannel, byte yawChannel) {
|
51 |
this->_initialize(rollChannel, pitchChannel, yawChannel); |
52 |
} |
53 |
virtual void measure(void); |
54 |
virtual void calibrate(void); |
55 |
virtual void autoZero(void){}; |
56 |
virtual const int getFlightData(byte); |
57 |
|
58 |
// The following functions are common between all Gyro subclasses
|
59 |
void _initialize(byte rollChannel, byte pitchChannel, byte yawChannel) {
|
60 |
gyroChannel[ROLL] = rollChannel; |
61 |
gyroChannel[PITCH] = pitchChannel; |
62 |
gyroChannel[ZAXIS] = yawChannel; |
63 |
|
64 |
gyroZero[ROLL] = readFloat(GYRO_ROLL_ZERO_ADR); |
65 |
gyroZero[PITCH] = readFloat(GYRO_PITCH_ZERO_ADR); |
66 |
gyroZero[ZAXIS] = readFloat(GYRO_YAW_ZERO_ADR); |
67 |
|
68 |
previousTime = millis(); |
69 |
} |
70 |
|
71 |
const int getRaw(byte axis) { |
72 |
return gyroADC[axis] * sign[axis];
|
73 |
} |
74 |
|
75 |
const int getData(byte axis) { |
76 |
return gyroData[axis] * sign[axis];
|
77 |
} |
78 |
|
79 |
void setData(byte axis, int value) { |
80 |
gyroData[axis] = value; |
81 |
} |
82 |
|
83 |
const int invert(byte axis) { |
84 |
sign[axis] = -sign[axis]; |
85 |
return sign[axis];
|
86 |
} |
87 |
|
88 |
const int getZero(byte axis) { |
89 |
return gyroZero[axis];
|
90 |
} |
91 |
|
92 |
void setZero(byte axis, int value) { |
93 |
gyroZero[axis] = value; |
94 |
} |
95 |
|
96 |
const float getScaleFactor() { |
97 |
return gyroScaleFactor;
|
98 |
} |
99 |
|
100 |
const float getSmoothFactor(void) { |
101 |
return smoothFactor;
|
102 |
} |
103 |
|
104 |
void setSmoothFactor(float value) { |
105 |
smoothFactor = value; |
106 |
} |
107 |
|
108 |
const float rateDegPerSec(byte axis) { |
109 |
return ((gyroADC[axis] * sign[axis]) / 1024.0) * gyroScaleFactor; |
110 |
} |
111 |
|
112 |
const float rateRadPerSec(byte axis) { |
113 |
return radians(((gyroADC[axis] * sign[axis]) / 1024.0) * gyroScaleFactor); |
114 |
} |
115 |
|
116 |
void calculateHeading() {
|
117 |
currentTime = millis(); |
118 |
rawHeading += getData(YAW) * gyroScaleFactor * ((currentTime - previousTime) / 1000.0); |
119 |
previousTime = currentTime; |
120 |
} |
121 |
|
122 |
// returns heading as +/- 180 degrees
|
123 |
const float getHeading(void) { |
124 |
div_t integerDivide; |
125 |
|
126 |
integerDivide = div(rawHeading, 360);
|
127 |
gyroHeading = rawHeading + (integerDivide.quot * -360);
|
128 |
if (gyroHeading > 180) gyroHeading -= 360; |
129 |
if (gyroHeading < -180) gyroHeading += 360; |
130 |
return gyroHeading;
|
131 |
} |
132 |
|
133 |
void setStartHeading(float value) { |
134 |
// since a relative heading, get starting absolute heading from compass class
|
135 |
rawHeading = value; |
136 |
} |
137 |
|
138 |
void setReceiverYaw(int value) { |
139 |
receiverYaw = value; |
140 |
} |
141 |
}; |
142 |
|
143 |
/******************************************************/
|
144 |
/****************** AeroQuad_v1 Gyro ******************/
|
145 |
/******************************************************/
|
146 |
class Gyro_AeroQuad_v1 : public Gyro { |
147 |
private:
|
148 |
int findZero[FINDZERO];
|
149 |
|
150 |
public:
|
151 |
Gyro_AeroQuad_v1() : Gyro() { |
152 |
gyroFullScaleOutput = 500.0; // IDG/IXZ500 full scale output = +/- 500 deg/sec |
153 |
gyroScaleFactor = 0.002; // IDG/IXZ500 sensitivity = 2mV/(deg/sec) |
154 |
} |
155 |
|
156 |
void initialize(void) { |
157 |
analogReference(EXTERNAL); |
158 |
// Configure gyro auto zero pins
|
159 |
pinMode (AZPIN, OUTPUT); |
160 |
digitalWrite(AZPIN, LOW); |
161 |
delay(1);
|
162 |
|
163 |
// rollChannel = 4
|
164 |
// pitchChannel = 3
|
165 |
// yawChannel = 5
|
166 |
this->_initialize(4,3,5); |
167 |
smoothFactor = readFloat(GYROSMOOTH_ADR); |
168 |
} |
169 |
|
170 |
void measure(void) { |
171 |
for (axis = ROLL; axis < LASTAXIS; axis++) {
|
172 |
gyroADC[axis] = analogRead(gyroChannel[axis]) - gyroZero[axis]; |
173 |
gyroData[axis] = smooth(gyroADC[axis], gyroData[axis], smoothFactor); |
174 |
} |
175 |
} |
176 |
|
177 |
const int getFlightData(byte axis) { |
178 |
return getRaw(axis);
|
179 |
} |
180 |
|
181 |
void calibrate() {
|
182 |
autoZero(); |
183 |
writeFloat(gyroZero[ROLL], GYRO_ROLL_ZERO_ADR); |
184 |
writeFloat(gyroZero[PITCH], GYRO_PITCH_ZERO_ADR); |
185 |
writeFloat(gyroZero[YAW], GYRO_YAW_ZERO_ADR); |
186 |
} |
187 |
|
188 |
void autoZero() {
|
189 |
digitalWrite(AZPIN, HIGH); |
190 |
delayMicroseconds(750);
|
191 |
digitalWrite(AZPIN, LOW); |
192 |
delay(8);
|
193 |
|
194 |
for (byte calAxis = ROLL; calAxis < LASTAXIS; calAxis++) {
|
195 |
for (int i=0; i<FINDZERO; i++) |
196 |
findZero[i] = analogRead(gyroChannel[calAxis]); |
197 |
gyroZero[calAxis] = findMode(findZero, FINDZERO); |
198 |
} |
199 |
} |
200 |
}; |
201 |
|
202 |
/******************************************************/
|
203 |
/****************** AeroQuad_v2 Gyro ******************/
|
204 |
/******************************************************/
|
205 |
/*
|
206 |
10kOhm pull-ups on I2C lines.
|
207 |
VDD & VIO = 3.3V
|
208 |
SDA -> A4 (PC4)
|
209 |
SCL -> A5 (PC5)
|
210 |
INT -> D2 (PB2) (or no connection, not used here)
|
211 |
CLK -> GND
|
212 |
*/
|
213 |
class Gyro_AeroQuadMega_v2 : public Gyro { |
214 |
private:
|
215 |
int findZero[FINDZERO];
|
216 |
int gyroAddress;
|
217 |
int data;
|
218 |
int rawData[3]; |
219 |
byte select; // use to select which axis is being read
|
220 |
|
221 |
public:
|
222 |
Gyro_AeroQuadMega_v2() : Gyro() { |
223 |
gyroAddress = 0x69;
|
224 |
gyroFullScaleOutput = 2000.0; // ITG3200 full scale output = +/- 2000 deg/sec |
225 |
gyroScaleFactor = 1.0 / 14.375; // ITG3200 14.375 LSBs per °/sec |
226 |
|
227 |
lastReceiverYaw=0;
|
228 |
yawAge=0;
|
229 |
positiveGyroYawCount=1;
|
230 |
negativeGyroYawCount=1;
|
231 |
zeroGyroYawCount=1;
|
232 |
} |
233 |
|
234 |
void initialize(void) { |
235 |
this->_initialize(0,1,2); |
236 |
smoothFactor = readFloat(GYROSMOOTH_ADR); |
237 |
data = 0x0;
|
238 |
select = ROLL; |
239 |
|
240 |
// Check if gyro is connected
|
241 |
if (readWhoI2C(gyroAddress) != gyroAddress)
|
242 |
Serial.println("Gyro not found!");
|
243 |
|
244 |
// Thanks to SwiftingSpeed for updates on these settings
|
245 |
// http://aeroquad.com/showthread.php?991-AeroQuad-Flight-Software-v2.0&p=11207&viewfull=1#post11207
|
246 |
updateRegisterI2C(gyroAddress, 0x3E, 0x80); // send a reset to the device |
247 |
updateRegisterI2C(gyroAddress, 0x16, 0x1D); // 10Hz low pass filter |
248 |
updateRegisterI2C(gyroAddress, 0x3E, 0x01); // use internal oscillator |
249 |
} |
250 |
|
251 |
void measure(void) { |
252 |
// round robin between each axis so that I2C blocking time is low
|
253 |
if (select == ROLL) sendByteI2C(gyroAddress, 0x1D); |
254 |
if (select == PITCH) sendByteI2C(gyroAddress, 0x1F); |
255 |
if (select == YAW) sendByteI2C(gyroAddress, 0x21); |
256 |
rawData[select] = readWordI2C(gyroAddress); |
257 |
gyroADC[select] = rawData[select] - gyroZero[select]; |
258 |
//if ((gyroADC[YAW] < 5) && (gyroADC[YAW] > -5)) gyroADC[YAW] = 0;
|
259 |
gyroData[select] = smooth(gyroADC[select], gyroData[select], smoothFactor); |
260 |
//if ((gyroData[YAW] < 5) && (gyroData[YAW] > -5)) gyroData[YAW] = 0;
|
261 |
if (select == YAW) {
|
262 |
calculateHeading(); |
263 |
} |
264 |
if (++select == LASTAXIS) select = ROLL; // go to next axis, reset to ROLL if past ZAXIS |
265 |
|
266 |
// ************ Correct for gyro drift by FabQuad **************
|
267 |
// ************ http://aeroquad.com/entry.php?4- **************
|
268 |
// Modified FabQuad's approach to use yaw transmitter command instead of checking accelerometer
|
269 |
if (abs(lastReceiverYaw - receiverYaw) < 15) { |
270 |
yawAge++; |
271 |
if (yawAge >= 4) { // if gyro was the same long enough, we can assume that there is no (fast) rotation |
272 |
if (gyroData[YAW] < 0) { |
273 |
negativeGyroYawCount++; // if gyro still indicates negative rotation, that's additional signal that gyroZero is too low
|
274 |
} |
275 |
else if (gyroData[YAW] > 0) { |
276 |
positiveGyroYawCount++; // additional signal that gyroZero is too high
|
277 |
} |
278 |
else {
|
279 |
zeroGyroYawCount++; // additional signal that gyroZero is correct
|
280 |
} |
281 |
yawAge = 0;
|
282 |
if (zeroGyroYawCount + negativeGyroYawCount + positiveGyroYawCount > 50) { |
283 |
if (negativeGyroYawCount >= 1.3*(zeroGyroYawCount+positiveGyroYawCount)) gyroZero[YAW]--; // enough signals the gyroZero is too low |
284 |
if (positiveGyroYawCount >= 1.3*(zeroGyroYawCount+negativeGyroYawCount)) gyroZero[YAW]++; // enough signals the gyroZero is too high |
285 |
zeroGyroYawCount=0;
|
286 |
negativeGyroYawCount=0;
|
287 |
positiveGyroYawCount=0;
|
288 |
|
289 |
} |
290 |
} |
291 |
} |
292 |
else { // gyro different, restart |
293 |
lastReceiverYaw = receiverYaw; |
294 |
yawAge = 0;
|
295 |
} |
296 |
} |
297 |
|
298 |
const int getFlightData(byte axis) { |
299 |
int reducedData;
|
300 |
|
301 |
reducedData = getRaw(axis) >> 3;
|
302 |
//if ((reducedData < 5) && (reducedData > -5)) reducedData = 0;
|
303 |
return reducedData;
|
304 |
} |
305 |
|
306 |
void calibrate() {
|
307 |
for (byte calAxis = ROLL; calAxis < LASTAXIS; calAxis++) {
|
308 |
for (int i=0; i<FINDZERO; i++) { |
309 |
sendByteI2C(gyroAddress, (calAxis * 2) + 0x1D); |
310 |
findZero[i] = readWordI2C(gyroAddress); |
311 |
} |
312 |
gyroZero[calAxis] = findMode(findZero, FINDZERO); |
313 |
} |
314 |
writeFloat(gyroZero[ROLL], GYRO_ROLL_ZERO_ADR); |
315 |
writeFloat(gyroZero[PITCH], GYRO_PITCH_ZERO_ADR); |
316 |
writeFloat(gyroZero[YAW], GYRO_YAW_ZERO_ADR); |
317 |
} |
318 |
|
319 |
void autoZero() {
|
320 |
for (byte calAxis = ROLL; calAxis < LASTAXIS; calAxis++) {
|
321 |
for (int i=0; i<FINDZERO; i++) { |
322 |
sendByteI2C(gyroAddress, (calAxis * 2) + 0x1D); |
323 |
findZero[i] = readWordI2C(gyroAddress); |
324 |
} |
325 |
gyroZero[calAxis] = findMode(findZero, FINDZERO); |
326 |
} |
327 |
} |
328 |
}; |
329 |
|
330 |
/******************************************************/
|
331 |
/**************** ArduCopter Gyro *********************/
|
332 |
/******************************************************/
|
333 |
#ifdef ArduCopter
|
334 |
class Gyro_ArduCopter : public Gyro { |
335 |
private:
|
336 |
int findZero[FINDZERO];
|
337 |
int rawADC;
|
338 |
|
339 |
public:
|
340 |
Gyro_ArduCopter() : Gyro() { |
341 |
// IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4
|
342 |
// Tested values :
|
343 |
//#define Gyro_Gain_X 0.4 //X axis Gyro gain
|
344 |
//#define Gyro_Gain_Y 0.41 //Y axis Gyro gain
|
345 |
//#define Gyro_Gain_Z 0.41 //Z axis Gyro gain
|
346 |
//#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
|
347 |
//#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
|
348 |
//#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
|
349 |
gyroFullScaleOutput = 500.0; // IDG/IXZ500 full scale output = +/- 500 deg/sec |
350 |
gyroScaleFactor = 0.002; // IDG/IXZ500 sensitivity = 2mV/(deg/sec) |
351 |
} |
352 |
|
353 |
void initialize(void) { |
354 |
// rollChannel = 1
|
355 |
// pitchChannel = 2
|
356 |
// yawChannel = 0
|
357 |
this->_initialize(1, 2, 0); |
358 |
initialize_ArduCopter_ADC(); // this is needed for both gyros and accels, done once in this class
|
359 |
} |
360 |
|
361 |
void measure(void) { |
362 |
for (axis = ROLL; axis < LASTAXIS; axis++) {
|
363 |
rawADC = analogRead_ArduCopter_ADC(gyroChannel[axis]); |
364 |
if (rawADC > 500) // Check if good measurement |
365 |
gyroADC[axis] = gyroZero[axis] - rawADC; |
366 |
gyroData[axis] = gyroADC[axis]; // no smoothing needed
|
367 |
} |
368 |
} |
369 |
|
370 |
const int getFlightData(byte axis) { |
371 |
return getRaw(axis);
|
372 |
} |
373 |
|
374 |
void calibrate() {
|
375 |
for (byte calAxis = ROLL; calAxis < LASTAXIS; calAxis++) {
|
376 |
for (int i=0; i<FINDZERO; i++) { |
377 |
findZero[i] = analogRead_ArduCopter_ADC(gyroChannel[calAxis]); |
378 |
delay(1);
|
379 |
} |
380 |
gyroZero[calAxis] = findMode(findZero, FINDZERO); |
381 |
} |
382 |
writeFloat(gyroZero[ROLL], GYRO_ROLL_ZERO_ADR); |
383 |
writeFloat(gyroZero[PITCH], GYRO_PITCH_ZERO_ADR); |
384 |
writeFloat(gyroZero[YAW], GYRO_YAW_ZERO_ADR); |
385 |
} |
386 |
}; |
387 |
#endif
|
388 |
|
389 |
/******************************************************/
|
390 |
/********************** Wii Gyro **********************/
|
391 |
/******************************************************/
|
392 |
class Gyro_Wii : public Gyro { |
393 |
private:
|
394 |
int findZero[FINDZERO];
|
395 |
|
396 |
public:
|
397 |
Gyro_Wii() : Gyro() { |
398 |
gyroFullScaleOutput = 0;
|
399 |
gyroScaleFactor = 0;
|
400 |
} |
401 |
|
402 |
void initialize(void) { |
403 |
Init_Gyro_Acc(); // defined in DataAquisition.h
|
404 |
smoothFactor = readFloat(GYROSMOOTH_ADR); |
405 |
gyroZero[ROLL] = readFloat(GYRO_ROLL_ZERO_ADR); |
406 |
gyroZero[PITCH] = readFloat(GYRO_PITCH_ZERO_ADR); |
407 |
gyroZero[ZAXIS] = readFloat(GYRO_YAW_ZERO_ADR); |
408 |
} |
409 |
|
410 |
void measure(void) { |
411 |
updateControls(); // defined in DataAcquisition.h
|
412 |
gyroADC[ROLL] = NWMP_gyro[ROLL] - gyroZero[ROLL]; |
413 |
gyroData[ROLL] = smooth(gyroADC[ROLL], gyroData[ROLL], smoothFactor); |
414 |
gyroADC[PITCH] = NWMP_gyro[PITCH] - gyroZero[PITCH]; |
415 |
gyroData[PITCH] = smooth(gyroADC[PITCH], gyroData[PITCH], smoothFactor); |
416 |
gyroADC[YAW] = NWMP_gyro[YAW] - gyroZero[YAW]; |
417 |
gyroData[YAW] = smooth(gyroADC[YAW], gyroData[YAW], smoothFactor); |
418 |
} |
419 |
|
420 |
const int getFlightData(byte axis) { |
421 |
return getRaw(axis);
|
422 |
} |
423 |
|
424 |
void calibrate() {
|
425 |
for (byte calAxis = ROLL; calAxis < LASTAXIS; calAxis++) {
|
426 |
for (int i=0; i<FINDZERO; i++) { |
427 |
updateControls(); |
428 |
findZero[i] = NWMP_gyro[calAxis]; |
429 |
} |
430 |
gyroZero[calAxis] = findMode(findZero, FINDZERO); |
431 |
} |
432 |
writeFloat(gyroZero[ROLL], GYRO_ROLL_ZERO_ADR); |
433 |
writeFloat(gyroZero[PITCH], GYRO_PITCH_ZERO_ADR); |
434 |
writeFloat(gyroZero[YAW], GYRO_YAW_ZERO_ADR); |
435 |
} |
436 |
}; |
437 |
|
438 |
/******************************************************/
|
439 |
/******************* Multipilot Gyro ******************/
|
440 |
/******************************************************/
|
441 |
class Gyro_Multipilot : public Gyro { |
442 |
private:
|
443 |
int findZero[FINDZERO];
|
444 |
|
445 |
public:
|
446 |
Gyro_Multipilot() : Gyro() { |
447 |
gyroFullScaleOutput = 300.0; // ADXR610 full scale output = +/- 300 deg/sec |
448 |
gyroScaleFactor = aref / 0.006; // ADXR610 sensitivity = 6mV/(deg/sec) |
449 |
} |
450 |
|
451 |
void initialize(void) { |
452 |
analogReference(EXTERNAL); |
453 |
// Configure gyro auto zero pins
|
454 |
pinMode (AZPIN, OUTPUT); |
455 |
digitalWrite(AZPIN, LOW); |
456 |
delay(1);
|
457 |
|
458 |
// rollChannel = 1
|
459 |
// pitchChannel = 2
|
460 |
// yawChannel = 0
|
461 |
this->_initialize(1,2,0); |
462 |
smoothFactor = readFloat(GYROSMOOTH_ADR); |
463 |
} |
464 |
|
465 |
void measure(void) { |
466 |
for (axis = ROLL; axis < LASTAXIS; axis++) {
|
467 |
gyroADC[axis] = analogRead(gyroChannel[axis]) - gyroZero[axis]; |
468 |
gyroData[axis] = smooth(gyroADC[axis], gyroData[axis], smoothFactor); |
469 |
} |
470 |
} |
471 |
|
472 |
const int getFlightData(byte axis) { |
473 |
return getRaw(axis);
|
474 |
} |
475 |
|
476 |
void calibrate() {
|
477 |
autoZero(); |
478 |
writeFloat(gyroZero[ROLL], GYRO_ROLL_ZERO_ADR); |
479 |
writeFloat(gyroZero[PITCH], GYRO_PITCH_ZERO_ADR); |
480 |
writeFloat(gyroZero[YAW], GYRO_YAW_ZERO_ADR); |
481 |
} |
482 |
|
483 |
void autoZero() {
|
484 |
digitalWrite(AZPIN, HIGH); |
485 |
delayMicroseconds(750);
|
486 |
digitalWrite(AZPIN, LOW); |
487 |
delay(8);
|
488 |
|
489 |
for (byte calAxis = ROLL; calAxis < LASTAXIS; calAxis++) {
|
490 |
for (int i=0; i<FINDZERO; i++) |
491 |
findZero[i] = analogRead(gyroChannel[calAxis]); |
492 |
gyroZero[calAxis] = findMode(findZero, FINDZERO); |
493 |
} |
494 |
} |
495 |
}; |
496 |
|