diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index a03f7f94b..ad51b7ea3 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -391,16 +391,25 @@ void configureCrsfTelemetryPort(void) bool checkCrsfTelemetryState(void) { - const bool newTelemetryEnabled = telemetryDetermineEnabledState(portSharing); - if (newTelemetryEnabled == crsfTelemetryEnabled) { + if (serialPortConfig && telemetryCheckRxPortShared(serialPortConfig)) { + if (!crsfTelemetryEnabled && telemetrySharedPort != NULL) { + serialPort = telemetrySharedPort; + crsfTelemetryEnabled = true; + return true; + } return false; - } - if (newTelemetryEnabled) { - configureCrsfTelemetryPort(); } else { - freeCrsfTelemetryPort(); + const bool newTelemetryEnabled = telemetryDetermineEnabledState(portSharing); + if (newTelemetryEnabled == crsfTelemetryEnabled) { + return false; + } + if (newTelemetryEnabled) { + configureCrsfTelemetryPort(); + } else { + freeCrsfTelemetryPort(); + } + return true; } - return true; } int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)