root / quad1 / AeroQuad / FlightControl.pde @ 9240aaa3
History | View | Annotate | Download (12.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 |
// FlightControl.pde is responsible for combining sensor measurements and |
22 |
// transmitter commands into motor commands for the defined flight configuration (X, +, etc.) |
23 |
|
24 |
void flightControl(void) { |
25 |
if (flightMode == ACRO) { |
26 |
// Acrobatic Mode |
27 |
// updatePID(target, measured, PIDsettings); |
28 |
// measured = rate data from gyros scaled to PWM (1000-2000), since PID settings are found experimentally |
29 |
// updatePID() is defined in PID.h |
30 |
motors.setMotorAxisCommand(ROLL, updatePID(receiver.getData(ROLL), gyro.getFlightData(ROLL) + 1500, &PID[ROLL])); |
31 |
motors.setMotorAxisCommand(PITCH, updatePID(receiver.getData(PITCH), gyro.getFlightData(PITCH) + 1500, &PID[PITCH])); |
32 |
zeroIntegralError(); |
33 |
} |
34 |
|
35 |
if (flightMode == STABLE) { |
36 |
// Stable Mode |
37 |
levelAdjust[ROLL] = (receiver.getAngle(ROLL) - flightAngle.getData(ROLL)) * PID[LEVELROLL].P; |
38 |
levelAdjust[PITCH] = (receiver.getAngle(PITCH) + flightAngle.getData(PITCH)) * PID[LEVELPITCH].P; |
39 |
// Check if pilot commands are not in hover, don't auto trim |
40 |
if ((abs(receiver.getTrimData(ROLL)) > levelOff) || (abs(receiver.getTrimData(PITCH)) > levelOff)) { |
41 |
zeroIntegralError(); |
42 |
#if defined(AeroQuad_v18) || defined(AeroQuadMega_v2) |
43 |
digitalWrite(LED2PIN, LOW); |
44 |
#endif |
45 |
} |
46 |
else { |
47 |
PID[LEVELROLL].integratedError = constrain(PID[LEVELROLL].integratedError + (((receiver.getAngle(ROLL) - flightAngle.getData(ROLL)) * G_Dt) * PID[LEVELROLL].I), -levelLimit, levelLimit); |
48 |
PID[LEVELPITCH].integratedError = constrain(PID[LEVELPITCH].integratedError + (((receiver.getAngle(PITCH) + flightAngle.getData(PITCH)) * G_Dt) * PID[LEVELROLL].I), -levelLimit, levelLimit); |
49 |
#if defined(AeroQuad_v18) || defined(AeroQuadMega_v2) |
50 |
digitalWrite(LED2PIN, HIGH); |
51 |
#endif |
52 |
} |
53 |
motors.setMotorAxisCommand(ROLL, updatePID(receiver.getData(ROLL) + levelAdjust[ROLL], gyro.getFlightData(ROLL) + 1500, &PID[LEVELGYROROLL]) + PID[LEVELROLL].integratedError); |
54 |
motors.setMotorAxisCommand(PITCH, updatePID(receiver.getData(PITCH) + levelAdjust[PITCH], gyro.getFlightData(PITCH) + 1500, &PID[LEVELGYROPITCH]) + PID[LEVELPITCH].integratedError); |
55 |
} |
56 |
|
57 |
// ***************************** Update Yaw *************************** |
58 |
if (headingHoldConfig == ON) { |
59 |
gyro.calculateHeading(); |
60 |
|
61 |
#ifdef HeadingMagHold |
62 |
heading = compass.getHeading(); |
63 |
#else |
64 |
heading = gyro.getHeading(); |
65 |
#endif |
66 |
|
67 |
// Always center relative heading around absolute heading chosen during yaw command |
68 |
// This assumes that an incorrect yaw can't be forced on the AeroQuad >180 or <-180 degrees |
69 |
// This is done so that AeroQuad does not accidentally hit transition between 0 and 360 or -180 and 180 |
70 |
relativeHeading = heading - setHeading; |
71 |
if (heading <= (setHeading - 180)) relativeHeading += 360; |
72 |
if (heading >= (setHeading + 180)) relativeHeading -= 360; |
73 |
|
74 |
// Apply heading hold only when throttle high enough to start flight |
75 |
if (receiver.getData(THROTTLE) > MINCHECK ) { |
76 |
if ((receiver.getData(YAW) > (MIDCOMMAND + 25)) || (receiver.getData(YAW) < (MIDCOMMAND - 25))) { |
77 |
// If commanding yaw, turn off heading hold and store latest heading |
78 |
setHeading = heading; |
79 |
headingHold = 0; |
80 |
PID[HEADING].integratedError = 0; |
81 |
} |
82 |
else |
83 |
// No new yaw input, calculate current heading vs. desired heading heading hold |
84 |
// Relative heading is always centered around zero |
85 |
headingHold = updatePID(0, relativeHeading, &PID[HEADING]); |
86 |
} |
87 |
else { |
88 |
// minimum throttle not reached, use off settings |
89 |
setHeading = heading; |
90 |
headingHold = 0; |
91 |
PID[HEADING].integratedError = 0; |
92 |
} |
93 |
} |
94 |
commandedYaw = constrain(receiver.getData(YAW) + headingHold, 1000, 2000); |
95 |
motors.setMotorAxisCommand(YAW, updatePID(commandedYaw, gyro.getFlightData(YAW) + 1500, &PID[YAW])); |
96 |
|
97 |
// ****************************** Altitude Adjust ************************* |
98 |
// Thanks to Honk for his work with altitude hold |
99 |
// http://aeroquad.com/showthread.php?792-Problems-with-BMP085-I2C-barometer |
100 |
// Thanks to Sherbakov for his work in Z Axis dampening |
101 |
// http://aeroquad.com/showthread.php?359-Stable-flight-logic...&p=10325&viewfull=1#post10325 |
102 |
#ifdef AltitudeHold |
103 |
if (altitudeHold == ON) { |
104 |
throttleAdjust = updatePID(holdAltitude, altitude.getData(), &PID[ALTITUDE]); |
105 |
zDampening = updatePID(0, accel.getZaxis(), &PID[ZDAMPENING]); // This is stil under development - do not use (set PID=0) |
106 |
throttleAdjust = constrain(throttleAdjust + zDampening, minThrottleAdjust, maxThrottleAdjust); |
107 |
} |
108 |
else |
109 |
throttleAdjust = 0; |
110 |
#endif |
111 |
|
112 |
|
113 |
// ****************************** Sonar Adjust ************************* |
114 |
#ifdef SonarHold |
115 |
if (sonarHold == ON) { |
116 |
sonarThrottleAdjust = updatePID(sonarHoldAltitude, sonar.range, &PID[SONAR]); |
117 |
sonarThrottleAdjust = constrain(sonarThrottleAdjust, sonarMinThrottleAdjust, sonarMaxThrottleAdjust); |
118 |
} |
119 |
else |
120 |
sonarThrottleAdjust = 0; |
121 |
#endif |
122 |
|
123 |
// *********************** Calculate Motor Commands ********************** |
124 |
if (armed && safetyCheck) { |
125 |
#ifdef plusConfig |
126 |
motors.setMotorCommand(FRONT, receiver.getData(THROTTLE) - motors.getMotorAxisCommand(PITCH) - motors.getMotorAxisCommand(YAW) + throttleAdjust + sonarThrottleAdjust); |
127 |
motors.setMotorCommand(REAR, receiver.getData(THROTTLE) + motors.getMotorAxisCommand(PITCH) - motors.getMotorAxisCommand(YAW) + throttleAdjust + sonarThrottleAdjust); |
128 |
motors.setMotorCommand(RIGHT, receiver.getData(THROTTLE) - motors.getMotorAxisCommand(ROLL) + motors.getMotorAxisCommand(YAW) + throttleAdjust + sonarThrottleAdjust); |
129 |
motors.setMotorCommand(LEFT, receiver.getData(THROTTLE) + motors.getMotorAxisCommand(ROLL) + motors.getMotorAxisCommand(YAW) + throttleAdjust + sonarThrottleAdjust); |
130 |
#endif |
131 |
#ifdef XConfig |
132 |
// Front = Front/Right, Back = Left/Rear, Left = Front/Left, Right = Right/Rear |
133 |
motors.setMotorCommand(FRONT, receiver.getData(THROTTLE) - motors.getMotorAxisCommand(PITCH) + motors.getMotorAxisCommand(ROLL) - motors.getMotorAxisCommand(YAW) + throttleAdjust + sonarThrottleAdjust); |
134 |
motors.setMotorCommand(RIGHT, receiver.getData(THROTTLE) - motors.getMotorAxisCommand(PITCH) - motors.getMotorAxisCommand(ROLL) + motors.getMotorAxisCommand(YAW) + throttleAdjust + sonarThrottleAdjust); |
135 |
motors.setMotorCommand(LEFT, receiver.getData(THROTTLE) + motors.getMotorAxisCommand(PITCH) + motors.getMotorAxisCommand(ROLL) + motors.getMotorAxisCommand(YAW) + throttleAdjust + sonarThrottleAdjust); |
136 |
motors.setMotorCommand(REAR, receiver.getData(THROTTLE) + motors.getMotorAxisCommand(PITCH) - motors.getMotorAxisCommand(ROLL) - motors.getMotorAxisCommand(YAW) + throttleAdjust + sonarThrottleAdjust); |
137 |
#endif |
138 |
#ifdef MultipilotI2C |
139 |
// if using Mixertable need only Throttle MotorAxixCommand Roll,Pitch,Yaw Yet set |
140 |
motors.setThrottle(receiver.getData(THROTTLE)); |
141 |
#endif |
142 |
} |
143 |
|
144 |
// Prevents too little power applied to motors during hard manuevers |
145 |
// Also provides even motor power on both sides if limit encountered |
146 |
if ((motors.getMotorCommand(FRONT) <= MINTHROTTLE) || (motors.getMotorCommand(REAR) <= MINTHROTTLE)){ |
147 |
delta = receiver.getData(THROTTLE) - MINTHROTTLE; |
148 |
motors.setMaxCommand(RIGHT, constrain(receiver.getData(THROTTLE) + delta, MINTHROTTLE, MAXCHECK)); |
149 |
motors.setMaxCommand(LEFT, constrain(receiver.getData(THROTTLE) + delta, MINTHROTTLE, MAXCHECK)); |
150 |
} |
151 |
else if ((motors.getMotorCommand(FRONT) >= MAXCOMMAND) || (motors.getMotorCommand(REAR) >= MAXCOMMAND)) { |
152 |
delta = MAXCOMMAND - receiver.getData(THROTTLE); |
153 |
motors.setMinCommand(RIGHT, constrain(receiver.getData(THROTTLE) - delta, MINTHROTTLE, MAXCOMMAND)); |
154 |
motors.setMinCommand(LEFT, constrain(receiver.getData(THROTTLE) - delta, MINTHROTTLE, MAXCOMMAND)); |
155 |
} |
156 |
else { |
157 |
motors.setMaxCommand(RIGHT, MAXCOMMAND); |
158 |
motors.setMaxCommand(LEFT, MAXCOMMAND); |
159 |
motors.setMinCommand(RIGHT, MINTHROTTLE); |
160 |
motors.setMinCommand(LEFT, MINTHROTTLE); |
161 |
} |
162 |
|
163 |
if ((motors.getMotorCommand(LEFT) <= MINTHROTTLE) || (motors.getMotorCommand(RIGHT) <= MINTHROTTLE)){ |
164 |
delta = receiver.getData(THROTTLE) - MINTHROTTLE; |
165 |
motors.setMaxCommand(FRONT, constrain(receiver.getData(THROTTLE) + delta, MINTHROTTLE, MAXCHECK)); |
166 |
motors.setMaxCommand(REAR, constrain(receiver.getData(THROTTLE) + delta, MINTHROTTLE, MAXCHECK)); |
167 |
} |
168 |
else if ((motors.getMotorCommand(LEFT) >= MAXCOMMAND) || (motors.getMotorCommand(RIGHT) >= MAXCOMMAND)) { |
169 |
delta = MAXCOMMAND - receiver.getData(THROTTLE); |
170 |
motors.setMinCommand(FRONT, constrain(receiver.getData(THROTTLE) - delta, MINTHROTTLE, MAXCOMMAND)); |
171 |
motors.setMinCommand(REAR, constrain(receiver.getData(THROTTLE) - delta, MINTHROTTLE, MAXCOMMAND)); |
172 |
} |
173 |
else { |
174 |
motors.setMaxCommand(FRONT, MAXCOMMAND); |
175 |
motors.setMaxCommand(REAR, MAXCOMMAND); |
176 |
motors.setMinCommand(FRONT, MINTHROTTLE); |
177 |
motors.setMinCommand(REAR, MINTHROTTLE); |
178 |
} |
179 |
|
180 |
// Allows quad to do acrobatics by lowering power to opposite motors during hard manuevers |
181 |
if (flightMode == ACRO) { |
182 |
#ifdef plusConfig |
183 |
if (receiver.getRaw(ROLL) < MINCHECK) { |
184 |
motors.setMinCommand(LEFT, minAcro); |
185 |
motors.setMaxCommand(RIGHT, MAXCOMMAND); |
186 |
} |
187 |
else if (receiver.getRaw(ROLL) > MAXCHECK) { |
188 |
motors.setMaxCommand(LEFT, MAXCOMMAND); |
189 |
motors.setMinCommand(RIGHT, minAcro); |
190 |
} |
191 |
else if (receiver.getRaw(PITCH) < MINCHECK) { |
192 |
motors.setMaxCommand(FRONT, MAXCOMMAND); |
193 |
motors.setMinCommand(REAR, minAcro); |
194 |
} |
195 |
else if (receiver.getRaw(PITCH) > MAXCHECK) { |
196 |
motors.setMinCommand(FRONT, minAcro); |
197 |
motors.setMaxCommand(REAR, MAXCOMMAND); |
198 |
} |
199 |
#endif |
200 |
#ifdef XConfig |
201 |
if (receiver.getRaw(ROLL) < MINCHECK) { |
202 |
motors.setMaxCommand(FRONT, minAcro); |
203 |
motors.setMaxCommand(REAR, MAXCOMMAND); |
204 |
motors.setMaxCommand(LEFT, minAcro); |
205 |
motors.setMaxCommand(RIGHT, MAXCOMMAND); |
206 |
} |
207 |
else if (receiver.getRaw(ROLL) > MAXCHECK) { |
208 |
motors.setMaxCommand(FRONT, MAXCOMMAND); |
209 |
motors.setMaxCommand(REAR, minAcro); |
210 |
motors.setMaxCommand(LEFT, MAXCOMMAND); |
211 |
motors.setMaxCommand(RIGHT, minAcro); |
212 |
} |
213 |
else if (receiver.getRaw(PITCH) < MINCHECK) { |
214 |
motors.setMaxCommand(FRONT, MAXCOMMAND); |
215 |
motors.setMaxCommand(REAR, minAcro); |
216 |
motors.setMaxCommand(LEFT, minAcro); |
217 |
motors.setMaxCommand(RIGHT, MAXCOMMAND); |
218 |
} |
219 |
else if (receiver.getRaw(PITCH) > MAXCHECK) { |
220 |
motors.setMaxCommand(FRONT, minAcro); |
221 |
motors.setMaxCommand(REAR, MAXCOMMAND); |
222 |
motors.setMaxCommand(LEFT, MAXCOMMAND); |
223 |
motors.setMaxCommand(RIGHT, minAcro); |
224 |
} |
225 |
#endif |
226 |
} |
227 |
|
228 |
// Apply limits to motor commands |
229 |
for (motor = FRONT; motor < LASTMOTOR; motor++) |
230 |
motors.setMotorCommand(motor, constrain(motors.getMotorCommand(motor), motors.getMinCommand(motor), motors.getMaxCommand(motor))); |
231 |
|
232 |
// If throttle in minimum position, don't apply yaw |
233 |
if (receiver.getData(THROTTLE) < MINCHECK) { |
234 |
for (motor = FRONT; motor < LASTMOTOR; motor++) |
235 |
motors.setMotorCommand(motor, MINTHROTTLE); |
236 |
} |
237 |
|
238 |
// ESC Calibration |
239 |
if (armed == OFF) { |
240 |
switch (calibrateESC) { // used for calibrating ESC's |
241 |
case 1: |
242 |
for (motor = FRONT; motor < LASTMOTOR; motor++) |
243 |
motors.setMotorCommand(motor, MAXCOMMAND); |
244 |
break; |
245 |
case 3: |
246 |
for (motor = FRONT; motor < LASTMOTOR; motor++) |
247 |
motors.setMotorCommand(motor, constrain(testCommand, 1000, 1200)); |
248 |
break; |
249 |
case 5: |
250 |
for (motor = FRONT; motor < LASTMOTOR; motor++) |
251 |
motors.setMotorCommand(motor, constrain(motors.getRemoteCommand(motor), 1000, 1200)); |
252 |
safetyCheck = ON; |
253 |
break; |
254 |
default: |
255 |
for (motor = FRONT; motor < LASTMOTOR; motor++) |
256 |
motors.setMotorCommand(motor, MINCOMMAND); |
257 |
} |
258 |
// Send calibration commands to motors |
259 |
motors.write(); // Defined in Motors.h |
260 |
} |
261 |
|
262 |
// *********************** Command Motors ********************** |
263 |
if (armed == ON && safetyCheck == ON) |
264 |
motors.write(); // Defined in Motors.h |
265 |
} |