diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index d4304b674..b30bf9429 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -518,6 +518,7 @@ bool processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; static bool airmodeIsActivated; + static bool sharedPortTelemetryEnabled = false; if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) { return false; @@ -775,14 +776,18 @@ bool processRx(timeUs_t currentTimeUs) #ifdef USE_TELEMETRY if (feature(FEATURE_TELEMETRY)) { - if ((!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || - (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) { - + bool enableSharedPortTelemetry = (!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY)); + if (enableSharedPortTelemetry && !sharedPortTelemetryEnabled) { mspSerialReleaseSharedTelemetryPorts(); - } else { + telemetryCheckState(); + + sharedPortTelemetryEnabled = true; + } else if (!enableSharedPortTelemetry && sharedPortTelemetryEnabled) { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspSerialAllocatePorts(); + + sharedPortTelemetryEnabled = false; } } #endif diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 0419fe355..cfd6b5803 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -202,8 +202,6 @@ static void taskCalculateAltitude(timeUs_t currentTimeUs) #ifdef USE_TELEMETRY static void taskTelemetry(timeUs_t currentTimeUs) { - telemetryCheckState(); - if (!cliMode && feature(FEATURE_TELEMETRY)) { telemetryProcess(currentTimeUs); }