Rev 1841 | Rev 1845 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1841 | Rev 1844 | ||
|---|---|---|---|
| Line 401... | Line 401... | ||
| 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] - control[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 | ||
| Line 406... | Line 406... | ||
| 406 | tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64; |
406 | tmp_int = (int32_t)((int32_t) dynamicParams.DynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
| 407 | 407 | ||
| 408 | // TODO: From which planet comes the 16000? |
408 | // TODO: From which planet comes the 16000? |
| 409 | CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
409 | CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
| Line 436... | Line 436... | ||
| 436 | DebugOut.Analog[15] = throttleTerm; |
436 | DebugOut.Analog[15] = throttleTerm; |
| Line 437... | Line 437... | ||
| 437 | 437 | ||
| 438 | for(i = 0; i < MAX_MOTORS; i++) { |
438 | for(i = 0; i < MAX_MOTORS; i++) { |
| 439 | int16_t tmp; |
439 | int16_t tmp; |
| 440 | if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
440 | if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
| 441 | tmp = ((int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L; |
441 | tmp = ((int32_t) throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L; |
| 442 | tmp += ((int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]) / 64L; |
442 | tmp += ((int32_t) term[PITCH] * Mixer.Motor[i][MIX_PITCH]) / 64L; |
| 443 | tmp += ((int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]) / 64L; |
443 | tmp += ((int32_t) term[ROLL] * Mixer.Motor[i][MIX_ROLL]) / 64L; |
| 444 | tmp += ((int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]) / 64L; |
444 | tmp += ((int32_t) yawTerm * Mixer.Motor[i][MIX_YAW]) / 64L; |
| 445 | motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
445 | motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
| 446 | // Now we scale back down to a 0..255 range. |
446 | // Now we scale back down to a 0..255 range. |
| Line 447... | Line 447... | ||
| 447 | tmp = motorFilters[i] / CONTROL_SCALING; |
447 | tmp = motorFilters[i] / MOTOR_SCALING; |
| 448 | 448 | ||
| 449 | // So this was the THIRD time a throttle was limited. But should the limitation |
449 | // So this was the THIRD time a throttle was limited. But should the limitation |
| 450 | // apply to the common throttle signal (the one used for setting the "power" of |
450 | // apply to the common throttle signal (the one used for setting the "power" of |
| 451 | // all motors together) or should it limit the throttle set for each motor, |
451 | // all motors together) or should it limit the throttle set for each motor, |
| 452 | // including mix components of pitch, roll and yaw? I think only the common |
452 | // including mix components of pitch, roll and yaw? I think only the common |
| 453 | // throttle should be limited. |
453 | // throttle should be limited. |
| 454 | // --> WRONG. This caused motors to stall completely in tight maneuvers. |
- | |
| 455 | // Apply to individual signals instead. |
454 | // --> WRONG. This caused motors to stall completely in tight maneuvers. |
| 456 | // CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle); |
455 | // Apply to individual signals instead. |
| 457 | CHECK_MIN_MAX(tmp, 8, 255); |
456 | CHECK_MIN_MAX(tmp, 8, 255); |
| 458 | motor[i].SetPoint = tmp; |
457 | motor[i].SetPoint = tmp; |
| 459 | } |
458 | } |