| 168,46 → 168,45 |
|---|
| // synchronize channel index |
| index = 1; |
| } else { // within the PPM frame |
| if(index < MAX_CHANNELS-1) // PPM24 supports 12 channels |
| { |
| // check for valid signal length (0.8 ms < signal < 2.1984 ms) |
| // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
| if((signal > 250) && (signal < 687)) { |
| // shift signal to zero symmetric range -154 to 159 |
| signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
| // check for stable signal |
| if(abs(signal - PPM_in[index]) < 6) { |
| if(RC_Quality < 200) RC_Quality +=10; |
| else RC_Quality = 200; |
| } |
| // If signal is the same as before +/- 1, just keep it there. |
| if (signal>=PPM_in[index]-1 && signal<=PPM_in[index]+1) { |
| // In addition, if the signal is very close to 0, just set it to 0. |
| if (signal >=-1 && signal <= 1) { |
| tmp = 0; |
| } else { |
| tmp = PPM_in[index]; |
| } |
| } |
| else |
| tmp = signal; |
| // calculate signal difference on good signal level |
| if(RC_Quality >= 195) |
| PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
| else PPM_diff[index] = 0; |
| PPM_in[index] = tmp; // update channel value |
| if(index < MAX_CHANNELS-1) { // PPM24 supports 12 channels |
| // check for valid signal length (0.8 ms < signal < 2.1984 ms) |
| // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
| if((signal > 250) && (signal < 687)) { |
| // shift signal to zero symmetric range -154 to 159 |
| signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
| // check for stable signal |
| if(abs(signal - PPM_in[index]) < 6) { |
| if(RC_Quality < 200) RC_Quality +=10; |
| else RC_Quality = 200; |
| } |
| // If signal is the same as before +/- 1, just keep it there. |
| if (signal>=PPM_in[index]-1 && signal<=PPM_in[index]+1) { |
| // In addition, if the signal is very close to 0, just set it to 0. |
| if (signal >=-1 && signal <= 1) { |
| tmp = 0; |
| } else { |
| tmp = PPM_in[index]; |
| } |
| index++; // next channel |
| // demux sum signal for channels 5 to 7 to J3, J4, J5 |
| // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the |
| // channels are usually available at the receiver anyway. |
| // if(index == 5) J3HIGH; else J3LOW; |
| // if(index == 6) J4HIGH; else J4LOW; |
| // if(CPUType != ATMEGA644P) // not used as TXD1 |
| // { |
| // if(index == 7) J5HIGH; else J5LOW; |
| // } |
| } |
| else |
| tmp = signal; |
| // calculate signal difference on good signal level |
| if(RC_Quality >= 195) |
| PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
| else PPM_diff[index] = 0; |
| PPM_in[index] = tmp; // update channel value |
| } |
| index++; // next channel |
| // demux sum signal for channels 5 to 7 to J3, J4, J5 |
| // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the |
| // channels are usually available at the receiver anyway. |
| // if(index == 5) J3HIGH; else J3LOW; |
| // if(index == 6) J4HIGH; else J4LOW; |
| // if(CPUType != ATMEGA644P) // not used as TXD1 |
| // { |
| // if(index == 7) J5HIGH; else J5LOW; |
| // } |
| } |
| } |
| } |
| |
| 246,9 → 245,9 |
|---|
| if(RC_Quality) { |
| RC_Quality--; |
| if (NewPpmData-- == 0) { |
| RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP - /* stickOffsetPitch */ + RCDiff(CH_PITCH) * staticParams.StickD; |
| RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP - /* stickOffsetRoll */ + RCDiff(CH_ROLL) * staticParams.StickD; |
| RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 120; |
| 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; |
| 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 |