root / quad1 / AeroQuad / FlightCommand.pde @ 9240aaa3
History | View | Annotate | Download (4.59 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 | // 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 |