Statistics
| Branch: | Revision:

root / quad1 / AeroQuad / AeroQuad.pde @ 9240aaa3

History | View | Annotate | Download (11.7 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
/****************************************************************************
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
}