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