Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

SNR telemetry changes

Browse source
This commit is contained in:
phobos- 2022-08-01 17:22:31 +02:00
parent 8e4f6e0021
commit e9d7fe4445
6 changed files with 44 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

22
src/main/common/filter.c
View file

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

9
src/main/common/filter.h
View file

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

9
src/main/drivers/rx/rx_sx127x.c
View file

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

2
src/main/drivers/rx/rx_sx127x.h
View file

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

6
src/main/drivers/rx/rx_sx1280.c
View file

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

7
src/main/rx/expresslrs.c
View file

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