214,8 → 214,8 |
} |
} |
|
#define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]] |
#define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]] |
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]] |
#define RCDiff(dimension) PPM_diff[channelMap.channels[dimension]] |
#define COMMAND_THRESHOLD 85 |
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
249,19 → 249,19 |
if (RC_Quality) { |
RC_Quality--; |
if (NewPpmData-- == 0) { |
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP |
+ RCDiff(CH_PITCH) * staticParams.StickD; |
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP |
+ RCDiff(CH_ROLL) * staticParams.StickD; |
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP |
+ RCDiff(CH_PITCH) * staticParams.stickD; |
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP |
+ RCDiff(CH_ROLL) * staticParams.stickD; |
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) |
* dynamicParams.UserParams[3] + 120; |
* staticParams.stickThrottleD + 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 yawing rate |
tmp2 = (int32_t) staticParams.StickYawP * ((int32_t) tmp1 * abs(tmp1)) |
tmp2 = (int32_t) staticParams.stickYawP * ((int32_t) tmp1 * abs(tmp1)) |
/ 512L; // expo y = ax + bx^2 |
tmp2 += (staticParams.StickYawP * tmp1) >> 2; |
tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
RC_PRTY[CONTROL_YAW] = tmp2; |
} |
uint8_t command = RC_getStickCommand(); |
392,6 → 392,7 |
} |
} |
|
/* |
uint8_t RC_getLooping(uint8_t looping) { |
// static uint8_t looping = 0; |
|
429,6 → 430,7 |
|
return looping; |
} |
*/ |
|
uint8_t RC_testCompassCalState(void) { |
static uint8_t stick = 1; |