Rev 1874 | Rev 1955 | Go to most recent revision | Show entire file | Regard 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++) { |
438 | for (i = 0; i < MAX_MOTORS; i++) { |
| - | 439 | int32_t tmp; |
|
| 439 | int32_t tmp; |
440 | uint8_t throttle; |
| 440 | if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
441 | |
| 441 | tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]; |
442 | 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[PITCH] * Mixer.Motor[i][MIX_PITCH]; |
| 443 | tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]; |
444 | tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]; |
| Line 453... | Line 454... | ||
| 453 | // including mix components of pitch, roll and yaw? I think only the common |
454 | // including mix components of pitch, roll and yaw? I think only the common |
| 454 | // throttle should be limited. |
455 | // throttle should be limited. |
| 455 | // --> WRONG. This caused motors to stall completely in tight maneuvers. |
456 | // --> WRONG. This caused motors to stall completely in tight maneuvers. |
| 456 | // Apply to individual signals instead. |
457 | // Apply to individual signals instead. |
| 457 | CHECK_MIN_MAX(tmp, 1, 255); |
458 | CHECK_MIN_MAX(tmp, 1, 255); |
| - | 459 | throttle = tmp; |
|
| - | 460 | ||
| - | 461 | if (i < 4) DebugOut.Analog[22 + i] = throttle; |
|
| - | 462 | ||
| - | 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; |
468 | motor[i].SetPoint = 0; |
| 463 | } |
469 | } |
| 464 | if (i < 4) |
- | |
| 465 | DebugOut.Analog[22 + i] = motor[i].SetPoint; |
- | |
| 466 | } |
470 | } |
| Line 467... | Line 471... | ||
| 467 | 471 | ||
| Line 468... | Line 472... | ||
| 468 | I2C_Start(TWI_STATE_MOTOR_TX); |
472 | I2C_Start(TWI_STATE_MOTOR_TX); |