CRSF: code cleanup and BROADCAST address removed from accepted start …

…bytes list
cruwaller · Jun 24, 2024 · 0dad892 · 0dad892
1 parent 4132896
commit 0dad892
Show file tree

Hide file tree

Showing 5 changed files with 62 additions and 58 deletions.
diff --git a/src/lib/CRSF/CRSF.cpp b/src/lib/CRSF/CRSF.cpp
@@ -46,19 +46,23 @@ uint8_t *CRSF::ParseInByte(uint8_t const inChar)
 
     if (CRSFframeActive == false)
     {
-        if ((inChar == CRSF_ADDRESS_CRSF_RECEIVER) || (inChar == CRSF_ADDRESS_CRSF_TRANSMITTER) ||
-            (inChar == CRSF_SYNC_BYTE) || (inChar == CRSF_ADDRESS_BROADCAST))
+        if ((inChar == CRSF_SYNC_BYTE) || (inChar == CRSF_ADDRESS_CRSF_TRANSMITTER)
+#if defined(RX_MODULE)
+            // RX module should capture its onw packages just in case
+            || (inChar == CRSF_ADDRESS_CRSF_RECEIVER)
+#endif
+            /*|| (inChar == CRSF_ADDRESS_BROADCAST)*/)
         {
             CRSFframeActive = true;
-            SerialInPacketLen = 0;
+            SerialInPacketLen = -1;
 #ifdef DBF_PIN_CRSF_PACKET
             gpio_out_write(crsf_packet, 1);
 #endif
         }
         return NULL;
     }
 
-    if (SerialInPacketLen == 0) // we read the packet length and save it
+    if (SerialInPacketLen < 0) // we read the packet length and save it
     {
         SerialInCrc = 0;
         SerialInPacketPtr = 0;
@@ -123,8 +127,6 @@ uint8_t *CRSF::ParseInByte(uint8_t const inChar)
 
     // packet handled, start next
     CRSFframeActive = false;
-    SerialInPacketPtr = 0;
-    SerialInPacketLen = 0;
 
     return packet_ptr;
 }
diff --git a/src/lib/CRSF/CRSF.h b/src/lib/CRSF/CRSF.h
@@ -547,7 +547,7 @@ class CRSF
 private:
     bool CRSFframeActive;
     uint8_t SerialInCrc;
-    uint8_t SerialInPacketLen;      // length of the CRSF packet as measured
+    int16_t SerialInPacketLen;      // length of the CRSF packet as measured
     uint8_t SerialInPacketPtr;      // index where we are reading/writing
     bool pingpong;
     uint8_t * p_buff;

diff --git a/src/lib/CRSF/CRSF_TX.cpp b/src/lib/CRSF/CRSF_TX.cpp
@@ -11,6 +11,8 @@
 struct gpio_out crsf_byte_in;
 #endif
 
+#define OPENTX_SYNC_SCALE 10
+
 static void rcNullCb(uint8_t const *const) {}
 static void nullCallback(void){};
 
@@ -88,7 +90,7 @@ void CRSF_TX::Begin(void)
     p_msp_packet.header.orig_addr = CRSF_ADDRESS_FLIGHT_CONTROLLER;
 
     CRSF::Begin();
-    p_UartNextCheck = millis(); //  +UARTwdtInterval * 2;
+    p_UartNextCheck = millis();
 }
 
 void FAST_CODE_2 CRSF_TX::CrsfFramePushToFifo(uint8_t *buff, uint8_t size) const
@@ -193,29 +195,38 @@ void CRSF_TX::sendMspPacketToRadio(mspPacket_t &msp) const
     CrsfFramePushToFifo((uint8_t*)&p_msp_packet, sizeof(p_msp_packet));
 }
 
-int CRSF_TX::sendSyncPacketToRadio()
+void CRSF_TX::sendSyncPacketToRadio(void)
 {
 #if (FEATURE_OPENTX_SYNC)
-    if (RCdataLastRecv && p_RadioConnected)
+    if (!RCdataLastRecv || !p_RadioConnected)
     {
-        uint32_t const current = millis();
-        // Adjust radio timing if not in requested window or not sent within 200ms
-        if (OTX_SYNC_INTERVAL <= (current - OpenTXsynNextSend))
-        {
-            int32_t offset = (int32_t)(OpenTXsyncOffset - OTX_SYNC_ADVANCE);
-            OpenTXsynNextSend = current;
-
-            //DEBUG_PRINTF("Sync: int=%u,off=%d\n", RequestedRCpacketInterval, offset);
+        return;
+    }
 
-            p_otx_sync_packet.packetRate =
-                __builtin_bswap32(RequestedRCpacketInterval);
-            p_otx_sync_packet.offset = __builtin_bswap32(offset * 10);
-            CrsfFramePushToFifo((uint8_t*)&p_otx_sync_packet, sizeof(p_otx_sync_packet));
-            return 0;
-        }
+    uint32_t const current = millis();
+    // Adjust radio timing if not in requested window or not sent within 200ms
+    if ((int32_t)(current - OpenTXsynNextSend) < OTX_SYNC_INTERVAL)
+    {
+        return;
     }
+
+    int32_t const offset = (int32_t)(OpenTXsyncOffset - OTX_SYNC_ADVANCE);
+    p_otx_sync_packet.offset = __builtin_bswap32(offset * OPENTX_SYNC_SCALE);
+    CrsfFramePushToFifo((uint8_t*)&p_otx_sync_packet, sizeof(p_otx_sync_packet));
+
+    //DEBUG_PRINTF("Sync: int=%u,off=%d\n", p_otx_sync_packet.packetRate, offset);
+
+    OpenTXsynNextSend = current;
 #endif /* FEATURE_OPENTX_SYNC */
-    return -1;
+    return;
+}
+
+void CRSF_TX::setRcPacketRate(uint32_t const interval) const
+{
+#if (FEATURE_OPENTX_SYNC)
+    // Update packet rate and scale value to correct format
+    p_otx_sync_packet.packetRate = __builtin_bswap32(interval * OPENTX_SYNC_SCALE);
+#endif
 }
 
 void CRSF_TX::processPacket(uint8_t const *input)
@@ -314,35 +325,34 @@ uint8_t CRSF_TX::handleUartIn(void)
 void CRSF_TX::uart_wdt(void)
 {
     uint32_t now = millis();
-    if (UARTwdtInterval <= (now - p_UartNextCheck))
+    if ((int32_t)(now - p_UartNextCheck) < UARTwdtInterval)
     {
-        DEBUG_PRINTF("CRSF Bad:Good %u:%u\n", BadPktsCount, GoodPktsCount);
-
-        if (BadPktsCount >= GoodPktsCount)
-        {
-            if (p_RadioConnected == true)
-            {
-                DEBUG_PRINTF("CRSF UART Disconnect. ");
-                disconnected();
-                p_RadioConnected = false;
-#if (FEATURE_OPENTX_SYNC)
-                OpenTXsynNextSend = 0;
-                OpenTXsyncOffset = 0;
-#endif
-            }
+        // No need to check result yet
+        return;
+    }
 
-            _dev->end();
-            platform_wd_feed();
+    DEBUG_PRINTF("CRSF Bad:Good %u:%u\n", BadPktsCount, GoodPktsCount);
 
-            p_baudrateIdx = (p_baudrateIdx + 1) % ARRAY_SIZE(availableHandsetBauds);
-            _dev->Begin(availableHandsetBauds[p_baudrateIdx]);
-            DEBUG_PRINTF("Switched to baudrate %u\n", availableHandsetBauds[p_baudrateIdx]);
+    if (BadPktsCount >= GoodPktsCount)
+    {
+        if (p_RadioConnected == true)
+        {
+            DEBUG_PRINTF("CRSF UART Disconnect. ");
+            disconnected();
+            p_RadioConnected = false;
         }
 
-        p_UartNextCheck = now;
-        BadPktsCount = 0;
-        GoodPktsCount = 0;
+        _dev->end();
+        platform_wd_feed();
+
+        p_baudrateIdx = (p_baudrateIdx + 1) % ARRAY_SIZE(availableHandsetBauds);
+        _dev->Begin(availableHandsetBauds[p_baudrateIdx]);
+        DEBUG_PRINTF("Switched to baudrate %u\n", availableHandsetBauds[p_baudrateIdx]);
     }
+
+    p_UartNextCheck = now;
+    BadPktsCount = 0;
+    GoodPktsCount = 0;
 }
 
 #endif // BACKPACK_LOGGER_BUILD
diff --git a/src/lib/CRSF/CRSF_TX.h b/src/lib/CRSF/CRSF_TX.h
@@ -40,13 +40,7 @@ class CRSF_TX : public CRSF
     void sendMspPacketToRadio(mspPacket_t &msp) const;
 
     // OpenTX Syncing
-    void FAST_CODE_1 setRcPacketRate(uint32_t const interval)
-    {
-#if (FEATURE_OPENTX_SYNC)
-        // Scale value to correct format
-        RequestedRCpacketInterval = interval * 10;
-#endif
-    }
+    void setRcPacketRate(uint32_t const interval) const;
 
     void FAST_CODE_1 UpdateOpenTxSyncOffset(uint32_t const current_us)
     {
@@ -79,10 +73,9 @@ class CRSF_TX : public CRSF
 
     uint32_t RCdataLastRecv;
     int32_t OpenTXsyncOffset;
-    uint32_t RequestedRCpacketInterval;
     uint32_t OpenTXsynNextSend;
 #endif /* FEATURE_OPENTX_SYNC */
-    int sendSyncPacketToRadio();
+    void sendSyncPacketToRadio(void);
 
     // for the UART wdt, every 1000ms we change bauds when connect is lost
 #define UARTwdtInterval 1000

diff --git a/src/src/esp32/esp32.cpp b/src/src/esp32/esp32.cpp
@@ -237,7 +237,6 @@ void platform_reboot_into_bootloader(const uint8_t * info)
 void platform_wifi_start(void)
 {
 #if WIFI_LOGGER || WIFI_UPDATER
-    //platform_radio_force_stop();
     wifi_start();
 #endif
 }