diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index d4e249ff70..35169c471d 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -58,7 +58,7 @@ #define CRSF_LINK_STATUS_UPDATE_TIMEOUT_US 250000 // 250ms, 4 Hz mode 1 telemetry -#define CRSF_FRAME_ERROR_COUNT_THRESHOLD 100 +#define CRSF_FRAME_ERROR_COUNT_THRESHOLD 3 STATIC_UNIT_TESTED bool crsfFrameDone = false; STATIC_UNIT_TESTED crsfFrame_t crsfFrame; @@ -362,6 +362,12 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data) if (cmpTimeUs(currentTimeUs, crsfFrameStartAtUs) > CRSF_TIME_NEEDED_PER_FRAME_US) { // We've received a character after max time needed to complete a frame, // so this must be the start of a new frame. +#if defined(USE_CRSF_V3) + if (crsfFramePosition > 0) { + // count an error if full valid frame not received within the allowed time. + crsfFrameErrorCnt++; + } +#endif crsfFramePosition = 0; } @@ -456,14 +462,12 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data) crsfFrameErrorCnt++; #endif } - } else { -#if defined(USE_CRSF_V3) - if (crsfFrameErrorCnt < CRSF_FRAME_ERROR_COUNT_THRESHOLD) - crsfFrameErrorCnt++; -#endif } #if defined(USE_CRSF_V3) - if (crsfFrameErrorCnt >= CRSF_FRAME_ERROR_COUNT_THRESHOLD) { + if (crsfBaudNegotiationInProgress()) { + // don't count errors when negotiation in progress. + crsfFrameErrorCnt = 0; + } else if (crsfFrameErrorCnt >= CRSF_FRAME_ERROR_COUNT_THRESHOLD) { // fall back to default speed if speed mismatch detected setCrsfDefaultSpeed(); crsfFrameErrorCnt = 0; diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 8a75bcfb1c..15ce87fabe 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -119,6 +119,11 @@ void setCrsfDefaultSpeed(void) isCrsfV3Running = false; crsfRxUpdateBaudrate(getCrsfDesiredSpeed()); } + +bool crsfBaudNegotiationInProgress(void) +{ + return crsfSpeed.hasPendingReply || crsfSpeed.isNewSpeedValid; +} #endif void initCrsfMspBuffer(void) diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index c52f958cfd..d33820320e 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -48,4 +48,5 @@ int getCrsfMspFrame(uint8_t *frame, uint8_t *payload, const uint8_t payloadSize) #endif #if defined(USE_CRSF_V3) void speedNegotiationProcess(uint32_t currentTime); +bool crsfBaudNegotiationInProgress(void); #endif