62,7 → 62,7 |
volatile int16_t PPM_in[MAX_CHANNELS]; |
volatile int16_t PPM_diff[MAX_CHANNELS]; |
volatile uint8_t NewPpmData = 1; |
volatile uint8_t RC_Quality = 0; |
volatile uint8_t RCQuality = 0; |
int16_t RC_PRTY[4]; |
uint8_t lastRCCommand = COMMAND_NONE; |
uint8_t commandTimer = 0; |
114,7 → 114,7 |
TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1)); |
TIMSK1 |= (1<<ICIE1); |
|
RC_Quality = 0; |
RCQuality = 0; |
|
SREG = sreg; |
} |
173,10 → 173,10 |
signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms) |
// check for stable signal |
if (abs(signal - PPM_in[index]) < 6) { |
if (RC_Quality < 200) |
RC_Quality += 10; |
if (RCQuality < 200) |
RCQuality += 10; |
else |
RC_Quality = 200; |
RCQuality = 200; |
} |
// If signal is the same as before +/- 1, just keep it there. |
if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) { |
189,7 → 189,7 |
} else |
tmp = signal; |
// calculate signal difference on good signal level |
if (RC_Quality >= 195) |
if (RCQuality >= 195) |
PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
else |
PPM_diff[index] = 0; |
241,8 → 241,8 |
*/ |
void RC_update() { |
int16_t tmp1, tmp2; |
if (RC_Quality) { |
RC_Quality--; |
if (RCQuality) { |
RCQuality--; |
if (NewPpmData-- == 0) { |
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP |
+ RCDiff(CH_PITCH) * staticParams.stickD; |
274,6 → 274,8 |
RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] |
= RC_PRTY[CONTROL_YAW] = 0; |
} |
|
debugOut.analog[21] = RCQuality; |
} |
|
/* |
298,11 → 300,11 |
} |
|
uint8_t RC_getSignalQuality(void) { |
if (RC_Quality >= 160) |
if (RCQuality >= 160) |
return SIGNAL_GOOD; |
if (RC_Quality >= 140) |
if (RCQuality >= 140) |
return SIGNAL_OK; |
if (RC_Quality >= 120) |
if (RCQuality >= 120) |
return SIGNAL_BAD; |
return SIGNAL_LOST; |
} |