diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index fca34acd4c..49909d0032 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -267,13 +267,6 @@ static void taskCameraControl(uint32_t currentTime) } #endif -#ifdef USE_CRSF_V3 -static void taskSpeedNegotiation(uint32_t currentTime) -{ - speedNegotiationProcess(currentTime); -} -#endif - void tasksInit(void) { schedulerInit(); @@ -419,7 +412,8 @@ void tasksInit(void) #endif #ifdef USE_CRSF_V3 - setTaskEnabled(TASK_SPEED_NEGOTIATION, true); + const bool useCRSF = rxRuntimeState.serialrxProvider == SERIALRX_CRSF; + setTaskEnabled(TASK_SPEED_NEGOTIATION, useCRSF); #endif } @@ -541,7 +535,7 @@ task_t tasks[TASK_COUNT] = { #endif #ifdef USE_CRSF_V3 - [TASK_SPEED_NEGOTIATION] = DEFINE_TASK("SPEED_NEGOTIATION", NULL, NULL, taskSpeedNegotiation, TASK_PERIOD_HZ(100), TASK_PRIORITY_IDLE), + [TASK_SPEED_NEGOTIATION] = DEFINE_TASK("SPEED_NEGOTIATION", NULL, NULL, speedNegotiationProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_IDLE), #endif }; diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 78724f07ef..59714e32ce 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -104,7 +104,7 @@ bool checkCrsfCustomizedSpeed(void) return crsfSpeed.index < BAUD_COUNT ? true : false; } -uint32_t getCrsfDesireSpeed(void) +uint32_t getCrsfDesiredSpeed(void) { return checkCrsfCustomizedSpeed() ? baudRates[crsfSpeed.index] : CRSF_BAUDRATE; } @@ -116,7 +116,7 @@ void setCrsfDefaultSpeed(void) crsfSpeed.confirmationTime = 0; crsfSpeed.index = BAUD_COUNT; isCrsfV3Running = false; - crsfRxUpdateBaudrate(getCrsfDesireSpeed()); + crsfRxUpdateBaudrate(getCrsfDesiredSpeed()); } #endif @@ -399,36 +399,36 @@ void crsfScheduleSpeedNegotiationResponse(void) void speedNegotiationProcess(uint32_t currentTime) { - if (crsfSpeed.hasPendingReply) { - bool found = crsfSpeed.index < BAUD_COUNT ? true : false; - sbuf_t crsfSpeedNegotiationBuf; - sbuf_t *dst = &crsfSpeedNegotiationBuf; - crsfInitializeFrame(dst); - crsfFrameSpeedNegotiationResponse(dst, found); - crsfFinalize(dst); - crsfRxSendTelemetryData(); - crsfSpeed.hasPendingReply = false; - crsfSpeed.isNewSpeedValid = true; - crsfSpeed.confirmationTime = currentTime; - return; - } else if (crsfSpeed.isNewSpeedValid) { - if (currentTime - crsfSpeed.confirmationTime >= 4000) { - // delay 4ms before applying the new baudrate - crsfRxUpdateBaudrate(getCrsfDesireSpeed()); - crsfSpeed.isNewSpeedValid = false; - isCrsfV3Running = true; - return; - } - } - - // to notify the RX to fall back by sedning device info frame if telemetry is disabled - if (!featureIsEnabled(FEATURE_TELEMETRY) && getCrsfDesireSpeed() == CRSF_BAUDRATE) { + if (!featureIsEnabled(FEATURE_TELEMETRY) && getCrsfDesiredSpeed() == CRSF_BAUDRATE) { + // to notify the RX to fall back to default baud rate by sending device info frame if telemetry is disabled sbuf_t crsfPayloadBuf; sbuf_t *dst = &crsfPayloadBuf; crsfInitializeFrame(dst); crsfFrameDeviceInfo(dst); crsfFinalize(dst); crsfRxSendTelemetryData(); + } else { + if (crsfSpeed.hasPendingReply) { + bool found = crsfSpeed.index < BAUD_COUNT ? true : false; + sbuf_t crsfSpeedNegotiationBuf; + sbuf_t *dst = &crsfSpeedNegotiationBuf; + crsfInitializeFrame(dst); + crsfFrameSpeedNegotiationResponse(dst, found); + crsfFinalize(dst); + crsfRxSendTelemetryData(); + crsfSpeed.hasPendingReply = false; + crsfSpeed.isNewSpeedValid = true; + crsfSpeed.confirmationTime = currentTime; + return; + } else if (crsfSpeed.isNewSpeedValid) { + if (currentTime - crsfSpeed.confirmationTime >= 4000) { + // delay 4ms before applying the new baudrate + crsfRxUpdateBaudrate(getCrsfDesiredSpeed()); + crsfSpeed.isNewSpeedValid = false; + isCrsfV3Running = true; + return; + } + } } } #endif diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 519ed8c082..00cd6f893c 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -31,7 +31,7 @@ #define CRSF_MSP_TX_BUF_SIZE 128 void initCrsfTelemetry(void); -uint32_t getCrsfDesireSpeed(void); +uint32_t getCrsfDesiredSpeed(void); void setCrsfDefaultSpeed(void); bool checkCrsfTelemetryState(void); void handleCrsfTelemetry(timeUs_t currentTimeUs);