root / quad1 / AeroQuad / FlightCommand.pde @ 9240aaa3
History | View | Annotate | Download (4.59 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 |
// FlightCommand.pde is responsible for decoding transmitter stick combinations |
22 |
// for setting up AeroQuad modes such as motor arming and disarming |
23 |
|
24 |
void readPilotCommands() { |
25 |
receiver.read(); |
26 |
// Read quad configuration commands from transmitter when throttle down |
27 |
if (receiver.getRaw(THROTTLE) < MINCHECK) { |
28 |
zeroIntegralError(); |
29 |
throttleAdjust = 0; |
30 |
//receiver.adjustThrottle(throttleAdjust); |
31 |
// Disarm motors (left stick lower left corner) |
32 |
if (receiver.getRaw(YAW) < MINCHECK && armed == ON) { |
33 |
armed = OFF; |
34 |
motors.commandAllMotors(MINCOMMAND); |
35 |
} |
36 |
// Zero Gyro and Accel sensors (left stick lower left, right stick lower right corner) |
37 |
if ((receiver.getRaw(YAW) < MINCHECK) && (receiver.getRaw(ROLL) > MAXCHECK) && (receiver.getRaw(PITCH) < MINCHECK)) { |
38 |
gyro.calibrate(); // defined in Gyro.h |
39 |
accel.calibrate(); // defined in Accel.h |
40 |
zeroIntegralError(); |
41 |
motors.pulseMotors(3); |
42 |
#ifdef ArduCopter |
43 |
zero_ArduCopter_ADC(); |
44 |
#endif |
45 |
} |
46 |
// Multipilot Zero Gyro sensors (left stick no throttle, right stick upper right corner) |
47 |
if ((receiver.getRaw(ROLL) > MAXCHECK) && (receiver.getRaw(PITCH) > MAXCHECK)) { |
48 |
accel.calibrate(); // defined in Accel.h |
49 |
zeroIntegralError(); |
50 |
motors.pulseMotors(3); |
51 |
#ifdef ArduCopter |
52 |
zero_ArduCopter_ADC(); |
53 |
#endif |
54 |
} |
55 |
// Multipilot Zero Gyros (left stick no throttle, right stick upper left corner) |
56 |
if ((receiver.getRaw(ROLL) < MINCHECK) && (receiver.getRaw(PITCH) > MAXCHECK)) { |
57 |
gyro.calibrate(); |
58 |
zeroIntegralError(); |
59 |
motors.pulseMotors(4); |
60 |
#ifdef ArduCopter |
61 |
zero_ArduCopter_ADC(); |
62 |
#endif |
63 |
} |
64 |
// Arm motors (left stick lower right corner) |
65 |
if (receiver.getRaw(YAW) > MAXCHECK && armed == OFF && safetyCheck == ON) { |
66 |
zeroIntegralError(); |
67 |
armed = ON; |
68 |
for (motor=FRONT; motor < LASTMOTOR; motor++) |
69 |
motors.setMinCommand(motor, MINTHROTTLE); |
70 |
delay(100); |
71 |
} |
72 |
// Prevents accidental arming of motor output if no transmitter command received |
73 |
if (receiver.getRaw(YAW) > MINCHECK) safetyCheck = ON; |
74 |
} |
75 |
|
76 |
// Get center value of roll/pitch/yaw channels when enough throttle to lift off |
77 |
if (receiver.getRaw(THROTTLE) < 1300) { |
78 |
receiver.setTransmitterTrim(ROLL, receiver.getRaw(ROLL)); |
79 |
receiver.setTransmitterTrim(PITCH, receiver.getRaw(PITCH)); |
80 |
receiver.setTransmitterTrim(YAW, receiver.getRaw(YAW)); |
81 |
} |
82 |
|
83 |
// Check Mode switch for Acro or Stable |
84 |
if (receiver.getRaw(MODE) > 1500) { |
85 |
#if defined(AeroQuad_v18) || defined(AeroQuadMega_v2) |
86 |
if (flightMode == ACRO) |
87 |
digitalWrite(LED2PIN, HIGH); |
88 |
#endif |
89 |
flightMode = STABLE; |
90 |
} |
91 |
else { |
92 |
#if defined(AeroQuad_v18) || defined(AeroQuadMega_v2) |
93 |
if (flightMode == STABLE) |
94 |
digitalWrite(LED2PIN, LOW); |
95 |
#endif |
96 |
flightMode = ACRO; |
97 |
} |
98 |
|
99 |
#ifdef AltitudeHold |
100 |
// Check if altitude hold is enabled |
101 |
if (receiver.getRaw(AUX) < 1400) { |
102 |
// return to preset altitude or land? |
103 |
} |
104 |
else if (receiver.getRaw(AUX) < 1700) { |
105 |
if (storeAltitude == ON) { |
106 |
holdAltitude = altitude.getData(); |
107 |
PID[ALTITUDE].integratedError = 0; |
108 |
accel.setOneG(accel.getFlightData(ZAXIS)); |
109 |
storeAltitude = OFF; |
110 |
} |
111 |
altitudeHold = ON; |
112 |
} |
113 |
else { |
114 |
storeAltitude = ON; |
115 |
altitudeHold = OFF; |
116 |
} |
117 |
#endif |
118 |
|
119 |
#ifdef SonarHold |
120 |
// Check if sonar hold is enabled |
121 |
if (receiver.getRaw(AUX2) > 1500) { |
122 |
if (sonarStoreAltitude == ON) { |
123 |
sonarHoldAltitude = 50;//sonar.range; |
124 |
PID[SONAR].integratedError = 0; |
125 |
sonarStoreAltitude = OFF; |
126 |
} |
127 |
sonarHold = ON; |
128 |
} |
129 |
else { |
130 |
sonarStoreAltitude = ON; |
131 |
sonarHold = OFF; |
132 |
} |
133 |
#endif |
134 |
|
135 |
// Use for correcting gyro drift with v2.0 Shield |
136 |
gyro.setReceiverYaw(receiver.getData(YAW)); |
137 |
} |
138 |
|
139 |
|