Project

General

Profile

Statistics
| Branch: | Revision:

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
}