rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ExpressLRS - fixed slow initial connection

This commit is contained in:
phobos- 2022-01-01 11:57:28 +01:00
parent b86106941f
commit b42c907742
12 changed files with 95 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/main/config/config.c
View File

@ -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();

2
src/main/drivers/rx/rx_sx127x.c
View File

@ -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)

2
src/main/drivers/rx/rx_sx1280.c
View File

@ -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 */

101
src/main/rx/expresslrs.c
View File

@ -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 */

1
src/main/rx/expresslrs.h
View File

@ -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);

1
src/main/rx/expresslrs_impl.h
View File

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

11
src/main/rx/rx_spi.c
View File

@ -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

1
src/main/rx/rx_spi.h
View File

@ -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);

2
src/main/target/BETAFPVF4SX1280/target.h
View File

@ -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

2
src/main/target/CRAZYBEEF4SX1280/target.h
View File

@ -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

1
src/test/Makefile
View File

@ -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= \

3
src/test/unit/rx_spi_expresslrs_unittest.cc
View File

@ -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;