Statistics
| Branch: | Revision:

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