From e9d7fe44452bb430f5f940836cc386ad8d95484c Mon Sep 17 00:00:00 2001 From: phobos- Date: Mon, 1 Aug 2022 17:22:31 +0200 Subject: [PATCH] SNR telemetry changes --- src/main/common/filter.c | 22 ++++++++++++++++++++++ src/main/common/filter.h | 9 +++++++++ src/main/drivers/rx/rx_sx127x.c | 9 ++++----- src/main/drivers/rx/rx_sx127x.h | 2 +- src/main/drivers/rx/rx_sx1280.c | 6 +++--- src/main/rx/expresslrs.c | 7 +++++-- 6 files changed, 44 insertions(+), 11 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 8e887aeb56..d3c5671325 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -309,3 +309,25 @@ void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpS filter->beta = beta; filter->fpShift = fpShift; } + +void meanAccumulatorAdd(meanAccumulator_t *filter, const int8_t newVal) +{ + filter->accumulator += newVal; + filter->count++; +} + +int8_t meanAccumulatorMean(meanAccumulator_t *filter, const int8_t defaultValue) +{ + if (filter->count) { + int8_t retVal = filter->accumulator / filter->count; + meanAccumulatorInit(filter); + return retVal; + } + return defaultValue; +} + +void meanAccumulatorInit(meanAccumulator_t *filter) +{ + filter->accumulator = 0; + filter->count = 0; +} diff --git a/src/main/common/filter.h b/src/main/common/filter.h index fe00bee8a5..b9d9740e57 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -119,3 +119,12 @@ typedef struct simpleLowpassFilter_s { int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal); void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift); + +typedef struct meanAccumulator_s { + int32_t accumulator; + int32_t count; +} meanAccumulator_t; + +void meanAccumulatorAdd(meanAccumulator_t *filter, const int8_t newVal); +int8_t meanAccumulatorMean(meanAccumulator_t *filter, const int8_t defaultValue); +void meanAccumulatorInit(meanAccumulator_t *filter); diff --git a/src/main/drivers/rx/rx_sx127x.c b/src/main/drivers/rx/rx_sx127x.c index e85fc20986..64f86a0129 100644 --- a/src/main/drivers/rx/rx_sx127x.c +++ b/src/main/drivers/rx/rx_sx127x.c @@ -417,17 +417,16 @@ int8_t sx127xGetCurrRSSI(void) return (-157 + sx127xGetRegisterValue(SX127X_REG_RSSI_VALUE, 7, 0)); } -int8_t sx127xGetLastPacketSNR(void) +int8_t sx127xGetLastPacketSNRRaw(void) { - int8_t rawSNR = (int8_t)sx127xGetRegisterValue(SX127X_REG_PKT_SNR_VALUE, 7, 0); - return (rawSNR / 4); + return (int8_t)sx127xGetRegisterValue(SX127X_REG_PKT_SNR_VALUE, 7, 0); } void sx127xGetLastPacketStats(int8_t *rssi, int8_t *snr) { *rssi = sx127xGetLastPacketRSSI(); - *snr = sx127xGetLastPacketSNR(); - int8_t negOffset = (*snr < 0) ? *snr : 0; + *snr = sx127xGetLastPacketSNRRaw(); + int8_t negOffset = (*snr < 0) ? (*snr / 4) : 0; *rssi += negOffset; } diff --git a/src/main/drivers/rx/rx_sx127x.h b/src/main/drivers/rx/rx_sx127x.h index 63a085dd19..407fcc1867 100644 --- a/src/main/drivers/rx/rx_sx127x.h +++ b/src/main/drivers/rx/rx_sx127x.h @@ -329,7 +329,7 @@ void sx127xAdjustFrequency(int32_t offset, const uint32_t freq); uint8_t sx127xUnsignedGetLastPacketRSSI(void); int8_t sx127xGetLastPacketRSSI(void); int8_t sx127xGetCurrRSSI(void); -int8_t sx127xGetLastPacketSNR(void); +int8_t sx127xGetLastPacketSNRRaw(void); uint8_t sx127xGetIrqFlags(void); void sx127xClearIrqFlags(void); uint8_t sx127xGetIrqReason(void); diff --git a/src/main/drivers/rx/rx_sx1280.c b/src/main/drivers/rx/rx_sx1280.c index e552106a4c..2b01d028db 100644 --- a/src/main/drivers/rx/rx_sx1280.c +++ b/src/main/drivers/rx/rx_sx1280.c @@ -360,7 +360,7 @@ void sx1280Config(const sx1280LoraBandwidths_e bw, const sx1280LoraSpreadingFact sx1280ConfigLoraDefaults(); sx1280SetOutputPower(13); //default is max power (12.5dBm for SX1280 RX) - sx1280SetMode(SX1280_MODE_STDBY_XOSC); + sx1280SetMode(SX1280_MODE_STDBY_RC); sx1280ClearIrqStatus(SX1280_IRQ_RADIO_ALL); sx1280ConfigLoraModParams(bw, sf, cr); sx1280SetPacketParams(preambleLength, SX1280_LORA_PACKET_IMPLICIT, 8, SX1280_LORA_CRC_OFF, (sx1280LoraIqModes_e)((uint8_t)!iqInverted << 6)); // TODO don't make static etc. @@ -530,8 +530,8 @@ void sx1280StartReceiving(void) void sx1280GetLastPacketStats(int8_t *rssi, int8_t *snr) { *rssi = -(int8_t)(packetStats[0] / 2); - *snr = ((int8_t) packetStats[1]) / 4; - int8_t negOffset = (*snr < 0) ? *snr : 0; + *snr = (int8_t) packetStats[1]; + int8_t negOffset = (*snr < 0) ? (*snr / 4) : 0; *rssi += negOffset; } diff --git a/src/main/rx/expresslrs.c b/src/main/rx/expresslrs.c index 2de2ba7816..a989229bd1 100644 --- a/src/main/rx/expresslrs.c +++ b/src/main/rx/expresslrs.c @@ -78,6 +78,7 @@ static uint8_t txPower = 0; static uint8_t wideSwitchIndex = 0; static uint8_t currTlmDenom = 1; static simpleLowpassFilter_t rssiFilter; +static meanAccumulator_t snrFilter; static volatile DMA_DATA uint8_t dmaBuffer[ELRS_RX_TX_BUFF_SIZE]; static volatile DMA_DATA uint8_t telemetryPacket[ELRS_RX_TX_BUFF_SIZE]; @@ -387,7 +388,7 @@ static void expressLrsSendTelemResp(void) otaPkt.tlm_dl.ul_link_stats.antenna = 0; otaPkt.tlm_dl.ul_link_stats.modelMatch = connectionHasModelMatch; otaPkt.tlm_dl.ul_link_stats.lq = receiver.uplinkLQ; - otaPkt.tlm_dl.ul_link_stats.SNR = receiver.snr; + otaPkt.tlm_dl.ul_link_stats.SNR = meanAccumulatorMean(&snrFilter, -16); #ifdef USE_MSP_OVER_TELEMETRY otaPkt.tlm_dl.ul_link_stats.mspConfirm = getCurrentMspConfirm() ? 1 : 0; #else @@ -518,6 +519,7 @@ static void tentativeConnection(const uint32_t timeStampMs) pl.offsetUs = 0; pl.previousRawOffsetUs = 0; expressLrsPhaseLockReset(); //also resets PFD + meanAccumulatorInit(&snrFilter); receiver.rfModeCycledAtMs = timeStampMs; // give another 3 sec for lock to occur // The caller MUST call hwTimer.resume(). It is not done here because @@ -742,6 +744,7 @@ rx_spi_received_e processRFPacket(volatile uint8_t *payload, uint32_t timeStampU // Store the LQ/RSSI/Antenna receiver.getRfLinkInfo(&receiver.rssi, &receiver.snr); + meanAccumulatorAdd(&snrFilter, receiver.snr); // Received a packet, that's the definition of LQ lqIncrease(); @@ -1089,7 +1092,7 @@ rx_spi_received_e expressLrsDataReceived(uint8_t *payloadBuffer) DEBUG_SET(DEBUG_RX_EXPRESSLRS_SPI, 0, lostConnectionCounter); DEBUG_SET(DEBUG_RX_EXPRESSLRS_SPI, 1, receiver.rssiFiltered); - DEBUG_SET(DEBUG_RX_EXPRESSLRS_SPI, 2, receiver.snr); + DEBUG_SET(DEBUG_RX_EXPRESSLRS_SPI, 2, receiver.snr / 4); DEBUG_SET(DEBUG_RX_EXPRESSLRS_SPI, 3, receiver.uplinkLQ); receiver.inBindingMode ? rxSpiLedBlinkBind() : rxSpiLedBlinkRxLoss(rfPacketStatus);