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 | /* |