216,13 → 216,9 |
if(RC_Quality) { |
RC_Quality--; |
if (NewPpmData-- == 0) { |
|
DebugOut.Analog[12] = stickOffsetPitch; |
DebugOut.Analog[13] = stickOffsetRoll; |
|
RC_PRTY[CONTROL_PITCH] = (RCChannel(CH_PITCH) - stickOffsetPitch) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD; |
RC_PRTY[CONTROL_ROLL] = (RCChannel(CH_ROLL) - stickOffsetRoll) * staticParams.StickP + RCDiff(CH_ROLL) * staticParams.StickD; |
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[6] + 120; |
RC_PRTY[CONTROL_ROLL] = (RCChannel(CH_ROLL) - stickOffsetRoll) * staticParams.StickP + RCDiff(CH_ROLL) * staticParams.StickD; |
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 120; |
if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
// exponential stick sensitivity in yawring rate |
290,23 → 286,26 |
} |
|
#define COMMAND_THRESHOLD 85 |
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
|
uint8_t RC_getCommand(void) { |
if(RCChannel(CH_THROTTLE) > COMMAND_THRESHOLD) { |
// throttle is up |
if(RCChannel(CH_YAW) > COMMAND_THRESHOLD) |
if(RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) { |
// vertical is up |
if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
return COMMAND_GYROCAL; |
if(RCChannel(CH_YAW) < -COMMAND_THRESHOLD) |
if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
return COMMAND_ACCCAL; |
return COMMAND_NONE; |
} else if(RCChannel(CH_THROTTLE) < -COMMAND_THRESHOLD) { |
// pitch is down |
if(RCChannel(CH_YAW) > COMMAND_THRESHOLD) |
} else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) { |
// vertical is down |
if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
return COMMAND_STOP; |
if(RCChannel(CH_YAW) < -COMMAND_THRESHOLD) |
if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
return COMMAND_START; |
return COMMAND_NONE; |
} else { |
// pitch is around center |
// vertical is around center |
return COMMAND_NONE; |
} |
} |
324,27 → 323,31 |
* |
* Not in any of these positions: 0 |
*/ |
|
#define ARGUMENT_THRESHOLD 70 |
#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH |
#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL |
|
uint8_t RC_getArgument(void) { |
if(RCChannel(CH_PITCH) > ARGUMENT_THRESHOLD) { |
// pitch is up |
if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD) |
if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) { |
// vertical is up |
if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 2; |
if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD) |
if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 4; |
return 3; |
} else if(RCChannel(CH_PITCH) < -ARGUMENT_THRESHOLD) { |
// pitch is down |
if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD) |
} else if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) { |
// vertical is down |
if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 8; |
if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD) |
if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 6; |
return 7; |
} else { |
// pitch is around center |
if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD) |
// vertical is around center |
if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 1; |
if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD) |
if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 5; |
return 0; |
} |