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