Statistics
| Branch: | Revision:

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
};