| 184,17 → 184,17 |
|---|
| int16_t tmp1, tmp2; |
| if (RCQuality) { |
| RCQuality--; |
| PRTY[CONTROL_PITCH] += RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD; |
| PRTY[CONTROL_ROLL] += RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD; |
| PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD; |
| PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD; |
| int16_t throttle = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120; |
| // Negative throttle values are taken as zero. |
| if (throttle > 0) |
| PRTY[CONTROL_THROTTLE] += throttle; |
| PRTY[CONTROL_THROTTLE] = throttle; |
| tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
| // exponential stick sensitivity in yawing rate |
| tmp2 = (int32_t)staticParams.stickYawP * ((int32_t)tmp1 * abs(tmp1)) >> 9; // expo y = ax + bx^2 |
| tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
| PRTY[CONTROL_YAW] += tmp2; |
| PRTY[CONTROL_YAW] = tmp2; |
| |
| uint8_t command = RC_getStickCommand(); |
| if (lastRCCommand == command) { |