184,15 → 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_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120; |
if (PRTY[CONTROL_THROTTLE] < 0) PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
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; |
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) { |
204,7 → 206,7 |
lastRCCommand = command; |
commandTimer = 0; |
} |
} |
} // if RCQuality is no good, we just do nothing. |
debugOut.analog[18] = RCQuality; |
} |
|