| 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; |