From b42c90774225a21cd3fb05de4994754cfd7644f0 Mon Sep 17 00:00:00 2001 From: phobos- Date: Sat, 1 Jan 2022 11:57:28 +0100 Subject: [PATCH] ExpressLRS - fixed slow initial connection --- src/main/config/config.c | 3 + src/main/drivers/rx/rx_sx127x.c | 2 + src/main/drivers/rx/rx_sx1280.c | 2 + src/main/rx/expresslrs.c | 101 +++++++++++++------- src/main/rx/expresslrs.h | 1 + src/main/rx/expresslrs_impl.h | 1 + src/main/rx/rx_spi.c | 11 +++ src/main/rx/rx_spi.h | 1 + src/main/target/BETAFPVF4SX1280/target.h | 2 +- src/main/target/CRAZYBEEF4SX1280/target.h | 2 +- src/test/Makefile | 1 + src/test/unit/rx_spi_expresslrs_unittest.cc | 3 + 12 files changed, 95 insertions(+), 35 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 071c6bd9f..b58af4294 100644 --- a/src/main/config/config.c +++ b/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(); diff --git a/src/main/drivers/rx/rx_sx127x.c b/src/main/drivers/rx/rx_sx127x.c index 51ab62521..aa85fd13a 100644 --- a/src/main/drivers/rx/rx_sx127x.c +++ b/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) diff --git a/src/main/drivers/rx/rx_sx1280.c b/src/main/drivers/rx/rx_sx1280.c index f0c635496..7d35d88f2 100644 --- a/src/main/drivers/rx/rx_sx1280.c +++ b/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 */ diff --git a/src/main/rx/expresslrs.c b/src/main/rx/expresslrs.c index 0cd023bbb..4991b06e5 100644 --- a/src/main/rx/expresslrs.c +++ b/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 - gotConnection(timeStampMs); + if ((receiver.connectionState == ELRS_TENTATIVE) && (ABS(pl.offsetDeltaUs) <= 10) && (pl.offsetUs < 100) && (lqGet() > minLqForChaos())) { + 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) { - 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 { + if (receiver.connectionState == ELRS_CONNECTED) { 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 tlmSent = handleSendTelemetryResponse(); + bool didFHSS = false; + 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 */ diff --git a/src/main/rx/expresslrs.h b/src/main/rx/expresslrs.h index c6ae3b874..5ed7cdb88 100644 --- a/src/main/rx/expresslrs.h +++ b/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); diff --git a/src/main/rx/expresslrs_impl.h b/src/main/rx/expresslrs_impl.h index eb480c4a6..2655cf2c9 100644 --- a/src/main/rx/expresslrs_impl.h +++ b/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; diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c index c6cab6fc6..68f6a21f1 100644 --- a/src/main/rx/rx_spi.c +++ b/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 diff --git a/src/main/rx/rx_spi.h b/src/main/rx/rx_spi.h index b19f2b20a..990cdda21 100644 --- a/src/main/rx/rx_spi.h +++ b/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); diff --git a/src/main/target/BETAFPVF4SX1280/target.h b/src/main/target/BETAFPVF4SX1280/target.h index 5758ec5f0..87ff15fa5 100644 --- a/src/main/target/BETAFPVF4SX1280/target.h +++ b/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 diff --git a/src/main/target/CRAZYBEEF4SX1280/target.h b/src/main/target/CRAZYBEEF4SX1280/target.h index c895b8c16..ee27fb83b 100644 --- a/src/main/target/CRAZYBEEF4SX1280/target.h +++ b/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 diff --git a/src/test/Makefile b/src/test/Makefile index a2c105ed9..696e44c0b 100644 --- a/src/test/Makefile +++ b/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= \ diff --git a/src/test/unit/rx_spi_expresslrs_unittest.cc b/src/test/unit/rx_spi_expresslrs_unittest.cc index bcfea96ac..f37b59473 100644 --- a/src/test/unit/rx_spi_expresslrs_unittest.cc +++ b/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;