Merge pull request #11500 from klutvott123/crsf-baud-negotiation-improvements-2

Fix CRSF baud negotiation

src/main/config/config.c

@@ -97,6 +97,8 @@ static bool configIsDirty; /* someone indicated that the config is modified and
 
 static bool rebootRequired = false;  // set if a config change requires a reboot to take effect
 
+static bool eepromWriteInProgress = false;
+
 pidProfile_t *currentPidProfile;
 
 #ifndef RX_SPI_DEFAULT_PROTOCOL
@@ -126,6 +128,11 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
     .enableStickArming = false,
 );
 
+bool isEepromWriteInProgress(void)
+{
+    return eepromWriteInProgress;
+}
+
 uint8_t getCurrentPidProfileIndex(void)
 {
     return systemConfig()->pidProfileIndex;
@@ -738,9 +745,9 @@ void writeUnmodifiedConfigToEEPROM(void)
     validateAndFixConfig();
 
     suspendRxSignal();
-
+    eepromWriteInProgress = true;
     writeConfigToEEPROM();
-
+    eepromWriteInProgress = false;
     resumeRxSignal();
     configIsDirty = false;
 }

src/main/config/config.h

@@ -90,3 +90,4 @@ void targetValidateConfiguration(void);
 bool isSystemConfigured(void);
 void setRebootRequired(void);
 bool getRebootRequired(void);
+bool isEepromWriteInProgress(void);

src/main/fc/tasks.c

@@ -429,7 +429,7 @@ task_attribute_t task_attributes[TASK_COUNT] = {
 #endif
 
 #ifdef USE_CRSF_V3
-    [TASK_SPEED_NEGOTIATION] = DEFINE_TASK("SPEED_NEGOTIATION", NULL, NULL, speedNegotiationProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOWEST),
+    [TASK_SPEED_NEGOTIATION] = DEFINE_TASK("SPEED_NEGOTIATION", NULL, NULL, speedNegotiationProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
 #endif
 };
 

src/main/rx/crsf.c

@@ -34,6 +34,8 @@
 #include "common/maths.h"
 #include "common/utils.h"
 
+#include "config/config.h"
+
 #include "pg/rx.h"
 
 #include "drivers/serial.h"
@@ -58,7 +60,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 +364,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 +464,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() || isEepromWriteInProgress()) {
+            // don't count errors when negotiation or eeprom write is in progress
+            crsfFrameErrorCnt = 0;
+        } else if (crsfFrameErrorCnt >= CRSF_FRAME_ERROR_COUNT_THRESHOLD) {
             // fall back to default speed if speed mismatch detected
             setCrsfDefaultSpeed();
             crsfFrameErrorCnt = 0;
@@ -630,11 +636,17 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
         return false;
     }
 
+    uint32_t crsfBaudrate = CRSF_BAUDRATE;
+
+#if defined(USE_CRSF_V3)
+    crsfBaudrate = (isMPUSoftReset() && rxConfig->crsf_use_negotiated_baud) ? getCrsfCachedBaudrate() : CRSF_BAUDRATE;
+#endif
+
     serialPort = openSerialPort(portConfig->identifier,
         FUNCTION_RX_SERIAL,
         crsfDataReceive,
         rxRuntimeState,
-        CRSF_BAUDRATE,
+        crsfBaudrate,
         CRSF_PORT_MODE,
         CRSF_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0)
         );

src/main/rx/crsf_protocol.h

@@ -37,6 +37,7 @@ enum { CRSF_PAYLOAD_SIZE_MAX = CRSF_FRAME_SIZE_MAX - 6 };
 typedef enum {
     CRSF_FRAMETYPE_GPS = 0x02,
     CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
+    CRSF_FRAMETYPE_HEARTBEAT = 0x0B,
     CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
     CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
     CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED = 0x17,
@@ -83,6 +84,7 @@ enum {
 enum {
     CRSF_FRAME_GPS_PAYLOAD_SIZE = 15,
     CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
+    CRSF_FRAME_HEARTBEAT_PAYLOAD_SIZE = 2,
     CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
     CRSF_FRAME_LINK_STATISTICS_TX_PAYLOAD_SIZE = 6,
     CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes.

src/main/telemetry/crsf.c

@@ -32,13 +32,14 @@
 
 #include "cms/cms.h"
 
+#include "config/config.h"
 #include "config/feature.h"
 
-#include "config/config.h"
 #include "common/crc.h"
 #include "common/maths.h"
 #include "common/printf.h"
 #include "common/streambuf.h"
+#include "common/time.h"
 #include "common/utils.h"
 
 #include "drivers/nvic.h"
@@ -88,6 +89,9 @@ typedef struct mspBuffer_s {
 static mspBuffer_t mspRxBuffer;
 
 #if defined(USE_CRSF_V3)
+
+#define CRSF_TELEMETRY_FRAME_INTERVAL_MAX_US 20000 // 20ms
+
 static bool isCrsfV3Running = false;
 typedef struct {
     uint8_t hasPendingReply:1;
@@ -99,6 +103,19 @@ typedef struct {
 
 static crsfSpeedControl_s crsfSpeed = {0};
 
+uint32_t crsfCachedBaudrate __attribute__ ((section (".noinit"))); // Used for retaining negotiated baudrate after soft reset
+
+uint32_t getCrsfCachedBaudrate(void)
+{
+    // check if valid first. return default baudrate if not
+    for (unsigned i = 0; i < BAUD_COUNT; i++) {
+        if (crsfCachedBaudrate == baudRates[i] && baudRates[i] >= CRSF_BAUDRATE) {
+            return crsfCachedBaudrate;
+        }
+    }
+    return CRSF_BAUDRATE;
+}
+
 bool checkCrsfCustomizedSpeed(void)
 {
     return crsfSpeed.index < BAUD_COUNT ? true : false;
@@ -116,7 +133,13 @@ void setCrsfDefaultSpeed(void)
     crsfSpeed.confirmationTime = 0;
     crsfSpeed.index = BAUD_COUNT;
     isCrsfV3Running = false;
-    crsfRxUpdateBaudrate(getCrsfDesiredSpeed());
+    crsfCachedBaudrate = getCrsfDesiredSpeed();
+    crsfRxUpdateBaudrate(crsfCachedBaudrate);
+}
+
+bool crsfBaudNegotiationInProgress(void)
+{
+    return crsfSpeed.hasPendingReply || crsfSpeed.isNewSpeedValid;
 }
 #endif
 
@@ -248,6 +271,18 @@ void crsfFrameBatterySensor(sbuf_t *dst)
     sbufWriteU8(dst, batteryRemainingPercentage);
 }
 
+/*
+0x0B Heartbeat
+Payload:
+int16_t    origin_add ( Origin Device address )
+*/
+void crsfFrameHeartbeat(sbuf_t *dst)
+{
+    sbufWriteU8(dst, CRSF_FRAME_HEARTBEAT_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
+    sbufWriteU8(dst, CRSF_FRAMETYPE_HEARTBEAT);
+    sbufWriteU16BigEndian(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+}
+
 typedef enum {
     CRSF_ACTIVE_ANTENNA1 = 0,
     CRSF_ACTIVE_ANTENNA2 = 1
@@ -418,40 +453,37 @@ void crsfScheduleSpeedNegotiationResponse(void)
     crsfSpeed.isNewSpeedValid = false;
 }
 
-void speedNegotiationProcess(uint32_t currentTime)
+void speedNegotiationProcess(timeUs_t currentTimeUs)
 {
-    if (!featureIsEnabled(FEATURE_TELEMETRY) && getCrsfDesiredSpeed() == CRSF_BAUDRATE) {
+    if (crsfSpeed.hasPendingReply) {
-        // to notify the RX to fall back to default baud rate by sending device info frame if telemetry is disabled
+        bool found = ((crsfSpeed.index < BAUD_COUNT) && crsfRxUseNegotiatedBaud()) ? true : false;
-        sbuf_t crsfPayloadBuf;
+        sbuf_t crsfSpeedNegotiationBuf;
-        sbuf_t *dst = &crsfPayloadBuf;
+        sbuf_t *dst = &crsfSpeedNegotiationBuf;
         crsfInitializeFrame(dst);
-        crsfFrameDeviceInfo(dst);
+        crsfFrameSpeedNegotiationResponse(dst, found);
         crsfRxSendTelemetryData(); // prevent overwriting previous data
         crsfFinalize(dst);
         crsfRxSendTelemetryData();
-    } else {
+        crsfSpeed.hasPendingReply = false;
-        if (crsfSpeed.hasPendingReply) {
+        crsfSpeed.isNewSpeedValid = found;
-            bool found = ((crsfSpeed.index < BAUD_COUNT) && crsfRxUseNegotiatedBaud()) ? true : false;
+        crsfSpeed.confirmationTime = currentTimeUs;
-            sbuf_t crsfSpeedNegotiationBuf;
+    } else if (crsfSpeed.isNewSpeedValid) {
-            sbuf_t *dst = &crsfSpeedNegotiationBuf;
+        if (cmpTimeUs(currentTimeUs, crsfSpeed.confirmationTime) >= 4000) {
-            crsfInitializeFrame(dst);
+            // delay 4ms before applying the new baudrate
-            crsfFrameSpeedNegotiationResponse(dst, found);
+            crsfCachedBaudrate = getCrsfDesiredSpeed();
-            crsfRxSendTelemetryData(); // prevent overwriting previous data
+            crsfRxUpdateBaudrate(crsfCachedBaudrate);
-            crsfFinalize(dst);
+            crsfSpeed.isNewSpeedValid = false;
-            crsfRxSendTelemetryData();
+            isCrsfV3Running = true;
-            crsfSpeed.hasPendingReply = false;
-            crsfSpeed.isNewSpeedValid = found;
-            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;
-            }
         }
+    } else if (!featureIsEnabled(FEATURE_TELEMETRY) && crsfRxUseNegotiatedBaud()) {
+        // Send heartbeat if telemetry is disabled to allow RX to detect baud rate mismatches
+        sbuf_t crsfPayloadBuf;
+        sbuf_t *dst = &crsfPayloadBuf;
+        crsfInitializeFrame(dst);
+        crsfFrameHeartbeat(dst);
+        crsfRxSendTelemetryData(); // prevent overwriting previous data
+        crsfFinalize(dst);
+        crsfRxSendTelemetryData();
     }
 }
 #endif
@@ -552,6 +584,7 @@ typedef enum {
     CRSF_FRAME_BATTERY_SENSOR_INDEX,
     CRSF_FRAME_FLIGHT_MODE_INDEX,
     CRSF_FRAME_GPS_INDEX,
+    CRSF_FRAME_HEARTBEAT_INDEX,
     CRSF_SCHEDULE_COUNT_MAX
 } crsfFrameTypeIndex_e;
 
@@ -621,6 +654,15 @@ static void processCrsf(void)
         crsfFinalize(dst);
     }
 #endif
+
+#if defined(USE_CRSF_V3)
+    if (currentSchedule & BIT(CRSF_FRAME_HEARTBEAT_INDEX)) {
+        crsfInitializeFrame(dst);
+        crsfFrameHeartbeat(dst);
+        crsfFinalize(dst);
+    }
+#endif
+
     crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
 }
 
@@ -661,6 +703,14 @@ void initCrsfTelemetry(void)
         crsfSchedule[index++] = BIT(CRSF_FRAME_GPS_INDEX);
     }
 #endif
+
+#if defined(USE_CRSF_V3)
+    while (index < (CRSF_CYCLETIME_US / CRSF_TELEMETRY_FRAME_INTERVAL_MAX_US) && index < CRSF_SCHEDULE_COUNT_MAX) {
+        // schedule heartbeat to ensure that telemetry/heartbeat frames are sent at minimum 50Hz
+        crsfSchedule[index++] = BIT(CRSF_FRAME_HEARTBEAT_INDEX);
+    }
+#endif
+
     crsfScheduleCount = (uint8_t)index;
 
 #if defined(USE_CRSF_CMS_TELEMETRY)
@@ -730,6 +780,13 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
     if (!crsfTelemetryEnabled) {
         return;
     }
+
+#if defined(USE_CRSF_V3)
+    if (crsfBaudNegotiationInProgress()) {
+        return;
+    }
+#endif
+
     // Give the receiver a chance to send any outstanding telemetry data.
     // This needs to be done at high frequency, to enable the RX to send the telemetry frame
     // in between the RX frames.

src/main/telemetry/crsf.h

@@ -48,4 +48,6 @@ 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);
+uint32_t getCrsfCachedBaudrate(void);
 #endif