Revision 9240aaa3

View differences:

README
1
Welcome to the respository for the quadrotor project!
2

  
3
Notable parts of the repository:
4

  
5
 * quad1 contains any resources relevant to the first quadrotor developed by the project:
6
 ** AeroQuad files and documentation
7
 * rgbdslam contains rgbdslam files modified to run on PandaBoard.
8

  
9
Please add to this README as new files are added.
README.txt
1
New git repository!
quad1/AeroQuad/Accel.h
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
};
quad1/AeroQuad/AeroQuad.h
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
#include <stdlib.h>
22
#include <math.h>
23
#include "WProgram.h"
24
#include "pins_arduino.h"
25

  
26
// Flight Software Version
27
#define VERSION 2.1
28

  
29
#define BAUD 115200
30
#define LEDPIN 13
31
#define ON 1
32
#define OFF 0
33

  
34
#ifdef AeroQuadMega_v2  
35
  #define LED2PIN 4
36
  #define LED3PIN 31
37
#endif
38
#ifdef AeroQuad_v18
39
  #define LED2PIN 12
40
  #define LED3PIN 12
41
#endif
42

  
43
// Basic axis definitions
44
#define ROLL 0
45
#define PITCH 1
46
#define YAW 2
47
#define THROTTLE 3
48
#define MODE 4
49
#define AUX 5
50
#define AUX2 6
51
#define XAXIS 0
52
#define YAXIS 1
53
#define ZAXIS 2
54
#define LASTAXIS 3
55
#define LEVELROLL 3
56
#define LEVELPITCH 4
57
#define LASTLEVELAXIS 5
58
#define HEADING 5
59
#define LEVELGYROROLL 6
60
#define LEVELGYROPITCH 7
61
#define ALTITUDE 8
62
#define ZDAMPENING 9
63
#define SONAR 10
64

  
65
// PID Variables
66
struct PIDdata {
67
  float P, I, D;
68
  float lastPosition;
69
  float integratedError;
70
  //float windupGuard; // Thinking about having individual wind up guards for each PID
71
} PID[11];
72
// This struct above declares the variable PID[] to hold each of the PID values for various functions
73
// The following constants are declared in AeroQuad.h
74
// ROLL = 0, PITCH = 1, YAW = 2 (used for Arcobatic Mode, gyros only)
75
// ROLLLEVEL = 3, PITCHLEVEL = 4, LEVELGYROROLL = 6, LEVELGYROPITCH = 7 (used for Stable Mode, accels + gyros)
76
// HEADING = 5 (used for heading hold)
77
// ALTITUDE = 8 (used for altitude hold)
78
// ZDAMPENING = 9 (used in altitude hold to dampen vertical accelerations)
79
// SONAR = 10 (used for sonar hold)
80
float windupGuard; // Read in from EEPROM
81

  
82
// Smoothing filter parameters
83
#define GYRO 0
84
#define ACCEL 1
85
#define FINDZERO 50
86
float smoothHeading;
87

  
88
// Sensor pin assignments
89
#define PITCHACCELPIN 0
90
#define ROLLACCELPIN 1
91
#define ZACCELPIN 2
92
#define PITCHRATEPIN 3
93
#define ROLLRATEPIN 4
94
#define YAWRATEPIN 5
95
#define AZPIN 12 // Auto zero pin for IDG500 gyros
96

  
97
// Motor control variables
98
#define FRONT 0
99
#define REAR 1
100
#define RIGHT 2
101
#define LEFT 3
102
#define MOTORID1 0		
103
#define MOTORID2 1		
104
#define MOTORID3 2		
105
#define MOTORID4 3		
106
#define MOTORID5 4		
107
#define MOTORID6 5
108
#define MINCOMMAND 1000
109
#define MAXCOMMAND 2000
110
#if defined(plusConfig) || defined(XConfig)
111
  #define LASTMOTOR 4
112
#endif
113
#if defined(HEXACOAXIAL) || defined(HEXARADIAL)
114
  #define LASTMOTOR 6
115
#endif
116
byte motor;
117

  
118
// Analog Reference Value
119
// This value provided from Configurator
120
// Use a DMM to measure the voltage between AREF and GND
121
// Enter the measured voltage below to define your value for aref
122
// If you don't have a DMM use the following:
123
// AeroQuad Shield v1.7, aref = 3.0
124
// AeroQuad Shield v1.6 or below, aref = 2.8
125
float aref; // Read in from EEPROM
126
int axis;
127

  
128
// Flight Mode
129
#define ACRO 0
130
#define STABLE 1
131
byte flightMode;
132
int minAcro; // Read in from EEPROM, defines min throttle during flips
133

  
134
// Auto level setup
135
int levelAdjust[2] = {0,0};
136
int levelLimit; // Read in from EEPROM
137
int levelOff; // Read in from EEPROM
138
// Scale to convert 1000-2000 PWM to +/- 45 degrees
139
float mLevelTransmitter = 0.09;
140
float bLevelTransmitter = -135;
141

  
142
// Heading hold
143
byte headingHoldConfig;
144
//float headingScaleFactor;
145
float heading = 0; // measured heading from yaw gyro (process variable)
146
float headingHold = 0; // calculated adjustment for quad to go to heading (PID output)
147
float relativeHeading = 0; // current heading the quad is set to (set point)
148
float absoluteHeading = 0;;
149
float setHeading = 0;
150
float commandedYaw = 0;
151

  
152
// Altitude Hold
153
#define TEMPERATURE 0
154
#define PRESSURE 1
155
int throttleAdjust = 0;
156
int minThrottleAdjust = -50;
157
int maxThrottleAdjust = 50;
158
float holdAltitude;
159
float zDampening;
160
byte storeAltitude = OFF;
161
byte altitudeHold = OFF;
162

  
163
// Sonar Hold
164
int sonarThrottleAdjust = 0;
165
int sonarMinThrottleAdjust = -150;
166
int sonarMaxThrottleAdjust = 150;
167
float sonarHoldAltitude;
168
byte sonarStoreAltitude = OFF;
169
byte sonarHold = OFF;
170

  
171
// Receiver variables
172
#define TIMEOUT 25000
173
#define MINCOMMAND 1000
174
#define MIDCOMMAND 1500
175
#define MAXCOMMAND 2000
176
#define MINDELTA 200
177
#define MINCHECK MINCOMMAND + 100
178
#define MAXCHECK MAXCOMMAND - 100
179
#define MINTHROTTLE MINCOMMAND + 100
180
#define LEVELOFF 100
181
#define LASTCHANNEL 7
182
byte channel;
183
int delta;
184

  
185
#define RISING_EDGE 1
186
#define FALLING_EDGE 0
187
#define MINONWIDTH 950
188
#define MAXONWIDTH 2075
189
#define MINOFFWIDTH 12000
190
#define MAXOFFWIDTH 24000
191

  
192
// Flight angle variables
193
float timeConstant;
194
/*float rateRadPerSec(byte axis);
195
float rateDegPerSec(byte axis);
196
float angleDeg(byte axis);
197
*/
198

  
199
// Camera stabilization variables
200
// Note: stabilization camera software is still under development
201
#ifdef Camera
202
  #define ROLLCAMERAPIN 8
203
  #define PITCHCAMERAPIN 13
204
  // map +/-90 degrees to 1000-2000
205
  float mCamera = 5.556;
206
  float bCamera = 1500;
207
  Servo rollCamera;
208
  Servo pitchCamera;
209
#endif
210

  
211
// ESC Calibration
212
byte calibrateESC = 0;
213
int testCommand = 1000;
214

  
215
// Communication
216
char queryType = 'X';
217
byte tlmType = 0;
218
char string[32];
219
byte armed = OFF;
220
byte safetyCheck = OFF;
221
byte update = 0;
222

  
223
/**************************************************************/
224
/******************* Loop timing parameters *******************/
225
/**************************************************************/
226
#define RECEIVERLOOPTIME 20
227
#define TELEMETRYLOOPTIME 100
228
#define FASTTELEMETRYTIME 10
229
#define CONTROLLOOPTIME 2
230
#define CAMERALOOPTIME 20
231
#define AILOOPTIME 2
232
#define COMPASSLOOPTIME 100
233
#define ALTITUDELOOPTIME 26
234
#define SONARLOOPTIME 70
235

  
236
float AIdT = AILOOPTIME / 1000.0;
237
float controldT = CONTROLLOOPTIME / 1000.0;
238
float G_Dt = 0.02;
239

  
240
unsigned long previousTime = 0;
241
unsigned long currentTime = 0;
242
unsigned long deltaTime = 0;
243
unsigned long receiverTime = 0;
244
unsigned long telemetryTime = 50; // make telemetry output 50ms offset from receiver check
245
unsigned long sensorTime = 0;
246
unsigned long controlLoopTime = 1; // offset control loop from analog input loop by 1ms
247
unsigned long cameraTime = 10;
248
unsigned long fastTelemetryTime = 0;
249
unsigned long autoZeroGyroTime = 0;
250
unsigned long compassTime = 25;
251
unsigned long altitudeTime = 0;
252
unsigned long sonarTime=0;
253

  
254
/**************************************************************/
255
/********************** Debug Parameters **********************/
256
/**************************************************************/
257
// Enable/disable control loops for debug
258
//#define DEBUG
259
byte receiverLoop = ON;
260
byte telemetryLoop = ON;
261
byte sensorLoop = ON;
262
byte controlLoop = ON;
263
byte cameraLoop = ON; // Note: stabilization camera software is still under development, moved to Arduino Mega
264
byte fastTransfer = OFF; // Used for troubleshooting
265
byte testSignal = LOW;
266
byte sonarLoop = ON;
267
// Measured test signal with an oscilloscope:
268
// All loops on = 2.4 ms
269
// Analog Input and Control loop on = 2.0 ms
270
// Analog Input loop on = 1.6 ms
271
// Analog Input loop on = 1 ms (no comp filter)
272
// Analog Input loop on = 0.6 ms (comp filter only)
273
// Control loop on = 0.4 ms
274
// Receiver loop = 0.4 ms
275
// Telemetry loop = .04 ms (with no telemetry transmitted)
276
// Fast Telemetry Transfer (sending 12 bytes = 1.1 ms, sending 14 bytes = 1.3 ms, sending 16 bytes = 1.45 ms, sending 18 bytes = 1.625 ms) 2 bytes = 0.175 ms
277

  
278

  
279
// **************************************************************
280
// *************************** EEPROM ***************************
281
// **************************************************************
282
// EEPROM storage addresses
283
#define PGAIN_ADR 0
284
#define IGAIN_ADR 4
285
#define DGAIN_ADR 8
286
#define LEVEL_PGAIN_ADR 12
287
#define LEVEL_IGAIN_ADR 16
288
#define LEVEL_DGAIN_ADR 20
289
#define YAW_PGAIN_ADR 24
290
#define YAW_IGAIN_ADR 28
291
#define YAW_DGAIN_ADR 32
292
#define WINDUPGUARD_ADR 36
293
#define LEVELLIMIT_ADR 40
294
#define LEVELOFF_ADR 44
295
#define XMITFACTOR_ADR 48
296
#define GYROSMOOTH_ADR 52
297
#define ACCSMOOTH_ADR 56
298
#define LEVELPITCHCAL_ADR 60
299
#define LEVELROLLCAL_ADR 64
300
#define LEVELZCAL_ADR 68
301
#define FILTERTERM_ADR 72
302
#define MODESMOOTH_ADR 76
303
#define ROLLSMOOTH_ADR 80
304
#define PITCHSMOOTH_ADR 84
305
#define YAWSMOOTH_ADR 88
306
#define THROTTLESMOOTH_ADR 92
307
#define GYRO_ROLL_ZERO_ADR 96
308
#define GYRO_PITCH_ZERO_ADR 100
309
#define GYRO_YAW_ZERO_ADR 104
310
#define PITCH_PGAIN_ADR 124
311
#define PITCH_IGAIN_ADR 128
312
#define PITCH_DGAIN_ADR 132
313
#define LEVEL_PITCH_PGAIN_ADR 136
314
#define LEVEL_PITCH_IGAIN_ADR 140
315
#define LEVEL_PITCH_DGAIN_ADR 144
316
#define THROTTLESCALE_ADR 148
317
#define THROTTLEOFFSET_ADR 152
318
#define ROLLSCALE_ADR 156
319
#define ROLLOFFSET_ADR 160
320
#define PITCHSCALE_ADR 164
321
#define PITCHOFFSET_ADR 168
322
#define YAWSCALE_ADR 172
323
#define YAWOFFSET_ADR 176
324
#define MODESCALE_ADR 180
325
#define MODEOFFSET_ADR 184
326
#define AUXSCALE_ADR 188
327
#define AUXOFFSET_ADR 192
328
#define AUXSMOOTH_ADR 196
329
#define HEADINGSMOOTH_ADR 200
330
#define HEADING_PGAIN_ADR 204
331
#define HEADING_IGAIN_ADR 208
332
#define HEADING_DGAIN_ADR 212
333
#define AREF_ADR 216
334
#define FLIGHTMODE_ADR 220
335
#define LEVEL_GYRO_ROLL_PGAIN_ADR 224
336
#define LEVEL_GYRO_ROLL_IGAIN_ADR 228
337
#define LEVEL_GYRO_ROLL_DGAIN_ADR 232
338
#define LEVEL_GYRO_PITCH_PGAIN_ADR 236
339
#define LEVEL_GYRO_PITCH_IGAIN_ADR 240
340
#define LEVEL_GYRO_PITCH_DGAIN_ADR 244
341
#define HEADINGHOLD_ADR 248
342
#define MINACRO_ADR 252
343
#define ACCEL1G_ADR 256
344
#define ALTITUDE_PGAIN_ADR 260
345
#define ALTITUDE_IGAIN_ADR 264
346
#define ALTITUDE_DGAIN_ADR 268
347
#define ALTITUDE_MAX_THROTTLE_ADR 272
348
#define ALTITUDE_MIN_THROTTLE_ADR 276
349
#define ALTITUDE_SMOOTH_ADR  280
350
#define ZDAMP_PGAIN_ADR 284
351
#define ZDAMP_IGAIN_ADR 288
352
#define ZDAMP_DGAIN_ADR 292
353

  
354
int findMode(int *data, int arraySize); // defined in Sensors.pde
355
float arctan2(float y, float x); // defined in Sensors.pde
356
float readFloat(int address); // defined in DataStorage.h
357
void writeFloat(float value, int address); // defined in DataStorage.h
358
void readEEPROM(void); // defined in DataStorage.h
359
void readPilotCommands(void); // defined in FlightCommand.pde
360
void readSensors(void); // defined in Sensors.pde
361
void flightControl(void); // defined in FlightControl.pde
362
void readSerialCommand(void);  //defined in SerialCom.pde
363
void sendSerialTelemetry(void); // defined in SerialCom.pde
364
void printInt(int data); // defined in SerialCom.pde
365
float readFloatSerial(void); // defined in SerialCom.pde
366
void comma(void); // defined in SerialCom.pde
367
int findMode(int *data, int arraySize); // defined in Sensors.pde
368

  
quad1/AeroQuad/AeroQuad.pde
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
/****************************************************************************
22
   Before flight, select the different user options for your AeroQuad below
23
   Also, consult the ReadMe.mht file for additional details
24
   If you need additional assitance go to http://AeroQuad.com/forum
25
*****************************************************************************/
26

  
27
/****************************************************************************
28
 ************************* Hardware Configuration ***************************
29
 ****************************************************************************/
30
// Select which hardware you wish to use with the AeroQuad Flight Software
31

  
32
//#define AeroQuad_v1         // Arduino 2009 with AeroQuad Shield v1.7 and below
33
//#define AeroQuad_v18        // Arduino 2009 with AeroQuad Shield v1.8
34
//#define AeroQuad_Wii        // Arduino 2009 with Wii Sensors and AeroQuad Shield v1.x
35
//#define AeroQuadMega_v1     // Arduino Mega with AeroQuad Shield v1.7 and below
36
#define AeroQuadMega_v2     // Arduino Mega with AeroQuad Shield v2.x
37
//#define AeroQuadMega_Wii    // Arduino Mega with Wii Sensors and AeroQuad Shield v2.x
38
//#define ArduCopter          // ArduPilot Mega (APM) with APM Sensor Board
39
//#define Multipilot          // Multipilot board with Lys344 and ADXL 610 Gyro (needs debug)
40
//#define MultipilotI2C       // Active Multipilot I2C and Mixertable (needs debug)
41

  
42
/****************************************************************************
43
 *********************** Define Flight Configuration ************************
44
 ****************************************************************************/
45
// Use only one of the following definitions
46

  
47
#define plusConfig
48
//#define XConfig
49
//#define HEXACOAXIAL
50
//#define HEXARADIAL
51

  
52
// 5DOF IMU Version
53
// Uncomment this if you have the version of the 5DOF IMU which uses the older IDG300 or IDG500 gyros
54
//#define OriginalIMU 
55

  
56
// Yaw Gyro Type
57
// Use only one of the following definitions
58
#define IXZ // IXZ-500 Flat Yaw Gyro or ITG-3200 Triple Axis Gyro
59
//#define IDG // IDG-300 or IDG-500 Dual Axis Gyro
60

  
61
// Camera Stabilization (experimental)
62
// Not yet fully tested and implemented
63
//#define Camera
64

  
65
#define GPS
66

  
67
// Optional Sensors
68
#define HeadingMagHold // Enables HMC5843 Magnetometer
69
#define AltitudeHold // Enables BMP083 Barometer
70
#define SonarHold // Enables SRF02 Sonar
71

  
72
#define DebugPort Serial
73

  
74
/****************************************************************************
75
 ********************* End of User Definition Section ***********************
76
 ****************************************************************************/
77

  
78
#include <EEPROM.h>
79
//#include <Servo.h>
80
#include <Wire.h>
81
#include "AeroQuad.h"
82
#include "I2C.h"
83
#include "PID.h"
84
#include "Filter.h"
85
#include "Receiver.h"
86
#include "DataAcquisition.h"
87
#include "Accel.h"
88
#include "Gyro.h"
89
#include "Motors.h"
90

  
91

  
92
// Create objects defined from Configuration Section above
93
#ifdef AeroQuad_v1 
94
  Accel_AeroQuad_v1 accel;
95
  Gyro_AeroQuad_v1 gyro;
96
  Receiver_AeroQuad receiver;
97
  Motors_PWM motors;
98
  #include "FlightAngle.h"
99
  FlightAngle_CompFilter flightAngle;
100
  //FlightAngle_MultiWii flightAngle;
101
#endif
102

  
103
#ifdef AeroQuad_v18
104
  Accel_AeroQuadMega_v2 accel;
105
  Gyro_AeroQuadMega_v2 gyro;
106
  Receiver_AeroQuad receiver;
107
  Motors_PWM motors;
108
  #include "FlightAngle.h"
109
  FlightAngle_CompFilter flightAngle;
110
  //FlightAngle_DCM flightAngle;
111
#endif
112

  
113
#ifdef AeroQuadMega_v1
114
  // Special thanks to Wilafau for fixes for this setup
115
  // http://aeroquad.com/showthread.php?991-AeroQuad-Flight-Software-v2.0&p=11466&viewfull=1#post11466
116
  Receiver_AeroQuadMega receiver;
117
  Accel_AeroQuad_v1 accel;
118
  Gyro_AeroQuad_v1 gyro;
119
  Motors_PWM motors;
120
  #include "FlightAngle.h"
121
  FlightAngle_DCM flightAngle;
122
#endif
123

  
124
#ifdef AeroQuadMega_v2
125
  Receiver_AeroQuadMega receiver;
126
  Motors_PWM motors;
127
  Accel_AeroQuadMega_v2 accel;
128
  Gyro_AeroQuadMega_v2 gyro;
129
  #include "FlightAngle.h"
130
  FlightAngle_DCM flightAngle;
131
#endif
132

  
133
#ifdef ArduCopter
134
  Gyro_ArduCopter gyro;
135
  Accel_ArduCopter accel;
136
  Receiver_ArduCopter receiver;
137
  Motors_ArduCopter motors;
138
  #include "FlightAngle.h"
139
  FlightAngle_DCM flightAngle;
140
#endif
141

  
142
#ifdef AeroQuad_Wii
143
  Accel_Wii accel;
144
  Gyro_Wii gyro;
145
  Receiver_AeroQuad receiver;
146
  Motors_PWM motors;
147
  #include "FlightAngle.h"
148
  FlightAngle_CompFilter flightAngle;
149
  //FlightAngle_DCM flightAngle;
150
#endif
151

  
152
#ifdef AeroQuadMega_Wii
153
  Accel_Wii accel;
154
  Gyro_Wii gyro;
155
  Receiver_AeroQuadMega receiver;
156
  Motors_PWM motors;
157
  #include "FlightAngle.h"
158
  FlightAngle_DCM flightAngle;
159
#endif
160

  
161
#ifdef Multipilot
162
  Accel_AeroQuad_v1 accel;
163
  Gyro_AeroQuad_v1 gyro;
164
  Receiver_Multipilot receiver;
165
  Motors_PWM motors;
166
  //#define PRINT_MIXERTABLE
167
  //#define TELEMETRY_DEBUG
168
  #include "FlightAngle.h"
169
  FlightAngle_DCM flightAngle;
170
#endif
171

  
172
#ifdef MultipilotI2C  
173
  Accel_AeroQuad_v1 accel;
174
  Gyro_AeroQuad_v1 gyro;
175
  Receiver_Multipilot receiver;
176
  Motors_I2C motors;
177
  //#define PRINT_MIXERTABLE
178
  //#define TELEMETRY_DEBUG
179
  #include "FlightAngle.h"
180
  FlightAngle_DCM flightAngle;
181
#endif
182

  
183
// Optional Sensors, currently defined for the AeroQuad v2.0 Shield
184
// Defined here in case other configurations want to incorporate these sensors
185
#ifdef HeadingMagHold
186
  #include "Compass.h"
187
  Compass_AeroQuad_v2 compass; // HMC5843
188
#endif
189
#ifdef AltitudeHold
190
  #include "Altitude.h"
191
  Altitude_AeroQuad_v2 altitude;  //BMP085
192
#endif
193
#ifdef SonarHold
194
  #include "Sonar.h"
195
  Sonar sonar;  //SRF02
196
#endif
197
#ifdef GPS
198
  #include "GPS.h"
199
  #include "TinyGPS.h"
200
  TinyGPS myGPS;
201
#endif
202

  
203
// Include this last as it contains objects from above declarations
204
#include "DataStorage.h"
205

  
206
// Angle Estimation Objects
207
// Class definition for angle estimation found in FlightAngle.h
208
// Use only one of the following variable declarations
209
// Insert into the appropriate #ifdef's above
210
//#include "FlightAngle.h"
211
//FlightAngle_CompFilter flightAngle; // Use this for Complementary Filter
212
//FlightAngle_KalmanFilter flightAngle; // Use this for Kalman Filter
213
//FlightAngle_IMU flightAngle; // Use this for IMU filter (do not use, for experimentation only)
214
//FlightAngle_MultiWii flightAngle;
215

  
216
// DCM gyro null values are defined in flightAngle.initalize()
217
// Review those values is you add a new #define which uses FlightAngle_DCM
218
//FlightAngle_DCM flightAngle; // Use this for DCM (only for Arduino Mega)
219

  
220
// ************************************************************
221
// ********************** Setup AeroQuad **********************
222
// ************************************************************
223
void setup() {
224
  DebugPort.begin(BAUD);
225
  pinMode(LEDPIN, OUTPUT);
226
  digitalWrite(LEDPIN, LOW);
227
  
228
  #if defined(AeroQuad_v18) || defined(AeroQuadMega_v2)
229
    pinMode(LED2PIN, OUTPUT);
230
    digitalWrite(LED2PIN, LOW);
231
    pinMode(LED3PIN, OUTPUT);
232
    digitalWrite(LED3PIN, LOW);
233
  #endif
234
  
235
  #if defined(AeroQuad_v18) || defined(AeroQuadMega_v2) || defined(AeroQuad_Wii) || defined(AeroQuadMega_Wii)
236
    Wire.begin();
237
  #endif
238
  #if defined(AeroQuad_v18) || defined(AeroQuadMega_v2)
239
    // Recommendation from Mgros to increase I2C speed
240
    // http://aeroquad.com/showthread.php?991-AeroQuad-Flight-Software-v2.0&p=11262&viewfull=1#post11262
241
    TWBR = 12;
242
  #endif
243

  
244
  // Read user values from EEPROM
245
  readEEPROM(); // defined in DataStorage.h
246
  
247
  // Configure motors
248
  motors.initialize(); // defined in Motors.h
249

  
250
  // Setup receiver pins for pin change interrupts
251
  if (receiverLoop == ON)
252
    receiver.initialize(); // defined in Received.h
253
       
254
  // Initialize sensors
255
  // If sensors have a common initialization routine
256
  // insert it into the gyro class because it executes first
257
  gyro.initialize(); // defined in Gyro.h
258
  accel.initialize(); // defined in Accel.h
259
  
260
  // Calibrate sensors
261
  gyro.autoZero(); // defined in Gyro.h
262
  zeroIntegralError();
263
  levelAdjust[ROLL] = 0;
264
  levelAdjust[PITCH] = 0;
265
  
266
  // Setup correct sensor orientation
267
  #ifdef AeroQuad_v1
268
    gyro.invert(PITCH);
269
    gyro.invert(ROLL);
270
  #endif 
271
  #ifdef AeroQuadMega_v1
272
    gyro.invert(PITCH);
273
    gyro.invert(ROLL);
274
  #endif 
275
  #ifdef OriginalIMU
276
    gyro.invert(PITCH);
277
    gyro.invert(ROLL);
278
  #endif
279
  #ifdef IXZ
280
    gyro.invert(YAW);
281
  #endif
282
  #ifdef ArduCopter
283
    gyro.invert(YAW);
284
    gyro.invert(PITCH);
285
    gyro.invert(ROLL);
286
  #endif
287
  #ifdef AeroQuad_Wii
288
    accel.invert(ROLL);
289
    gyro.invert(PITCH);
290
    gyro.invert(YAW);
291
  #endif
292
  #ifdef AeroQuadMega_Wii
293
    accel.invert(ROLL);
294
    gyro.invert(PITCH);
295
    gyro.invert(YAW);
296
  #endif
297
  #ifdef Multipilot
298
    accel.invert(PITCH);
299
    gyro.invert(ROLL);
300
  #endif
301
  
302
  // Flight angle estimiation
303
  flightAngle.initialize(); // defined in FlightAngle.h
304

  
305
  // Optional Sensors
306
  #ifdef HeadingMagHold
307
    compass.initialize();
308
    setHeading = compass.getHeading();
309
  #endif
310
  #ifdef AltitudeHold
311
    altitude.initialize();
312
  #endif
313
  #ifdef SonarHold
314
    sonar.initialize(0x70);
315
  #endif
316
    
317
  // Camera stabilization setup
318
  #ifdef Camera
319
    rollCamera.attach(ROLLCAMERAPIN);
320
    pitchCamera.attach(PITCHCAMERAPIN);
321
  #endif
322
  
323
  previousTime = millis();
324
  digitalWrite(LEDPIN, HIGH);
325
  safetyCheck = 0;
326
  
327

  
328

  
329
  
330
}
331

  
332
// ************************************************************
333
// ******************** Main AeroQuad Loop ********************
334
// ************************************************************
335
void loop () {
336
  // Measure loop rate
337
  currentTime = millis();
338
  deltaTime = currentTime - previousTime;
339
  G_Dt = deltaTime / 1000.0;
340
  previousTime = currentTime;
341
  #ifdef DEBUG
342
    if (testSignal == LOW) testSignal = HIGH;
343
    else testSignal = LOW;
344
    digitalWrite(LEDPIN, testSignal);
345
  #endif
346
  
347
  // show what we're reading from GPS
348
  Serial.print("Show GPS info: \n\n");
349
  gpsdump(myGPS);
350
  
351
  // Reads external pilot commands and performs functions based on stick configuration
352
  if ((currentTime > (receiverTime + RECEIVERLOOPTIME)) && (receiverLoop == ON)) {// 10Hz
353
    readPilotCommands(); // defined in FlightCommand.pde
354
    receiverTime = currentTime;
355
  }
356
  
357
  // Measures sensor data and calculates attitude
358
  if ((currentTime > (sensorTime + AILOOPTIME)) && (sensorLoop == ON)) { // 500Hz
359
    readSensors(); // defined in Sensors.pde
360
    sensorTime = currentTime;
361
  } 
362

  
363
  // Combines external pilot commands and measured sensor data to generate motor commands
364
  if ((currentTime > controlLoopTime + CONTROLLOOPTIME) && (controlLoop == ON)) { // 500Hz
365
    flightControl(); // defined in FlightControl.pde
366
    controlLoopTime = currentTime;
367
  } 
368
  
369
  // Listen for configuration commands and reports telemetry
370
  if ((currentTime > telemetryTime + TELEMETRYLOOPTIME) && (telemetryLoop == ON)) { // 10Hz    
371
    readSerialCommand(); // defined in SerialCom.pde
372
    sendSerialTelemetry(); // defined in SerialCom.pde
373
    telemetryTime = currentTime;
374
  }
375
  
376
#ifdef Camera // Experimental, not fully implemented yet
377
  // Command camera stabilization servos (requires #include <servo.h>)
378
  if ((currentTime > (cameraTime + CAMERALOOPTIME)) && (cameraLoop == ON)) { // 50Hz
379
    rollCamera.write((mCamera * flightAngle.get(ROLL)) + bCamera);
380
    pitchCamera.write((mCamera * flightAngle.get(PITCH)) + bCamera);
381
    cameraTime = currentTime;
382
  }
383
#endif
384
}
quad1/AeroQuad/Altitude.h
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 to define sensors that can determine altitude
22

  
23
// ***********************************************************************
24
// ************************** Altitude Class *****************************
25
// ***********************************************************************
26

  
27
class Altitude {
28
public:
29
  double altitude, rawAltitude;
30
  float groundTemperature; // remove later
31
  float groundPressure; // remove later
32
  float groundAltitude;
33
  float smoothFactor;
34
  
35
  Altitude (void) { 
36
    altitude = 0;
37
    smoothFactor = 0.1;
38
  }
39

  
40
  // **********************************************************************
41
  // The following function calls must be defined inside any new subclasses
42
  // **********************************************************************
43
  virtual void initialize(void); 
44
  virtual void measure(void);
45
  
46
  // *********************************************************
47
  // The following functions are common between all subclasses
48
  // *********************************************************
49
  const float getData(void) {
50
    return altitude - groundAltitude;
51
  }
52
  
53
  const float getRawData(void) {
54
    return rawAltitude;
55
  }
56
  
57
  void setStartAltitude(float value) {
58
    altitude = value;
59
  }
60
  
61
  void measureGround(void) {
62
    // measure initial ground pressure (multiple samples)
63
    groundAltitude = 0;
64
    for (int i=0; i < 25; i++) {
65
      measure();
66
      delay(26);
67
      groundAltitude += rawAltitude;
68
    }
69
    groundAltitude = groundAltitude / 25.0;
70
  }
71
  
72
  void setGroundAltitude(float value) {
73
    groundAltitude = value;
74
  }
75
  
76
  const float getGroundAltitude(void) {
77
    return groundAltitude;
78
  }
79
  
80
  void setSmoothFactor(float value) {
81
    smoothFactor = value;
82
  }
83
  
84
  const float getSmoothFactor(void) {
85
    return smoothFactor;
86
  }
87
};
88

  
89
// ***********************************************************************
90
// ************************* BMP085 Subclass *****************************
91
// ***********************************************************************
92
class Altitude_AeroQuad_v2 : public Altitude {
93
// This sets up the BMP085 from Sparkfun
94
// Code from http://wiring.org.co/learning/libraries/bmp085.html
95
// Also made bug fixes based on BMP085 library from Jordi Munoz and Jose Julio
96
private:
97
  byte overSamplingSetting;
98
  int ac1, ac2, ac3;
99
  unsigned int ac4, ac5, ac6;
100
  int b1, b2, mb, mc, md;
101
  long pressure;
102
  long temperature;
103
  int altitudeAddress;
104
  long rawPressure, rawTemperature;
105
  byte select, pressureCount;
106
  float pressureFactor;
107
  
108
  void requestRawPressure(void) {
109
    updateRegisterI2C(altitudeAddress, 0xF4, 0x34+(overSamplingSetting<<6));
110
  }
111
  
112
  long readRawPressure(void) {
113
    unsigned char msb, lsb, xlsb;
114
    sendByteI2C(altitudeAddress, 0xF6);
115
    Wire.requestFrom(altitudeAddress, 3); // request three bytes
116
    while(!Wire.available()); // wait until data available
117
    msb = Wire.receive();
118
    while(!Wire.available()); // wait until data available
119
    lsb |= Wire.receive();
120
    while(!Wire.available()); // wait until data available
121
    xlsb |= Wire.receive();
122
    return (((long)msb<<16) | ((long)lsb<<8) | ((long)xlsb)) >>(8-overSamplingSetting);
123
  }
124

  
125
  void requestRawTemperature(void) {
126
    updateRegisterI2C(altitudeAddress, 0xF4, 0x2E);
127
  }
128
  
129
  unsigned int readRawTemperature(void) {
130
    sendByteI2C(altitudeAddress, 0xF6);
131
    return readWordWaitI2C(altitudeAddress);
132
  }
133

  
134
public: 
135
  Altitude_AeroQuad_v2() : Altitude(){
136
    altitudeAddress = 0x77;
137
    // oversampling setting
138
    // 0 = ultra low power
139
    // 1 = standard
140
    // 2 = high
141
    // 3 = ultra high resolution
142
    overSamplingSetting = 3;
143
    pressure = 0;
144
    groundPressure = 0;
145
    temperature = 0;
146
    groundTemperature = 0;
147
    groundAltitude = 0;
148
    pressureFactor = 1/5.255;
149
  }
150

  
151
  // ***********************************************************
152
  // Define all the virtual functions declared in the main class
153
  // ***********************************************************
154
  void initialize(void) {
155
    float verifyGroundAltitude;
156
    
157
    sendByteI2C(altitudeAddress, 0xAA);
158
    ac1 = readWordWaitI2C(altitudeAddress);
159
    sendByteI2C(altitudeAddress, 0xAC);
160
    ac2 = readWordWaitI2C(altitudeAddress);
161
    sendByteI2C(altitudeAddress, 0xAE);
162
    ac3 = readWordWaitI2C(altitudeAddress);
163
    sendByteI2C(altitudeAddress, 0xB0);
164
    ac4 = readWordWaitI2C(altitudeAddress);
165
    sendByteI2C(altitudeAddress, 0xB2);
166
    ac5 = readWordWaitI2C(altitudeAddress);
167
    sendByteI2C(altitudeAddress, 0xB4);
168
    ac6 = readWordWaitI2C(altitudeAddress);
169
    sendByteI2C(altitudeAddress, 0xB6);
170
    b1 = readWordWaitI2C(altitudeAddress);
171
    sendByteI2C(altitudeAddress, 0xB8);
172
    b2 = readWordWaitI2C(altitudeAddress);
173
    sendByteI2C(altitudeAddress, 0xBA);
174
    mb = readWordWaitI2C(altitudeAddress);
175
    sendByteI2C(altitudeAddress, 0xBC);
176
    mc = readWordWaitI2C(altitudeAddress);
177
    sendByteI2C(altitudeAddress, 0xBE);
178
    md = readWordWaitI2C(altitudeAddress);
179
    requestRawTemperature(); // setup up next measure() for temperature
180
    select = TEMPERATURE;
181
    pressureCount = 0;
182
    measure();
183
    delay(5); // delay for temperature
184
    measure();
185
    delay(26); // delay for pressure
186
    measureGround();
187
    // check if measured ground altitude is valid
188
    while (abs(getRawData() - getGroundAltitude()) > 10) {
189
      delay(26);
190
      measureGround();
191
    }
192
    setStartAltitude(getGroundAltitude());
193
  }
194
  
195
  void measure(void) {
196
    long x1, x2, x3, b3, b5, b6, p;
197
    unsigned long b4, b7;
198
    int32_t tmp;
199

  
200
    // switch between pressure and tempature measurements
201
    // each loop, since it's slow to measure pressure
202
    if (select == PRESSURE) {
203
      rawPressure = readRawPressure();
204
      if (pressureCount == 3) {
205
        requestRawTemperature();
206
        pressureCount = 0;
207
       select = TEMPERATURE;
208
      }
209
      else
210
        requestRawPressure();
211
      pressureCount++;
212
    }
213
    else { // select must equal TEMPERATURE
214
      rawTemperature = (long)readRawTemperature();
215
      requestRawPressure();
216
      select = PRESSURE;
217
    }
218
    
219
    //calculate true temperature
220
    x1 = ((long)rawTemperature - ac6) * ac5 >> 15;
221
    x2 = ((long) mc << 11) / (x1 + md);
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff