Rev 2058 | Rev 2073 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2058 | Rev 2071 | ||
|---|---|---|---|
| Line 182... | Line 182... | ||
| 182 | */ |
182 | */ |
| 183 | void RC_periodicTaskAndPRTY(int16_t* PRTY) { |
183 | void RC_periodicTaskAndPRTY(int16_t* PRTY) { |
| 184 | int16_t tmp1, tmp2; |
184 | int16_t tmp1, tmp2; |
| 185 | if (RCQuality) { |
185 | if (RCQuality) { |
| 186 | RCQuality--; |
186 | RCQuality--; |
| 187 | PRTY[CONTROL_PITCH] += RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD; |
187 | PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD; |
| 188 | PRTY[CONTROL_ROLL] += RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD; |
188 | PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD; |
| 189 | int16_t throttle = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120; |
189 | int16_t throttle = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120; |
| 190 | // Negative throttle values are taken as zero. |
190 | // Negative throttle values are taken as zero. |
| 191 | if (throttle > 0) |
191 | if (throttle > 0) |
| 192 | PRTY[CONTROL_THROTTLE] += throttle; |
192 | PRTY[CONTROL_THROTTLE] = throttle; |
| 193 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
193 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
| 194 | // exponential stick sensitivity in yawing rate |
194 | // exponential stick sensitivity in yawing rate |
| 195 | tmp2 = (int32_t)staticParams.stickYawP * ((int32_t)tmp1 * abs(tmp1)) >> 9; // expo y = ax + bx^2 |
195 | tmp2 = (int32_t)staticParams.stickYawP * ((int32_t)tmp1 * abs(tmp1)) >> 9; // expo y = ax + bx^2 |
| 196 | tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
196 | tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
| 197 | PRTY[CONTROL_YAW] += tmp2; |
197 | PRTY[CONTROL_YAW] = tmp2; |
| Line 198... | Line 198... | ||
| 198 | 198 | ||
| 199 | uint8_t command = RC_getStickCommand(); |
199 | uint8_t command = RC_getStickCommand(); |
| 200 | if (lastRCCommand == command) { |
200 | if (lastRCCommand == command) { |
| 201 | // Keep timer from overrunning. |
201 | // Keep timer from overrunning. |