Statistics
| Branch: | Revision:

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