Rev 2052 | Rev 2055 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2052 | Rev 2053 | ||
|---|---|---|---|
| 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 | PRTY[CONTROL_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. |
|
| - | 191 | if (throttle > 0) |
|
| 190 | if (PRTY[CONTROL_THROTTLE] < 0) PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
192 | PRTY[CONTROL_THROTTLE] += throttle; |
| 191 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
193 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
| 192 | // exponential stick sensitivity in yawing rate |
194 | // exponential stick sensitivity in yawing rate |
| 193 | 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 |
| 194 | tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
196 | tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
| 195 | PRTY[CONTROL_YAW] = tmp2; |
197 | PRTY[CONTROL_YAW] += tmp2; |
| Line 196... | Line 198... | ||
| 196 | 198 | ||
| 197 | uint8_t command = RC_getStickCommand(); |
199 | uint8_t command = RC_getStickCommand(); |
| 198 | if (lastRCCommand == command) { |
200 | if (lastRCCommand == command) { |
| 199 | // Keep timer from overrunning. |
201 | // Keep timer from overrunning. |
| Line 202... | Line 204... | ||
| 202 | } else { |
204 | } else { |
| 203 | // There was a change. |
205 | // There was a change. |
| 204 | lastRCCommand = command; |
206 | lastRCCommand = command; |
| 205 | commandTimer = 0; |
207 | commandTimer = 0; |
| 206 | } |
208 | } |
| 207 | } |
- | |
| - | 209 | } // if RCQuality is no good, we just do nothing. |
|
| 208 | debugOut.analog[18] = RCQuality; |
210 | debugOut.analog[18] = RCQuality; |
| 209 | } |
211 | } |
| Line 210... | Line 212... | ||
| 210 | 212 | ||
| 211 | /* |
213 | /* |