Rev 1634 | Rev 1775 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1634 | Rev 1635 | ||
|---|---|---|---|
| Line 214... | Line 214... | ||
| 214 | void RC_update() { |
214 | void RC_update() { |
| 215 | int16_t tmp1, tmp2; |
215 | int16_t tmp1, tmp2; |
| 216 | if(RC_Quality) { |
216 | if(RC_Quality) { |
| 217 | RC_Quality--; |
217 | RC_Quality--; |
| 218 | if (NewPpmData-- == 0) { |
218 | if (NewPpmData-- == 0) { |
| 219 | - | ||
| 220 | DebugOut.Analog[12] = stickOffsetPitch; |
- | |
| 221 | DebugOut.Analog[13] = stickOffsetRoll; |
- | |
| 222 | - | ||
| 223 | RC_PRTY[CONTROL_PITCH] = (RCChannel(CH_PITCH) - stickOffsetPitch) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD; |
219 | RC_PRTY[CONTROL_PITCH] = (RCChannel(CH_PITCH) - stickOffsetPitch) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD; |
| 224 | RC_PRTY[CONTROL_ROLL] = (RCChannel(CH_ROLL) - stickOffsetRoll) * staticParams.StickP + RCDiff(CH_ROLL) * staticParams.StickD; |
220 | RC_PRTY[CONTROL_ROLL] = (RCChannel(CH_ROLL) - stickOffsetRoll) * staticParams.StickP + RCDiff(CH_ROLL) * staticParams.StickD; |
| 225 | RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[6] + 120; |
221 | RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 120; |
| 226 | if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
222 | if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
| 227 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
223 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
| 228 | // exponential stick sensitivity in yawring rate |
224 | // exponential stick sensitivity in yawring rate |
| 229 | tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
225 | tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
| 230 | tmp2 += (staticParams.StickYawP * tmp1) / 4; |
226 | tmp2 += (staticParams.StickYawP * tmp1) / 4; |
| Line 288... | Line 284... | ||
| 288 | stickOffsetRoll = RCChannel(CH_ROLL); |
284 | stickOffsetRoll = RCChannel(CH_ROLL); |
| 289 | } |
285 | } |
| 290 | } |
286 | } |
| Line 291... | Line 287... | ||
| 291 | 287 | ||
| - | 288 | #define COMMAND_THRESHOLD 85 |
|
| - | 289 | #define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
|
| - | 290 | #define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
|
| 292 | #define COMMAND_THRESHOLD 85 |
291 | |
| 293 | uint8_t RC_getCommand(void) { |
292 | uint8_t RC_getCommand(void) { |
| 294 | if(RCChannel(CH_THROTTLE) > COMMAND_THRESHOLD) { |
293 | if(RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) { |
| 295 | // throttle is up |
294 | // vertical is up |
| 296 | if(RCChannel(CH_YAW) > COMMAND_THRESHOLD) |
295 | if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
| 297 | return COMMAND_GYROCAL; |
296 | return COMMAND_GYROCAL; |
| 298 | if(RCChannel(CH_YAW) < -COMMAND_THRESHOLD) |
297 | if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
| 299 | return COMMAND_ACCCAL; |
298 | return COMMAND_ACCCAL; |
| 300 | return COMMAND_NONE; |
299 | return COMMAND_NONE; |
| 301 | } else if(RCChannel(CH_THROTTLE) < -COMMAND_THRESHOLD) { |
300 | } else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) { |
| 302 | // pitch is down |
301 | // vertical is down |
| 303 | if(RCChannel(CH_YAW) > COMMAND_THRESHOLD) |
302 | if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
| 304 | return COMMAND_STOP; |
303 | return COMMAND_STOP; |
| 305 | if(RCChannel(CH_YAW) < -COMMAND_THRESHOLD) |
304 | if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
| 306 | return COMMAND_START; |
305 | return COMMAND_START; |
| 307 | return COMMAND_NONE; |
306 | return COMMAND_NONE; |
| 308 | } else { |
307 | } else { |
| 309 | // pitch is around center |
308 | // vertical is around center |
| 310 | return COMMAND_NONE; |
309 | return COMMAND_NONE; |
| 311 | } |
310 | } |
| Line 312... | Line 311... | ||
| 312 | } |
311 | } |
| Line 322... | Line 321... | ||
| 322 | * + <-- |
321 | * + <-- |
| 323 | * 0 |
322 | * 0 |
| 324 | * |
323 | * |
| 325 | * Not in any of these positions: 0 |
324 | * Not in any of these positions: 0 |
| 326 | */ |
325 | */ |
| - | 326 | ||
| 327 | #define ARGUMENT_THRESHOLD 70 |
327 | #define ARGUMENT_THRESHOLD 70 |
| - | 328 | #define ARGUMENT_CHANNEL_VERTICAL CH_PITCH |
|
| - | 329 | #define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL |
|
| - | 330 | ||
| 328 | uint8_t RC_getArgument(void) { |
331 | uint8_t RC_getArgument(void) { |
| 329 | if(RCChannel(CH_PITCH) > ARGUMENT_THRESHOLD) { |
332 | if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) { |
| 330 | // pitch is up |
333 | // vertical is up |
| 331 | if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD) |
334 | if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
| 332 | return 2; |
335 | return 2; |
| 333 | if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD) |
336 | if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
| 334 | return 4; |
337 | return 4; |
| 335 | return 3; |
338 | return 3; |
| 336 | } else if(RCChannel(CH_PITCH) < -ARGUMENT_THRESHOLD) { |
339 | } else if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) { |
| 337 | // pitch is down |
340 | // vertical is down |
| 338 | if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD) |
341 | if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
| 339 | return 8; |
342 | return 8; |
| 340 | if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD) |
343 | if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
| 341 | return 6; |
344 | return 6; |
| 342 | return 7; |
345 | return 7; |
| 343 | } else { |
346 | } else { |
| 344 | // pitch is around center |
347 | // vertical is around center |
| 345 | if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD) |
348 | if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
| 346 | return 1; |
349 | return 1; |
| 347 | if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD) |
350 | if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
| 348 | return 5; |
351 | return 5; |
| 349 | return 0; |
352 | return 0; |
| 350 | } |
353 | } |
| 351 | } |
354 | } |