support baud rate negotiation if telemetry is disabled

src/main/fc/tasks.c

@@ -94,6 +94,7 @@
 #include "scheduler/scheduler.h"
 
 #include "telemetry/telemetry.h"
+#include "telemetry/crsf.h"
 
 #ifdef USE_BST
 #include "i2c_bst.h"
@@ -266,6 +267,13 @@ static void taskCameraControl(uint32_t currentTime)
 }
 #endif
 
+#ifdef USE_CRSF_V3
+static void taskSpeedNegotiation(uint32_t currentTime)
+{
+    speedNegotiationProcess(currentTime);
+}
+#endif
+
 void tasksInit(void)
 {
     schedulerInit();
@@ -409,6 +417,10 @@ void tasksInit(void)
 #ifdef USE_RCDEVICE
     setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled());
 #endif
+
+#ifdef USE_CRSF_V3
+    setTaskEnabled(TASK_SPEED_NEGOTIATION, true);
+#endif
 }
 
 #if defined(USE_TASK_STATISTICS)
@@ -527,6 +539,10 @@ task_t tasks[TASK_COUNT] = {
 #ifdef USE_RANGEFINDER
     [TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, taskUpdateRangefinder, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE),
 #endif
+
+#ifdef USE_CRSF_V3
+    [TASK_SPEED_NEGOTIATION] = DEFINE_TASK("SPEED_NEGOTIATION", NULL, NULL, taskSpeedNegotiation, TASK_PERIOD_HZ(100), TASK_PRIORITY_IDLE),
+#endif
 };
 
 task_t *getTask(unsigned taskId)

src/main/rx/crsf.c

@@ -347,7 +347,7 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
 
     static uint8_t crsfFramePosition = 0;
 #if defined(USE_CRSF_V3)
-    static uint32_t crsfFrameErrorCnt = 0;
+    static uint8_t crsfFrameErrorCnt = 0;
 #endif
     const timeUs_t currentTimeUs = microsISR();
 

src/main/scheduler/scheduler.h

@@ -150,6 +150,10 @@ typedef enum {
     TASK_PINIOBOX,
 #endif
 
+#ifdef USE_CRSF_V3
+    TASK_SPEED_NEGOTIATION,
+#endif
+
     /* Count of real tasks */
     TASK_COUNT,
 

src/main/telemetry/crsf.c

@@ -116,10 +116,11 @@ uint32_t getCrsfDesireSpeed(void)
 void setCrsfDefaultSpeed(void)
 {
     crsfSpeed.hasPendingReply = false;
-    crsfSpeed.isNewSpeedValid = true;
+    crsfSpeed.isNewSpeedValid = false;
     crsfSpeed.confirmationTime = 0;
     crsfSpeed.index = BAUD_COUNT;
     isCrsfV3Running = false;
+    crsfRxUpdateBaudrate(getCrsfDesireSpeed());
 }
 #endif
 
@@ -396,6 +397,40 @@ void crsfScheduleSpeedNegotiationResponse(void) {
     crsfSpeed.hasPendingReply = true;
     crsfSpeed.isNewSpeedValid = false;
 }
+
+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) {
+        sbuf_t crsfPayloadBuf;
+        sbuf_t *dst = &crsfPayloadBuf;
+        crsfInitializeFrame(dst);
+        crsfFrameDeviceInfo(dst);
+        crsfFinalize(dst);
+        crsfRxSendTelemetryData();
+    }
+}
 #endif
 
 #if defined(USE_CRSF_CMS_TELEMETRY)
@@ -725,30 +760,6 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
     }
 #endif
 
-#if defined(USE_CRSF_V3)
-    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);
-        crsfSpeed.hasPendingReply = false;
-        crsfSpeed.isNewSpeedValid = true;
-        crsfSpeed.confirmationTime = currentTimeUs;
-        crsfLastCycleTime = currentTimeUs;
-        return;
-    } else if (crsfSpeed.isNewSpeedValid) {
-        if (currentTimeUs - crsfSpeed.confirmationTime >= 10000) {
-            // delay 10ms before applying the new baudrate
-            crsfRxUpdateBaudrate(getCrsfDesireSpeed());
-            crsfSpeed.isNewSpeedValid = false;
-            isCrsfV3Running = true;
-            return;
-        }
-    }
-#endif
-
     // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
     // Spread out scheduled frames evenly so each frame is sent at the same frequency.
     if (currentTimeUs >= crsfLastCycleTime + (CRSF_CYCLETIME_US / crsfScheduleCount)) {

src/main/telemetry/crsf.h

@@ -47,3 +47,6 @@ void crsfProcessDisplayPortCmd(uint8_t *frameStart);
 void initCrsfMspBuffer(void);
 bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength);
 #endif
+#if defined(USE_CRSF_V3)
+void speedNegotiationProcess(uint32_t currentTime);
+#endif