root / quad1 / AeroQuad / AeroQuad.h @ 9240aaa3
History | View | Annotate | Download (10.6 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 | #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 |