Rev 1874 | Rev 1955 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1874 | Rev 1908 | ||
---|---|---|---|
Line 170... | Line 170... | ||
170 | 170 | ||
171 | // Fire the main flight attitude calculation, including integration of angles. |
171 | // Fire the main flight attitude calculation, including integration of angles. |
172 | // We want that to kick as early as possible, not to delay new AD sampling further. |
172 | // We want that to kick as early as possible, not to delay new AD sampling further. |
173 | calculateFlightAttitude(); |
173 | calculateFlightAttitude(); |
174 | controlMixer_update(); |
174 | controlMixer_update(); |
Line 175... | Line 175... | ||
175 | throttleTerm = controlThrottle; |
175 | throttleTerm = controls[CONTROL_THROTTLE]; |
176 | 176 | ||
177 | // This check removed. Is done on a per-motor basis, after output matrix multiplication. |
177 | // This check removed. Is done on a per-motor basis, after output matrix multiplication. |
178 | if (throttleTerm < staticParams.MinThrottle + 10) |
178 | if (throttleTerm < staticParams.MinThrottle + 10) |
Line 259... | Line 259... | ||
259 | } |
259 | } |
Line 260... | Line 260... | ||
260 | 260 | ||
261 | /************************************************************************/ |
261 | /************************************************************************/ |
262 | /* Yawing */ |
262 | /* Yawing */ |
263 | /************************************************************************/ |
263 | /************************************************************************/ |
264 | if (abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
264 | if (abs(controls[CONTROL_YAW]) > 4 * staticParams.StickYawP) { // yaw stick is activated |
265 | ignoreCompassTimer = 1000; |
265 | ignoreCompassTimer = 1000; |
266 | if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
266 | if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
267 | updateCompassCourse = 1; |
267 | updateCompassCourse = 1; |
268 | } |
268 | } |
Line 271... | Line 271... | ||
271 | // yawControlRate = controlYaw; |
271 | // yawControlRate = controlYaw; |
Line 272... | Line 272... | ||
272 | 272 | ||
273 | // Trim drift of yawAngleDiff with controlYaw. |
273 | // Trim drift of yawAngleDiff with controlYaw. |
274 | // TODO: We want NO feedback of control related stuff to the attitude related stuff. |
274 | // TODO: We want NO feedback of control related stuff to the attitude related stuff. |
275 | // This seems to be used as: Difference desired <--> real heading. |
275 | // This seems to be used as: Difference desired <--> real heading. |
Line 276... | Line 276... | ||
276 | yawAngleDiff -= controlYaw; |
276 | yawAngleDiff -= controls[CONTROL_YAW]; |
277 | 277 | ||
Line 278... | Line 278... | ||
278 | // limit the effect |
278 | // limit the effect |
Line 351... | Line 351... | ||
351 | * However, at low throttle the yaw term is limited to a fixed value, |
351 | * However, at low throttle the yaw term is limited to a fixed value, |
352 | * and at high throttle it is limited by the throttle reserve (the difference |
352 | * and at high throttle it is limited by the throttle reserve (the difference |
353 | * between current throttle and maximum throttle). |
353 | * between current throttle and maximum throttle). |
354 | */ |
354 | */ |
355 | #define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
355 | #define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
356 | yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING; |
356 | yawTerm = PDPartYaw - controls[CONTROL_YAW] * CONTROL_SCALING; |
357 | // Limit yawTerm |
357 | // Limit yawTerm |
358 | DebugOut.Digital[0] &= ~DEBUG_CLIP; |
358 | DebugOut.Digital[0] &= ~DEBUG_CLIP; |
359 | if (throttleTerm > MIN_YAWGAS) { |
359 | if (throttleTerm > MIN_YAWGAS) { |
360 | if (yawTerm < -throttleTerm / 2) { |
360 | if (yawTerm < -throttleTerm / 2) { |
361 | DebugOut.Digital[0] |= DEBUG_CLIP; |
361 | DebugOut.Digital[0] |= DEBUG_CLIP; |
Line 394... | Line 394... | ||
394 | */ |
394 | */ |
395 | if (gyroIFactor) { |
395 | if (gyroIFactor) { |
396 | // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
396 | // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
397 | // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
397 | // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
398 | // TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
398 | // TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
399 | IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos. |
399 | IPart[axis] += PPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos. |
400 | } else { |
400 | } else { |
401 | // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
401 | // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
402 | // To keep up with a full stick PDPart should be about 156... |
402 | // To keep up with a full stick PDPart should be about 156... |
403 | IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
403 | 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. |
404 | } |
404 | } |
Line 405... | Line 405... | ||
405 | 405 | ||
406 | tmp_int = (int32_t) ((int32_t) dynamicParams.DynamicStability |
406 | tmp_int = (int32_t) ((int32_t) dynamicParams.DynamicStability |
Line 407... | Line 407... | ||
407 | * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
407 | * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
408 | 408 | ||
409 | // TODO: From which planet comes the 16000? |
409 | // TODO: From which planet comes the 16000? |
410 | CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
410 | CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
Line 411... | Line 411... | ||
411 | // Add (P, D) parts minus stick pos. to the scaled-down I part. |
411 | // Add (P, D) parts minus stick pos. to the scaled-down I part. |
412 | term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch |
412 | term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch |
413 | 413 | ||
414 | /* |
414 | /* |
Line 430... | Line 430... | ||
430 | // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
430 | // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
431 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
431 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 432... | Line 432... | ||
432 | 432 | ||
433 | DebugOut.Analog[12] = term[PITCH]; |
433 | DebugOut.Analog[12] = term[PITCH]; |
434 | DebugOut.Analog[13] = term[ROLL]; |
434 | DebugOut.Analog[13] = term[ROLL]; |
435 | DebugOut.Analog[14] = yawTerm; |
435 | //DebugOut.Analog[14] = yawTerm; |
Line 436... | Line 436... | ||
436 | DebugOut.Analog[15] = throttleTerm; |
436 | DebugOut.Analog[15] = throttleTerm; |
437 | 437 | ||
- | 438 | for (i = 0; i < MAX_MOTORS; i++) { |
|
- | 439 | int32_t tmp; |
|
- | 440 | uint8_t throttle; |
|
- | 441 | ||
- | 442 | tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]; |
|
- | 443 | tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]; |
|
- | 444 | tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]; |
|
- | 445 | tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]; |
|
- | 446 | tmp = tmp >> 6; |
|
- | 447 | motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
|
- | 448 | // Now we scale back down to a 0..255 range. |
|
- | 449 | tmp = motorFilters[i] / MOTOR_SCALING; |
|
- | 450 | ||
- | 451 | // So this was the THIRD time a throttle was limited. But should the limitation |
|
- | 452 | // apply to the common throttle signal (the one used for setting the "power" of |
|
- | 453 | // all motors together) or should it limit the throttle set for each motor, |
|
- | 454 | // including mix components of pitch, roll and yaw? I think only the common |
|
- | 455 | // throttle should be limited. |
|
- | 456 | // --> WRONG. This caused motors to stall completely in tight maneuvers. |
|
- | 457 | // Apply to individual signals instead. |
|
- | 458 | CHECK_MIN_MAX(tmp, 1, 255); |
|
- | 459 | throttle = tmp; |
|
- | 460 | ||
438 | for (i = 0; i < MAX_MOTORS; i++) { |
461 | if (i < 4) DebugOut.Analog[22 + i] = throttle; |
439 | int32_t tmp; |
- | |
440 | if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
- | |
441 | tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]; |
- | |
442 | tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]; |
- | |
443 | tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]; |
- | |
444 | tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]; |
- | |
445 | tmp = tmp >> 6; |
- | |
446 | motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
- | |
447 | // Now we scale back down to a 0..255 range. |
- | |
448 | tmp = motorFilters[i] / MOTOR_SCALING; |
- | |
449 | - | ||
450 | // So this was the THIRD time a throttle was limited. But should the limitation |
- | |
451 | // apply to the common throttle signal (the one used for setting the "power" of |
- | |
452 | // all motors together) or should it limit the throttle set for each motor, |
- | |
453 | // including mix components of pitch, roll and yaw? I think only the common |
- | |
454 | // throttle should be limited. |
- | |
455 | // --> WRONG. This caused motors to stall completely in tight maneuvers. |
- | |
456 | // Apply to individual signals instead. |
462 | |
457 | CHECK_MIN_MAX(tmp, 1, 255); |
463 | if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
458 | motor[i].SetPoint = tmp; |
464 | motor[i].SetPoint = throttle; |
459 | } else if (motorTestActive) { |
465 | } else if (motorTestActive) { |
460 | motor[i].SetPoint = motorTest[i]; |
466 | motor[i].SetPoint = motorTest[i]; |
461 | } else { |
467 | } else { |
462 | motor[i].SetPoint = 0; |
- | |
463 | } |
- | |
464 | if (i < 4) |
468 | motor[i].SetPoint = 0; |
Line 465... | Line 469... | ||
465 | DebugOut.Analog[22 + i] = motor[i].SetPoint; |
469 | } |
Line 466... | Line 470... | ||
466 | } |
470 | } |