root / quad1 / AeroQuad / AeroQuad.pde @ 9240aaa3
History | View | Annotate | Download (11.7 KB)
1 | 9240aaa3 | Alex | /* |
---|---|---|---|
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 | } |