ExpressLRS - fixed slow initial connection

src/main/config/config.c

@@ -736,6 +736,9 @@ void writeUnmodifiedConfigToEEPROM(void)
 
 void writeEEPROM(void)
 {
+#ifdef USE_RX_SPI
+    rxSpiStop(); // some rx spi protocols use hardware timer, which needs to be stopped before writing to eeprom
+#endif
     systemConfigMutable()->configurationState = CONFIGURATION_STATE_CONFIGURED;
 
     writeUnmodifiedConfigToEEPROM();

src/main/drivers/rx/rx_sx127x.c

@@ -427,6 +427,8 @@ void sx127xGetLastPacketStats(int8_t *rssi, int8_t *snr)
 {
     *rssi = sx127xGetLastPacketRSSI();
     *snr = sx127xGetLastPacketSNR();
+    int8_t negOffset = (*snr < 0) ? *snr : 0;
+    *rssi += negOffset;
 }
 
 uint8_t sx127xGetIrqFlags(void)

src/main/drivers/rx/rx_sx1280.c

@@ -458,6 +458,8 @@ void sx1280GetLastPacketStats(int8_t *rssi, int8_t *snr)
     sx1280ReadCommandBurst(SX1280_RADIO_GET_PACKETSTATUS, status, 2);
     *rssi = -(int8_t)(status[0] / 2);
     *snr = ((int8_t) status[1]) / 4;
+    int8_t negOffset = (*snr < 0) ? *snr : 0;
+    *rssi += negOffset;
 }
 
 #endif /* USE_RX_SX1280 */

src/main/rx/expresslrs.c

@@ -33,6 +33,7 @@
 
 #ifdef USE_RX_EXPRESSLRS
 
+#include "build/atomic.h"
 #include "build/debug.h"
 #include "build/debug_pin.h"
 
@@ -40,6 +41,7 @@
 #include "common/filter.h"
 
 #include "drivers/io.h"
+#include "drivers/nvic.h"
 #include "drivers/rx/rx_spi.h"
 #include "drivers/system.h"
 #include "drivers/time.h"
@@ -51,6 +53,8 @@
 #include "config/config.h"
 #include "config/feature.h"
 
+#include "fc/init.h"
+
 #include "pg/pg.h"
 #include "pg/pg_ids.h"
 #include "pg/rx_spi.h"
@@ -354,19 +358,23 @@ static bool handleFHSS(void)
     return true;
 }
 
-static bool handleSendTelemetryResponse(void)
+static bool shouldSendTelemetryResponse(void)
+{
+    uint8_t modresult = (receiver.nonceRX + 1) % tlmRatioEnumToValue(receiver.modParams->tlmInterval);
+    if ((receiver.connectionState == ELRS_DISCONNECTED) || (receiver.modParams->tlmInterval == TLM_RATIO_NO_TLM) || (receiver.alreadyTLMresp == true) || (modresult != 0)) {
+        return false; // don't bother sending tlm if disconnected or TLM is off
+    } else {
+        return true;
+    }
+}
+
+static void handleSendTelemetryResponse(void)
 {
     uint8_t packet[8];
 
     uint8_t *data;
     uint8_t maxLength;
     uint8_t packageIndex;
-    uint8_t modresult = (receiver.nonceRX + 1) % tlmRatioEnumToValue(receiver.modParams->tlmInterval);
-
-    if ((receiver.connectionState == ELRS_DISCONNECTED) || (receiver.modParams->tlmInterval == TLM_RATIO_NO_TLM) 
-        || (receiver.alreadyTLMresp == true) || (modresult != 0)) {
-        return false; // don't bother sending tlm if disconnected or TLM is off
-    }
 
     receiver.alreadyTLMresp = true;
     packet[0] = ELRS_TLM_PACKET;
@@ -408,13 +416,13 @@ static bool handleSendTelemetryResponse(void)
 
     dbgPinHi(1);
     receiver.transmitData(packet, ELRS_RX_TX_BUFF_SIZE);
-    return true;
 }
 
 static void updatePhaseLock(void)
 {
     if (receiver.connectionState != ELRS_DISCONNECTED && expressLrsEPRHaveBothEvents()) {
-        pl.rawOffsetUs = expressLrsEPRGetResult();
+        int32_t maxOffset = receiver.modParams->interval / 4;
+        pl.rawOffsetUs = constrain(expressLrsEPRGetResult(), -maxOffset, maxOffset);
 
         pl.offsetUs = simpleLPFilterUpdate(&pl.offsetFilter, pl.rawOffsetUs);
         pl.offsetDeltaUs = simpleLPFilterUpdate(&pl.offsetDxFilter, pl.rawOffsetUs - pl.previousRawOffsetUs);
@@ -552,6 +560,7 @@ static void initializeReceiver(void)
     receiver.rateIndex = receiver.inBindingMode ? bindingRateIndex : rxExpressLrsSpiConfig()->rateIndex;
     setRFLinkRate(receiver.rateIndex);
 
+    receiver.started = false;
     receiver.alreadyFHSS = false;
     receiver.alreadyTLMresp = false;
     receiver.lockRFmode = false;
@@ -584,7 +593,6 @@ static void unpackBindPacket(uint8_t *packet)
     initializeReceiver();
 
     receiver.configChanged = true; //after initialize as it sets it to false
-    startReceiving();
 }
 
 /**
@@ -679,14 +687,12 @@ static bool processRFSyncPacket(uint8_t *packet, const uint32_t timeStampMs)
     return false;
 }
 
-static rx_spi_received_e processRFPacket(uint8_t *payload)
+static rx_spi_received_e processRFPacket(uint8_t *payload, uint32_t timeStampUs)
 {
     uint8_t packet[ELRS_RX_TX_BUFF_SIZE];
 
     receiver.receiveData(packet, ELRS_RX_TX_BUFF_SIZE);
 
-    uint32_t timeStampUs = micros();
-
     elrsPacketType_e type = packet[0] & 0x03;
     uint16_t inCRC = (((uint16_t)(packet[0] & 0xFC)) << 6 ) | packet[7];
 
@@ -925,8 +931,6 @@ bool expressLrsSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeSta
     // Timer IRQs must only be enabled after the receiver is configured otherwise race conditions occur.
     expressLrsTimerEnableIRQs();
 
-    startReceiving();
-
     return true;
 }
 
@@ -955,17 +959,21 @@ static void handleConnectionStateUpdate(const uint32_t timeStampMs)
 
     uint32_t localLastValidPacket = receiver.lastValidPacketMs; // Required to prevent race condition due to LastValidPacket getting updated from ISR
     if ((receiver.connectionState == ELRS_DISCONNECT_PENDING) || // check if we lost conn.
-        ((receiver.connectionState == ELRS_CONNECTED) && ((int32_t)receiver.rfPerfParams->disconnectTimeoutMs < (int32_t)(timeStampMs - localLastValidPacket)))) {
+        ((receiver.connectionState == ELRS_CONNECTED) && (receiver.rfPerfParams->disconnectTimeoutMs < (timeStampMs - localLastValidPacket)))) {
         lostConnection();
     }
 
-    if ((receiver.connectionState == ELRS_TENTATIVE) && (ABS(pl.offsetDeltaUs) <= 10) && (pl.offsetUs < 100) && (lqGet() > minLqForChaos())) { //detects when we are connected
+    if ((receiver.connectionState == ELRS_TENTATIVE) && (ABS(pl.offsetDeltaUs) <= 10) && (pl.offsetUs < 100) && (lqGet() > minLqForChaos())) {
-        gotConnection(timeStampMs);
+        gotConnection(timeStampMs); // detects when we are connected
     }
 
     if ((receiver.timerState == ELRS_TIM_TENTATIVE) && ((timeStampMs - receiver.gotConnectionMs) > ELRS_CONSIDER_CONNECTION_GOOD_MS) && (ABS(pl.offsetDeltaUs) <= 5)) {
         receiver.timerState = ELRS_TIM_LOCKED;
     }
+
+    if ((receiver.connectionState == ELRS_CONNECTED) && (ABS(pl.offsetDeltaUs) > 10) && (pl.offsetUs >= 100) && (lqGet() <= minLqForChaos())) {
+        lostConnection(); // SPI: resync when we're in chaos territory
+    }
 }
 
 static void handleConfigUpdate(const uint32_t timeStampMs)
@@ -973,7 +981,6 @@ static void handleConfigUpdate(const uint32_t timeStampMs)
     if ((timeStampMs - receiver.configCheckedAtMs) > ELRS_CONFIG_CHECK_MS) {
         receiver.configCheckedAtMs = timeStampMs;
         if (receiver.configChanged) {
-            lostConnection();
             writeEEPROM();
             receiver.configChanged = false;
         }
@@ -986,15 +993,7 @@ static void handleLinkStatsUpdate(const uint32_t timeStampMs)
 
         receiver.statsUpdatedAtMs = timeStampMs;
 
-        if (receiver.connectionState != ELRS_CONNECTED) {
+        if (receiver.connectionState == ELRS_CONNECTED) {
-            setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
-#ifdef USE_RX_RSSI_DBM
-            setRssiDbmDirect(-130, RSSI_SOURCE_RX_PROTOCOL);
-#endif
-#ifdef USE_RX_LINK_QUALITY_INFO
-            setLinkQualityDirect(0);
-#endif
-        } else {
             receiver.rssiFiltered = simpleLPFilterUpdate(&rssiFilter, receiver.rssi);
             uint16_t rssiScaled = scaleRange(constrain(receiver.rssiFiltered, receiver.rfPerfParams->sensitivity, -50), receiver.rfPerfParams->sensitivity, -50, 0, 1023);
             setRssi(rssiScaled, RSSI_SOURCE_RX_PROTOCOL);
@@ -1006,6 +1005,14 @@ static void handleLinkStatsUpdate(const uint32_t timeStampMs)
 #endif
 #ifdef USE_RX_LINK_UPLINK_POWER
             rxSetUplinkTxPwrMw(txPowerIndexToValue(txPower));
+#endif
+        } else {
+            setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
+#ifdef USE_RX_RSSI_DBM
+            setRssiDbmDirect(-130, RSSI_SOURCE_RX_PROTOCOL);
+#endif
+#ifdef USE_RX_LINK_QUALITY_INFO
+            setLinkQualityDirect(0);
 #endif
         }
     }
@@ -1050,10 +1057,16 @@ static void enterBindingMode(void)
     startReceiving();
 }
 
+static uint32_t isrTimeStampUs;
+
 rx_spi_received_e expressLrsDataReceived(uint8_t *payload)
 {
     rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
-    uint32_t isrTimeStampUs;
+
+    if (!receiver.started && (systemState & SYSTEM_STATE_READY)) {
+        receiver.started = true;
+        startReceiving(); // delay receiving after initialization to ensure a clean connect
+    }
 
     if (rxSpiCheckBindRequested(true)) {
         enterBindingMode();
@@ -1063,15 +1076,30 @@ rx_spi_received_e expressLrsDataReceived(uint8_t *payload)
     if (irqReason == ELRS_DIO_TX_DONE) {
         startReceiving();
     } else if (irqReason == ELRS_DIO_RX_DONE) {
-        result = processRFPacket(payload);
+        result = processRFPacket(payload, isrTimeStampUs);
     }
 
     if (receiver.fhssRequired) {
         receiver.fhssRequired = false;
-        bool didFHSS = handleFHSS();
+        bool didFHSS = false;
-        bool tlmSent = handleSendTelemetryResponse();
+        bool tlmReq = false;
+        ATOMIC_BLOCK(NVIC_PRIO_TIMER) { // prevent from updating nonce in TICK
+            didFHSS = handleFHSS();
+            tlmReq = shouldSendTelemetryResponse();
+        }
 
-        if (!didFHSS && !tlmSent && lqPeriodIsSet() && rxExpressLrsSpiConfig()->domain != ISM2400) {
+        if (tlmReq) {
+            // in case we miss a packet before TLM we still need to estimate processing time using %
+            uint32_t processingTime = (micros() - isrTimeStampUs) % receiver.modParams->interval;
+            if (processingTime < PACKET_HANDLING_TO_TOCK_ISR_DELAY_US && receiver.timerState == ELRS_TIM_LOCKED) {
+                handleSendTelemetryResponse();
+            } else {
+                receiver.alreadyTLMresp = true;
+                startReceiving();
+            }
+        }
+
+        if (rxExpressLrsSpiConfig()->domain != ISM2400 && !didFHSS && !tlmReq && lqPeriodIsSet()) {
             receiver.handleFreqCorrection(receiver.freqOffset, receiver.currentFreq); //corrects for RX freq offset
         }
     }
@@ -1094,4 +1122,11 @@ rx_spi_received_e expressLrsDataReceived(uint8_t *payload)
     return result;
 }
 
+void expressLrsStop(void)
+{
+    if (receiver.started) {
+        lostConnection();
+    }
+}
+
 #endif /* USE_RX_EXPRESSLRS */

src/main/rx/expresslrs.h

@@ -34,3 +34,4 @@
 bool expressLrsSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
 void expressLrsSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
 rx_spi_received_e expressLrsDataReceived(uint8_t *payload);
+void expressLrsStop(void);

src/main/rx/expresslrs_impl.h

@@ -73,6 +73,7 @@ typedef struct elrsReceiver_s {
     bool alreadyFHSS;
     bool alreadyTLMresp;
     bool lockRFmode;
+    bool started;
 
     timerState_e timerState;
     connectionState_e connectionState;

src/main/rx/rx_spi.c

@@ -57,15 +57,19 @@ uint16_t rxSpiRcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 STATIC_UNIT_TESTED uint8_t rxSpiPayload[RX_SPI_MAX_PAYLOAD_SIZE];
 STATIC_UNIT_TESTED uint8_t rxSpiNewPacketAvailable; // set true when a new packet is received
 
+static void nullProtocolStop(void) {}
+
 typedef bool (*protocolInitFnPtr)(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
 typedef rx_spi_received_e (*protocolDataReceivedFnPtr)(uint8_t *payload);
 typedef rx_spi_received_e (*protocolProcessFrameFnPtr)(uint8_t *payload);
 typedef void (*protocolSetRcDataFromPayloadFnPtr)(uint16_t *rcData, const uint8_t *payload);
+typedef void (*protocolStopFnPtr)(void);
 
 static protocolInitFnPtr protocolInit;
 static protocolDataReceivedFnPtr protocolDataReceived;
 static protocolProcessFrameFnPtr protocolProcessFrame;
 static protocolSetRcDataFromPayloadFnPtr protocolSetRcDataFromPayload;
+static protocolStopFnPtr protocolStop = nullProtocolStop;
 
 STATIC_UNIT_TESTED float rxSpiReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
 {
@@ -180,6 +184,7 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
         protocolInit = expressLrsSpiInit;
         protocolDataReceived = expressLrsDataReceived;
         protocolSetRcDataFromPayload = expressLrsSetRcDataFromPayload;
+        protocolStop = expressLrsStop;
         break;
 #endif
     default:
@@ -270,4 +275,10 @@ bool rxSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeStat
 
     return ret;
 }
+
+void rxSpiStop(void)
+{
+    protocolStop();
+}
+
 #endif

src/main/rx/rx_spi.h

@@ -95,3 +95,4 @@ typedef struct {
 #define RC_CHANNEL_RTH         RC_SPI_AUX6 // return to home
 
 bool rxSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState);
+void rxSpiStop(void);

src/main/target/BETAFPVF4SX1280/target.h

@@ -134,7 +134,7 @@
 
 #define USE_ESCSERIAL
 #define ENABLE_DSHOT_DMAR       DSHOT_DMAR_AUTO
-#define DSHOT_BITBANG_DEFAULT   DSHOT_BITBANG_OFF
+#define DSHOT_BITBANG_DEFAULT   DSHOT_BITBANG_ON
 
 #define TARGET_IO_PORTA         0xffff
 #define TARGET_IO_PORTB         0xffff

src/main/target/CRAZYBEEF4SX1280/target.h

@@ -125,7 +125,7 @@
 #define USE_LED_STRIP
 
 #define ENABLE_DSHOT_DMAR       DSHOT_DMAR_AUTO
-#define DSHOT_BITBANG_DEFAULT   DSHOT_BITBANG_OFF
+#define DSHOT_BITBANG_DEFAULT   DSHOT_BITBANG_ON
 
 #define USE_PINIO
 //#define PINIO1_PIN              PB5  // VTX  switcher

src/test/Makefile

@@ -422,6 +422,7 @@ rx_spi_expresslrs_unittest_SRC := \
         $(USER_DIR)/pg/rx_spi_expresslrs.c \
 		$(USER_DIR)/rx/expresslrs_common.c \
 		$(USER_DIR)/rx/expresslrs.c \
+		$(USER_DIR)/build/atomic.c \
 
 rx_spi_expresslrs_unittest_DEFINES := \
 		USE_RX_SPI= \

src/test/unit/rx_spi_expresslrs_unittest.cc

@@ -28,6 +28,8 @@
 extern "C" {
     #include "platform.h"
 
+    #include "build/atomic.h"
+
     #include "drivers/io.h"
     #include "common/filter.h"
 
@@ -372,6 +374,7 @@ TEST(RxSpiExpressLrsUnitTest, TestAnalogDecode)
 
 extern "C" {
 
+    uint8_t systemState;
     int16_t *debug;
     uint8_t debugMode;