Rev 2052 | Rev 2057 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2052 | Rev 2053 | ||
---|---|---|---|
Line 76... | Line 76... | ||
76 | */ |
76 | */ |
77 | // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
77 | // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
Line 78... | Line 78... | ||
78 | 78 | ||
79 | uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
79 | uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
80 | uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
- | |
81 | - | ||
82 | // Some integral weight constant... |
80 | uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
Line 83... | Line 81... | ||
83 | uint16_t Ki = 10300 / 33; |
81 | uint8_t invKi = 64; |
84 | 82 | ||
85 | /************************************************************************/ |
83 | /************************************************************************/ |
86 | /* Filter for motor value smoothing (necessary???) */ |
84 | /* Filter for motor value smoothing (necessary???) */ |
Line 118... | Line 116... | ||
118 | dynamicParams.KalmanMaxFusion = 32; |
116 | dynamicParams.KalmanMaxFusion = 32; |
119 | */ |
117 | */ |
120 | controlMixer_initVariables(); |
118 | controlMixer_initVariables(); |
121 | } |
119 | } |
Line 122... | Line 120... | ||
122 | 120 | ||
123 | void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, |
121 | void setFlightParameters(uint8_t _invKi, uint8_t _gyroPFactor, |
124 | uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
122 | uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
125 | Ki = 10300 / _Ki; |
123 | invKi = _invKi; |
126 | gyroPFactor = _gyroPFactor; |
124 | gyroPFactor = _gyroPFactor; |
127 | gyroIFactor = _gyroIFactor; |
125 | gyroIFactor = _gyroIFactor; |
128 | yawPFactor = _yawPFactor; |
126 | yawPFactor = _yawPFactor; |
129 | yawIFactor = _yawIFactor; |
127 | yawIFactor = _yawIFactor; |
Line 138... | Line 136... | ||
138 | staticParams.yawIFactor |
136 | staticParams.yawIFactor |
139 | ); |
137 | ); |
140 | } |
138 | } |
Line 141... | Line 139... | ||
141 | 139 | ||
142 | void setStableFlightParameters(void) { |
140 | void setStableFlightParameters(void) { |
143 | setFlightParameters(33, 90, 120, 90, 120); |
141 | setFlightParameters(0, 90, 120, 90, 120); |
Line 144... | Line 142... | ||
144 | } |
142 | } |
145 | 143 | ||
146 | /************************************************************************/ |
144 | /************************************************************************/ |
147 | /* Main Flight Control */ |
145 | /* Main Flight Control */ |
148 | /************************************************************************/ |
146 | /************************************************************************/ |
149 | void flight_control(void) { |
147 | void flight_control(void) { |
150 | int16_t tmp_int; |
148 | uint16_t tmp_int; |
Line 151... | Line 149... | ||
151 | // Mixer Fractions that are combined for Motor Control |
149 | // Mixer Fractions that are combined for Motor Control |
152 | int16_t yawTerm, throttleTerm, term[2]; |
150 | int16_t yawTerm, throttleTerm, term[2]; |
153 | 151 | ||
154 | // PID controller variables |
152 | // PID controller variables |
155 | int16_t PDPart[2],/* DPart[2],*/ PDPartYaw /*, DPartYaw */; |
153 | int16_t PDPart; |
Line 156... | Line 154... | ||
156 | static int32_t IPart[2] = {0, 0}; |
154 | static int32_t IPart[2] = {0, 0}; |
Line 214... | Line 212... | ||
214 | /* |
212 | /* |
215 | * When standing on the ground, do not apply I controls and zero the yaw stick. |
213 | * When standing on the ground, do not apply I controls and zero the yaw stick. |
216 | * Probably to avoid integration effects that will cause the copter to spin |
214 | * Probably to avoid integration effects that will cause the copter to spin |
217 | * or flip when taking off. |
215 | * or flip when taking off. |
218 | */ |
216 | */ |
219 | if (isFlying < 256) { |
217 | if (isFlying < 256) { |
220 | IPart[PITCH] = IPart[ROLL] = 0; |
218 | IPart[PITCH] = IPart[ROLL] = 0; |
221 | PDPartYaw = 0; |
- | |
222 | if (isFlying == 250) { |
219 | if (isFlying == 250) { |
223 | HC_setGround(); |
220 | HC_setGround(); |
224 | #ifdef USE_MK3MAG |
221 | #ifdef USE_MK3MAG |
225 | attitude_resetHeadingToMagnetic(); |
222 | attitude_resetHeadingToMagnetic(); |
226 | compass_setTakeoffHeading(heading); |
223 | compass_setTakeoffHeading(heading); |
227 | #endif |
224 | #endif |
228 | // Set target heading to the one just gotten off compass. |
225 | // Set target heading to the one just gotten off compass. |
229 | // targetHeading = heading; |
226 | // targetHeading = heading; |
230 | } |
227 | } |
231 | } else { |
228 | } else { |
232 | // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
229 | // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
233 | // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
230 | // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
234 | MKFlags |= (MKFLAG_FLY); |
231 | MKFlags |= (MKFLAG_FLY); |
235 | } |
232 | } |
Line 236... | Line 233... | ||
236 | 233 | ||
237 | commands_handleCommands(); |
234 | commands_handleCommands(); |
238 | setNormalFlightParameters(); |
235 | setNormalFlightParameters(); |
Line 239... | Line -... | ||
239 | } // end else (not bad signal case) |
- | |
240 | - | ||
241 | #if defined (USE_NAVICTRL) |
- | |
242 | /************************************************************************/ |
- | |
243 | /* GPS is currently not supported. */ |
- | |
244 | /************************************************************************/ |
- | |
245 | if(staticParams.GlobalConfig & CFG_GPS_ENABLED) { |
- | |
246 | GPS_Main(); |
- | |
247 | MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
- | |
248 | } else { |
- | |
249 | } |
236 | } // end else (not bad signal case) |
250 | #endif |
237 | |
251 | // end part 1: 750-800 usec. |
- | |
252 | // start part 3: 350 - 400 usec. |
- | |
253 | /************************************************************************/ |
- | |
254 | - | ||
255 | /* Calculate control feedback from angle (gyro integral) */ |
- | |
256 | /* and angular velocity (gyro signal) */ |
- | |
257 | /************************************************************************/ |
- | |
258 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
- | |
259 | for (axis = PITCH; axis <= ROLL; axis++) { |
- | |
260 | PDPart[axis] = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); // P-Part - Proportional to Integral |
- | |
261 | PDPart[axis] += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 5); |
- | |
262 | PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
- | |
263 | - | ||
264 | //CHECK_MIN_MAX(PDPart[axis], -6L*GYRO_DEG_FACTOR_PITCHROLL, 6L*GYRO_DEG_FACTOR_PITCHROLL); |
- | |
265 | if (PDPart[axis] < -6L*GYRO_DEG_FACTOR_PITCHROLL) { |
- | |
266 | PDPart[axis] =- 6L*GYRO_DEG_FACTOR_PITCHROLL; |
- | |
267 | debugOut.digital[0] |= DEBUG_FLIGHTCLIP; |
- | |
268 | } else if (PDPart[axis] > 6L*GYRO_DEG_FACTOR_PITCHROLL) { |
- | |
269 | PDPart[axis] = 6L*GYRO_DEG_FACTOR_PITCHROLL; |
- | |
270 | debugOut.digital[0] |= DEBUG_FLIGHTCLIP; |
- | |
271 | } |
- | |
272 | } |
238 | // end part 1: 750-800 usec. |
273 | 239 | // start part 3: 350 - 400 usec. |
|
274 | #define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW) |
240 | #define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW) |
275 | // This is where control affects the target heading. It also (later) directly controls yaw. |
241 | // This is where control affects the target heading. It also (later) directly controls yaw. |
276 | headingError -= controls[CONTROL_YAW]; |
242 | headingError -= controls[CONTROL_YAW]; |
277 | debugOut.analog[28] = headingError / 100; |
243 | debugOut.analog[28] = headingError / 100; |
Line 278... | Line 244... | ||
278 | if (headingError < -YAW_I_LIMIT) headingError = -YAW_I_LIMIT; |
244 | if (headingError < -YAW_I_LIMIT) headingError = -YAW_I_LIMIT; |
279 | if (headingError > YAW_I_LIMIT) headingError = YAW_I_LIMIT; |
245 | if (headingError > YAW_I_LIMIT) headingError = YAW_I_LIMIT; |
280 | 246 | ||
281 | PDPartYaw = (int32_t)(headingError * yawIFactor) / (GYRO_DEG_FACTOR_PITCHROLL << 3); |
- | |
282 | // Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on. |
- | |
283 | PDPartYaw += (int32_t)(yawRate * yawPFactor) / (GYRO_DEG_FACTOR_PITCHROLL >> 6); |
- | |
Line 284... | Line 247... | ||
284 | 247 | PDPart = (int32_t)(headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4); |
|
285 | // limit control feedback |
248 | // Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on. |
286 | // CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
249 | PDPart += (int32_t)(yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5); |
287 | 250 | ||
Line 305... | Line 268... | ||
305 | * However, at low throttle the yaw term is limited to a fixed value, |
268 | * However, at low throttle the yaw term is limited to a fixed value, |
306 | * and at high throttle it is limited by the throttle reserve (the difference |
269 | * and at high throttle it is limited by the throttle reserve (the difference |
307 | * between current throttle and maximum throttle). |
270 | * between current throttle and maximum throttle). |
308 | */ |
271 | */ |
309 | #define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
272 | #define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
310 | yawTerm = PDPartYaw - controls[CONTROL_YAW] * CONTROL_SCALING; |
273 | yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING; |
311 | // Limit yawTerm |
274 | // Limit yawTerm |
312 | debugOut.digital[0] &= ~DEBUG_CLIP; |
275 | debugOut.digital[0] &= ~DEBUG_CLIP; |
313 | if (throttleTerm > MIN_YAWGAS) { |
276 | if (throttleTerm > MIN_YAWGAS) { |
314 | if (yawTerm < -throttleTerm / 2) { |
277 | if (yawTerm < -throttleTerm / 2) { |
315 | debugOut.digital[0] |= DEBUG_CLIP; |
278 | debugOut.digital[0] |= DEBUG_CLIP; |
Line 335... | Line 298... | ||
335 | } else if (yawTerm > (tmp_int - throttleTerm)) { |
298 | } else if (yawTerm > (tmp_int - throttleTerm)) { |
336 | yawTerm = (tmp_int - throttleTerm); |
299 | yawTerm = (tmp_int - throttleTerm); |
337 | debugOut.digital[0] |= DEBUG_CLIP; |
300 | debugOut.digital[0] |= DEBUG_CLIP; |
338 | } |
301 | } |
Line 339... | Line -... | ||
339 | - | ||
340 | // CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
302 | |
- | 303 | debugOut.digital[1] &= ~DEBUG_CLIP; |
|
- | 304 | ||
- | 305 | tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + abs(yawTerm) / 2)) >> 6; |
|
- | 306 | ||
- | 307 | /************************************************************************/ |
|
- | 308 | /* Calculate control feedback from angle (gyro integral) */ |
|
- | 309 | /* and angular velocity (gyro signal) */ |
|
- | 310 | /************************************************************************/ |
|
341 | debugOut.digital[1] &= ~DEBUG_CLIP; |
311 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
342 | for (axis = PITCH; axis <= ROLL; axis++) { |
312 | for (axis = PITCH; axis <= ROLL; axis++) { |
- | 313 | int16_t iDiff; |
|
- | 314 | iDiff = PDPart = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 3); |
|
- | 315 | PDPart += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4); |
|
343 | /* |
316 | PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
344 | * Compose pitch and roll terms. This is finally where the sticks come into play. |
- | |
- | 317 | // In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick. |
|
345 | */ |
318 | // In HH mode, the I part is summed from P and D of gyros minus stick. |
346 | if (gyroIFactor) { |
- | |
347 | // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
- | |
348 | // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
- | |
349 | // TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
319 | if (gyroIFactor) { |
350 | IPart[axis] += PDPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos. |
320 | IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
351 | } else { |
- | |
352 | // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
- | |
353 | // To keep up with a full stick PDPart should be about 156... |
321 | } else { |
354 | IPart[axis] += PDPart[axis] - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
322 | IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
Line 355... | Line -... | ||
355 | } |
- | |
356 | 323 | } |
|
357 | tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability |
- | |
358 | * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
- | |
359 | 324 | ||
360 | //CHECK_MIN_MAX(IPart[axis], -25L*GYRO_DEG_FACTOR_PITCHROLL, 25L*GYRO_DEG_FACTOR_PITCHROLL); |
325 | // With normal Ki, limit effect to +/- 205 (of 1024!!!) |
361 | if (IPart[axis] < -25L*GYRO_DEG_FACTOR_PITCHROLL) { |
326 | if (IPart[axis] < -64000) { |
362 | IPart[axis] =- 25L*GYRO_DEG_FACTOR_PITCHROLL; |
327 | IPart[axis] = -64000; |
363 | debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
328 | debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
364 | } else if (PDPart[axis] > 25L*GYRO_DEG_FACTOR_PITCHROLL) { |
329 | } else if (IPart[axis] > 64000) { |
365 | PDPart[axis] = 25L*GYRO_DEG_FACTOR_PITCHROLL; |
330 | IPart[axis] = 64000; |
Line 366... | Line -... | ||
366 | debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
- | |
367 | } |
331 | debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
368 | 332 | } |
|
369 | // Add (P, D) parts minus stick pos. to the scaled-down I part. |
333 | |
370 | term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch |
334 | term[axis] = PDPart - controls[axis] + ((int32_t)IPart[axis] * invKi) >> 14; |
371 | term[axis] += (dynamicParams.levelCorrection[axis] - 128); |
335 | term[axis] += (dynamicParams.levelCorrection[axis] - 128); |
372 | /* |
336 | /* |