diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 472c1fdaaf..ca4bd1dd3a 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -511,7 +511,7 @@ static const char* const lookupTableFreqDomain[] = { "AU433", "AU915", "EU433", "EU868", "IN866", "FCC915", #endif #ifdef USE_RX_SX1280 - "ISM2400", + "ISM2400", "CE2400" #endif #if !defined(USE_RX_SX127X) && !defined(USE_RX_SX1280) "NONE", @@ -519,7 +519,7 @@ static const char* const lookupTableFreqDomain[] = { }; static const char* const lookupTableSwitchMode[] = { - "HYBRID", "WIDE", + "WIDE", "HYBRID", }; #endif diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 8e887aeb56..49df7747d7 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 meanAccumulatorCalc(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..876a556559 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 meanAccumulatorCalc(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 93815353e6..96db5b325b 100644 --- a/src/main/rx/expresslrs.c +++ b/src/main/rx/expresslrs.c @@ -76,8 +76,9 @@ static uint8_t bindingRateIndex = 0; static bool connectionHasModelMatch = false; 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]; @@ -192,12 +193,28 @@ static void setRssiChannelData(uint16_t *rcData) rcData[ELRS_RSSI_CHANNEL] = scaleRange(constrain(receiver.rssiFiltered, receiver.rfPerfParams->sensitivity, -50), receiver.rfPerfParams->sensitivity, -50, 988, 2011); } -static void unpackAnalogChannelData(uint16_t *rcData, const uint8_t *payload) +static void unpackAnalogChannelData(uint16_t *rcData, volatile elrsOtaPacket_t const * const otaPktPtr) { - rcData[0] = convertAnalog((payload[0] << 3) | ((payload[4] & 0xC0) >> 5)); - rcData[1] = convertAnalog((payload[1] << 3) | ((payload[4] & 0x30) >> 3)); - rcData[2] = convertAnalog((payload[2] << 3) | ((payload[4] & 0x0C) >> 1)); - rcData[3] = convertAnalog((payload[3] << 3) | ((payload[4] & 0x03) << 1)); + const uint8_t numOfChannels = 4; + const uint8_t srcBits = 10; + const uint16_t inputChannelMask = (1 << srcBits) - 1; + + uint8_t bitsMerged = 0; + uint32_t readValue = 0; + uint8_t readByteIndex = 0; + for (uint8_t n = 0; n < numOfChannels; n++) { + while (bitsMerged < srcBits) { + uint8_t readByte = otaPktPtr->rc.ch[readByteIndex++]; + readValue |= ((uint32_t) readByte) << bitsMerged; + bitsMerged += 8; + } + rcData[n] = 988 + (readValue & inputChannelMask); + readValue >>= srcBits; + bitsMerged -= srcBits; + } + + // The low latency switch + rcData[4] = convertSwitch1b(otaPktPtr->rc.ch4); } /** @@ -208,47 +225,23 @@ static void unpackAnalogChannelData(uint16_t *rcData, const uint8_t *payload) * * sets telemetry status bit */ -static void unpackChannelDataHybridSwitch8(uint16_t *rcData, const uint8_t *payload) +static void unpackChannelDataHybridSwitch8(uint16_t *rcData, volatile elrsOtaPacket_t const * const otaPktPtr) { - unpackAnalogChannelData(rcData, payload); + unpackAnalogChannelData(rcData, otaPktPtr); - const uint8_t switchByte = payload[5]; - - // The low latency switch - rcData[4] = convertSwitch1b((switchByte & 0x40) >> 6); + const uint8_t switchByte = otaPktPtr->rc.switches; // The round-robin switch, switchIndex is actually index-1 // to leave the low bit open for switch 7 (sent as 0b11x) // where x is the high bit of switch 7 uint8_t switchIndex = (switchByte & 0x38) >> 3; - uint16_t switchValue = convertSwitch3b(switchByte & 0x07); - switch (switchIndex) { - case 0: - rcData[5] = switchValue; - break; - case 1: - rcData[6] = switchValue; - break; - case 2: - rcData[7] = switchValue; - break; - case 3: - rcData[8] = switchValue; - break; - case 4: - rcData[9] = switchValue; - break; - case 5: - rcData[10] = switchValue; - break; - case 6: - FALLTHROUGH; - case 7: + if (switchIndex >= 6) { + // Because AUX1 (index 0) is the low latency switch, the low bit + // of the switchIndex can be used as data, and arrives as index "6" rcData[11] = convertSwitchNb(switchByte & 0x0F, 15); //4-bit - break; - default: - break; + } else { + rcData[5 + switchIndex] = convertSwitch3b(switchByte & 0x07); } setRssiChannelData(rcData); @@ -266,13 +259,10 @@ static void unpackChannelDataHybridSwitch8(uint16_t *rcData, const uint8_t *payl * Output: crsf.PackedRCdataOut, crsf.LinkStatistics.uplink_TX_Power * Returns: TelemetryStatus bit */ -static void unpackChannelDataHybridWide(uint16_t *rcData, const uint8_t *payload) +static void unpackChannelDataHybridWide(uint16_t *rcData, volatile elrsOtaPacket_t const * const otaPktPtr) { - unpackAnalogChannelData(rcData, payload); - const uint8_t switchByte = payload[5]; - - // The low latency switch - rcData[4] = convertSwitch1b((switchByte & 0x80) >> 7); + unpackAnalogChannelData(rcData, otaPktPtr); + const uint8_t switchByte = otaPktPtr->rc.switches; // The round-robin switch, 6-7 bits with the switch index implied by the nonce. Some logic moved to processRFPacket if (wideSwitchIndex >= 7) { @@ -280,7 +270,7 @@ static void unpackChannelDataHybridWide(uint16_t *rcData, const uint8_t *payload } else { uint8_t bins; uint16_t switchValue; - if (tlmRatioEnumToValue(receiver.modParams->tlmInterval) < 8) { + if (currTlmDenom < 8) { bins = 63; switchValue = switchByte & 0x3F; // 6-bit } else { @@ -288,8 +278,7 @@ static void unpackChannelDataHybridWide(uint16_t *rcData, const uint8_t *payload switchValue = switchByte & 0x7F; // 7-bit } - switchValue = convertSwitchNb(switchValue, bins); - rcData[5 + wideSwitchIndex] = switchValue; + rcData[5 + wideSwitchIndex] = convertSwitchNb(switchValue, bins); } setRssiChannelData(rcData); @@ -311,11 +300,18 @@ static uint8_t minLqForChaos(void) return interval * ((interval * numfhss + 99) / (interval * numfhss)); } +static bool domainIsTeam24() +{ + const elrsFreqDomain_e domain = rxExpressLrsSpiConfig()->domain; + return (domain == ISM2400) || (domain == CE2400); +} + static void setRfLinkRate(const uint8_t index) { #if defined(USE_RX_SX1280) && defined(USE_RX_SX127X) - receiver.modParams = (rxExpressLrsSpiConfig()->domain == ISM2400) ? &airRateConfig[1][index] : &airRateConfig[0][index]; - receiver.rfPerfParams = (rxExpressLrsSpiConfig()->domain == ISM2400) ? &rfPerfConfig[1][index] : &rfPerfConfig[0][index]; + const uint8_t domainIdx = domainIsTeam24() ? 1 : 0; + receiver.modParams = &airRateConfig[domainIdx][index]; + receiver.rfPerfParams = &rfPerfConfig[domainIdx][index]; #else receiver.modParams = &airRateConfig[0][index]; receiver.rfPerfParams = &rfPerfConfig[0][index]; @@ -325,6 +321,10 @@ static void setRfLinkRate(const uint8_t index) receiver.cycleIntervalMs = ((uint32_t)11U * fhssGetNumEntries() * receiver.modParams->fhssHopInterval * receiver.modParams->interval) / (10U * 1000U); receiver.config(receiver.modParams->bw, receiver.modParams->sf, receiver.modParams->cr, receiver.currentFreq, receiver.modParams->preambleLen, receiver.UID[5] & 0x01); +#if defined(USE_RX_SX1280) + if (rxExpressLrsSpiConfig()->domain == CE2400) + sx1280SetOutputPower(10); +#endif expressLrsUpdateTimerInterval(receiver.modParams->interval); @@ -333,7 +333,7 @@ static void setRfLinkRate(const uint8_t index) telemBurstValid = false; #ifdef USE_RX_LINK_QUALITY_INFO - rxSetRfMode((uint8_t)RATE_4HZ - (uint8_t)receiver.modParams->enumRate); + rxSetRfMode((uint8_t)receiver.modParams->enumRate); #endif } @@ -377,8 +377,8 @@ bool expressLrsIsFhssReq(void) bool expressLrsTelemRespReq(void) { - uint8_t modresult = (receiver.nonceRX + 1) % tlmRatioEnumToValue(receiver.modParams->tlmInterval); - if (receiver.inBindingMode || (receiver.connectionState == ELRS_DISCONNECTED) || (receiver.modParams->tlmInterval == TLM_RATIO_NO_TLM) || (modresult != 0)) { + uint8_t modresult = (receiver.nonceRX + 1) % currTlmDenom; + if (receiver.inBindingMode || (receiver.connectionState == ELRS_DISCONNECTED) || (currTlmDenom == 1) || (modresult != 0)) { return false; // don't bother sending tlm if disconnected or TLM is off } else { return true; @@ -387,23 +387,23 @@ bool expressLrsTelemRespReq(void) static void expressLrsSendTelemResp(void) { - uint8_t *data; - uint8_t maxLength; - uint8_t packageIndex; + elrsOtaPacket_t otaPkt = {0}; receiver.alreadyTelemResp = true; - telemetryPacket[0] = ELRS_TLM_PACKET; + otaPkt.type = ELRS_TLM_PACKET; if (nextTelemetryType == ELRS_TELEMETRY_TYPE_LINK || !isTelemetrySenderActive()) { - telemetryPacket[1] = ELRS_TELEMETRY_TYPE_LINK; - telemetryPacket[2] = receiver.rssiFiltered > 0 ? 0 : -receiver.rssiFiltered; //diversity not supported - telemetryPacket[3] = connectionHasModelMatch << 7; - telemetryPacket[4] = receiver.snr; - telemetryPacket[5] = receiver.uplinkLQ; + otaPkt.tlm_dl.type = ELRS_TELEMETRY_TYPE_LINK; + otaPkt.tlm_dl.ul_link_stats.uplink_RSSI_1 = receiver.rssiFiltered > 0 ? 0 : -receiver.rssiFiltered; + otaPkt.tlm_dl.ul_link_stats.uplink_RSSI_2 = 0; //diversity not supported + 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 = meanAccumulatorCalc(&snrFilter, -16); #ifdef USE_MSP_OVER_TELEMETRY - telemetryPacket[6] = getCurrentMspConfirm() ? 1 : 0; + otaPkt.tlm_dl.ul_link_stats.mspConfirm = getCurrentMspConfirm() ? 1 : 0; #else - telemetryPacket[6] = 0; + otaPkt.tlm_dl.ul_link_stats.mspConfirm = 0; #endif nextTelemetryType = ELRS_TELEMETRY_TYPE_DATA; // Start the count at 1 because the next will be DATA and doing +1 before checking @@ -416,18 +416,14 @@ static void expressLrsSendTelemResp(void) nextTelemetryType = ELRS_TELEMETRY_TYPE_LINK; } - getCurrentTelemetryPayload(&packageIndex, &maxLength, &data); - telemetryPacket[1] = (packageIndex << ELRS_TELEMETRY_SHIFT) + ELRS_TELEMETRY_TYPE_DATA; - telemetryPacket[2] = maxLength > 0 ? *data : 0; - telemetryPacket[3] = maxLength >= 1 ? *(data + 1) : 0; - telemetryPacket[4] = maxLength >= 2 ? *(data + 2) : 0; - telemetryPacket[5] = maxLength >= 3 ? *(data + 3) : 0; - telemetryPacket[6] = maxLength >= 4 ? *(data + 4) : 0; + otaPkt.tlm_dl.type = ELRS_TELEMETRY_TYPE_DATA; + otaPkt.tlm_dl.packageIndex = getCurrentTelemetryPayload(otaPkt.tlm_dl.payload); } - uint16_t crc = calcCrc14((uint8_t *)telemetryPacket, 7, crcInitializer); - telemetryPacket[0] |= (crc >> 6) & 0xFC; - telemetryPacket[7] = crc & 0xFF; + uint16_t crc = calcCrc14((uint8_t *) &otaPkt, 7, crcInitializer); + otaPkt.crcHigh = (crc >> 8); + otaPkt.crcLow = crc; + memcpy((uint8_t *) telemetryPacket, (uint8_t *) &otaPkt, ELRS_RX_TX_BUFF_SIZE); } static void updatePhaseLock(void) @@ -439,8 +435,10 @@ static void updatePhaseLock(void) pl.offsetUs = simpleLPFilterUpdate(&pl.offsetFilter, pl.rawOffsetUs); pl.offsetDeltaUs = simpleLPFilterUpdate(&pl.offsetDxFilter, pl.rawOffsetUs - pl.previousRawOffsetUs); - if (receiver.timerState == ELRS_TIM_LOCKED && lqPeriodIsSet()) { - if (receiver.nonceRX % 8 == 0) { //limit rate of freq offset adjustment slightly + if (receiver.timerState == ELRS_TIM_LOCKED) { + // limit rate of freq offset adjustment, use slot 1 because + // telemetry can fall on slot 1 and will never get here + if (receiver.nonceRX % 8 == 1) { if (pl.offsetUs > 0) { expressLrsTimerIncreaseFrequencyOffset(); } else if (pl.offsetUs < 0) { @@ -534,6 +532,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 @@ -594,13 +593,14 @@ static void initializeReceiver(void) static void unpackBindPacket(volatile uint8_t *packet) { - rxExpressLrsSpiConfigMutable()->UID[2] = packet[3]; - rxExpressLrsSpiConfigMutable()->UID[3] = packet[4]; - rxExpressLrsSpiConfigMutable()->UID[4] = packet[5]; - rxExpressLrsSpiConfigMutable()->UID[5] = packet[6]; + rxExpressLrsSpiConfigMutable()->UID[2] = packet[0]; + rxExpressLrsSpiConfigMutable()->UID[3] = packet[1]; + rxExpressLrsSpiConfigMutable()->UID[4] = packet[2]; + rxExpressLrsSpiConfigMutable()->UID[5] = packet[3]; receiver.UID = rxExpressLrsSpiConfigMutable()->UID; crcInitializer = (receiver.UID[4] << 8) | receiver.UID[5]; + crcInitializer ^= ELRS_OTA_VERSION_ID; receiver.inBindingMode = false; receiver.configChanged = true; //after initialize as it sets it to false } @@ -608,12 +608,12 @@ static void unpackBindPacket(volatile uint8_t *packet) /** * Process the assembled MSP packet in mspBuffer[] **/ -static void processRFMspPacket(volatile uint8_t *packet) +static void processRFMspPacket(volatile elrsOtaPacket_t const * const otaPktPtr) { // Always examine MSP packets for bind information if in bind mode // [1] is the package index, first packet of the MSP - if (receiver.inBindingMode && packet[1] == 1 && packet[2] == ELRS_MSP_BIND) { - unpackBindPacket(packet); //onELRSBindMSP + if (receiver.inBindingMode && otaPktPtr->msp_ul.packageIndex == 1 && otaPktPtr->msp_ul.payload[0] == ELRS_MSP_BIND) { + unpackBindPacket((uint8_t *) &otaPktPtr->msp_ul.payload[1]); //onELRSBindMSP return; } @@ -625,7 +625,7 @@ static void processRFMspPacket(volatile uint8_t *packet) } bool currentMspConfirmValue = getCurrentMspConfirm(); - receiveMspData(packet[1], packet + 2); + receiveMspData(otaPktPtr->msp_ul.packageIndex, otaPktPtr->msp_ul.payload); if (currentMspConfirmValue != getCurrentMspConfirm()) { nextTelemetryType = ELRS_TELEMETRY_TYPE_LINK; } @@ -645,26 +645,25 @@ static void processRFMspPacket(volatile uint8_t *packet) #endif } -static bool processRFSyncPacket(volatile uint8_t *packet, const uint32_t timeStampMs) +static bool processRFSyncPacket(volatile elrsOtaPacket_t const * const otaPktPtr, const uint32_t timeStampMs) { // Verify the first two of three bytes of the binding ID, which should always match - if (packet[4] != receiver.UID[3] || packet[5] != receiver.UID[4]) { + if (otaPktPtr->sync.UID3 != receiver.UID[3] || otaPktPtr->sync.UID4 != receiver.UID[4]) { return false; } // The third byte will be XORed with inverse of the ModelId if ModelMatch is on // Only require the first 18 bits of the UID to match to establish a connection // but the last 6 bits must modelmatch before sending any data to the FC - if ((packet[6] & ~ELRS_MODELMATCH_MASK) != (receiver.UID[5] & ~ELRS_MODELMATCH_MASK)) { + if ((otaPktPtr->sync.UID5 & ~ELRS_MODELMATCH_MASK) != (receiver.UID[5] & ~ELRS_MODELMATCH_MASK)) { return false; } receiver.lastSyncPacketMs = timeStampMs; // Will change the packet air rate in loop() if this changes - receiver.nextRateIndex = (packet[3] & 0xC0) >> 6; - uint8_t tlmRateIn = (packet[3] & 0x38) >> 3; - uint8_t switchEncMode = ((packet[3] & 0x06) >> 1) - 1; //spi implementation uses 0 based index for hybrid + receiver.nextRateIndex = domainIsTeam24() ? airRateIndexToIndex24(otaPktPtr->sync.rateIndex, receiver.rateIndex) : airRateIndexToIndex900(otaPktPtr->sync.rateIndex, receiver.rateIndex); + uint8_t switchEncMode = otaPktPtr->sync.switchEncMode; // Update switch mode encoding immediately if (switchEncMode != rxExpressLrsSpiConfig()->switchMode) { @@ -673,18 +672,20 @@ static bool processRFSyncPacket(volatile uint8_t *packet, const uint32_t timeSta } // Update TLM ratio - if (receiver.modParams->tlmInterval != tlmRateIn) { - receiver.modParams->tlmInterval = tlmRateIn; + uint8_t tlmRateIn = otaPktPtr->sync.newTlmRatio + TLM_RATIO_NO_TLM; + uint8_t tlmDenom = tlmRatioEnumToValue(tlmRateIn); + if (currTlmDenom != tlmDenom) { + currTlmDenom = tlmDenom; telemBurstValid = false; } // modelId = 0xff indicates modelMatch is disabled, the XOR does nothing in that case uint8_t modelXor = (~rxExpressLrsSpiConfig()->modelId) & ELRS_MODELMATCH_MASK; - bool modelMatched = packet[6] == (receiver.UID[5] ^ modelXor); + bool modelMatched = otaPktPtr->sync.UID5 == (receiver.UID[5] ^ modelXor); - if (receiver.connectionState == ELRS_DISCONNECTED || receiver.nonceRX != packet[2] || fhssGetCurrIndex() != packet[1] || connectionHasModelMatch != modelMatched) { - fhssSetCurrIndex(packet[1]); - receiver.nonceRX = packet[2]; + if (receiver.connectionState == ELRS_DISCONNECTED || receiver.nonceRX != otaPktPtr->sync.nonce || fhssGetCurrIndex() != otaPktPtr->sync.fhssIndex || connectionHasModelMatch != modelMatched) { + fhssSetCurrIndex(otaPktPtr->sync.fhssIndex); + receiver.nonceRX = otaPktPtr->sync.nonce; tentativeConnection(timeStampMs); connectionHasModelMatch = modelMatched; @@ -697,22 +698,25 @@ static bool processRFSyncPacket(volatile uint8_t *packet, const uint32_t timeSta return false; } +static bool validatePacketCrcStd(volatile elrsOtaPacket_t * const otaPktPtr) +{ + uint16_t const inCRC = ((uint16_t) otaPktPtr->crcHigh << 8) + otaPktPtr->crcLow; + // For smHybrid the CRC only has the packet type in byte 0 + // For smWide the FHSS slot is added to the CRC in byte 0 on PACKET_TYPE_RCDATAs + if (otaPktPtr->type == ELRS_RC_DATA_PACKET && rxExpressLrsSpiConfig()->switchMode == SM_WIDE) { + otaPktPtr->crcHigh = (receiver.nonceRX % receiver.modParams->fhssHopInterval) + 1; + } else { + otaPktPtr->crcHigh = 0; + } + uint16_t const calculatedCRC = calcCrc14((uint8_t *) otaPktPtr, 7, crcInitializer); + return inCRC == calculatedCRC; +} + rx_spi_received_e processRFPacket(volatile uint8_t *payload, uint32_t timeStampUs) { - elrsPacketType_e type = dmaBuffer[0] & 0x03; - uint16_t inCRC = (((uint16_t)(dmaBuffer[0] & 0xFC)) << 6 ) | dmaBuffer[7]; + volatile elrsOtaPacket_t * const otaPktPtr = (elrsOtaPacket_t * const) dmaBuffer; - // For SM_HYBRID the CRC only has the packet type in byte 0 - // For SM_HYBRID_WIDE the FHSS slot is added to the CRC in byte 0 on RC_DATA_PACKETs - if (type != ELRS_RC_DATA_PACKET || rxExpressLrsSpiConfig()->switchMode != SM_HYBRID_WIDE) { - dmaBuffer[0] = type; - } else { - uint8_t nonceFHSSresult = receiver.nonceRX % receiver.modParams->fhssHopInterval; - dmaBuffer[0] = type | (nonceFHSSresult << 2); - } - uint16_t calculatedCRC = calcCrc14((uint8_t *)dmaBuffer, 7, crcInitializer); - - if (inCRC != calculatedCRC) { + if (!validatePacketCrcStd(otaPktPtr)) { return RX_SPI_RECEIVED_NONE; } @@ -723,30 +727,30 @@ rx_spi_received_e processRFPacket(volatile uint8_t *payload, uint32_t timeStampU receiver.lastValidPacketMs = timeStampMs; - switch(type) { + switch(otaPktPtr->type) { case ELRS_RC_DATA_PACKET: // Must be fully connected to process RC packets, prevents processing RC // during sync, where packets can be received before connection if (receiver.connectionState == ELRS_CONNECTED && connectionHasModelMatch) { - if (rxExpressLrsSpiConfig()->switchMode == SM_HYBRID_WIDE) { + if (rxExpressLrsSpiConfig()->switchMode == SM_WIDE) { wideSwitchIndex = hybridWideNonceToSwitchIndex(receiver.nonceRX); - if ((tlmRatioEnumToValue(receiver.modParams->tlmInterval) < 8) || wideSwitchIndex == 7) { - confirmCurrentTelemetryPayload((dmaBuffer[6] & 0x40) >> 6); + if ((currTlmDenom < 8) || wideSwitchIndex == 7) { + confirmCurrentTelemetryPayload((otaPktPtr->rc.switches & 0x40) >> 6); } } else { - confirmCurrentTelemetryPayload(dmaBuffer[6] & (1 << 7)); + confirmCurrentTelemetryPayload(otaPktPtr->rc.switches & (1 << 6)); } - memcpy((uint8_t *)payload, (uint8_t *)&dmaBuffer[1], 6); // stick data handling is done in expressLrsSetRcDataFromPayload + memcpy((uint8_t *) payload, (uint8_t *) dmaBuffer, ELRS_RX_TX_BUFF_SIZE); // stick data handling is done in expressLrsSetRcDataFromPayload } break; case ELRS_MSP_DATA_PACKET: - processRFMspPacket(dmaBuffer); + processRFMspPacket(otaPktPtr); break; case ELRS_TLM_PACKET: //not implemented break; case ELRS_SYNC_PACKET: - shouldStartTimer = processRFSyncPacket(dmaBuffer, timeStampMs) && !receiver.inBindingMode; + shouldStartTimer = processRFSyncPacket(otaPktPtr, timeStampMs) && !receiver.inBindingMode; break; default: return RX_SPI_RECEIVED_NONE; @@ -754,6 +758,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(); @@ -776,8 +781,7 @@ static void updateTelemetryBurst(void) telemBurstValid = true; uint32_t hz = rateEnumToHz(receiver.modParams->enumRate); - uint32_t ratiodiv = tlmRatioEnumToValue(receiver.modParams->tlmInterval); - telemetryBurstMax = TELEM_MIN_LINK_INTERVAL * hz / ratiodiv / 1000U; + telemetryBurstMax = TELEM_MIN_LINK_INTERVAL * hz / currTlmDenom / 1000U; // Reserve one slot for LINK telemetry if (telemetryBurstMax > 1) { @@ -787,7 +791,7 @@ static void updateTelemetryBurst(void) } // Notify the sender to adjust its expected throughput - updateTelemetryRate(hz, ratiodiv, telemetryBurstMax); + updateTelemetryRate(hz, currTlmDenom, telemetryBurstMax); } /* If not connected will rotate through the RF modes looking for sync @@ -884,6 +888,8 @@ bool expressLrsSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeSta #endif #ifdef USE_RX_SX1280 case ISM2400: + FALLTHROUGH; + case CE2400: configureReceiverForSX1280(); bindingRateIndex = ELRS_BINDING_RATE_24; break; @@ -910,6 +916,7 @@ bool expressLrsSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeSta receiver.inBindingMode = false; receiver.UID = rxExpressLrsSpiConfig()->UID; crcInitializer = (receiver.UID[4] << 8) | receiver.UID[5]; + crcInitializer ^= ELRS_OTA_VERSION_ID; } else { receiver.inBindingMode = true; receiver.UID = BindingUID; @@ -926,7 +933,7 @@ bool expressLrsSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeSta initTelemetry(); #ifdef USE_MSP_OVER_TELEMETRY - setMspDataToReceive(ELRS_MSP_BUFFER_SIZE, mspBuffer, ELRS_MSP_BYTES_PER_CALL); + setMspDataToReceive(ELRS_MSP_BUFFER_SIZE, mspBuffer); #endif // Timer IRQs must only be enabled after the receiver is configured otherwise race conditions occur. @@ -1039,7 +1046,7 @@ void expressLrsHandleTelemetryUpdate(void) uint8_t *nextPayload = 0; uint8_t nextPlayloadSize = 0; if (!isTelemetrySenderActive() && getNextTelemetryPayload(&nextPlayloadSize, &nextPayload)) { - setTelemetryDataToTransmit(nextPlayloadSize, nextPayload, ELRS_TELEMETRY_BYTES_PER_CALL); + setTelemetryDataToTransmit(nextPlayloadSize, nextPayload); } updateTelemetryBurst(); } @@ -1047,7 +1054,8 @@ void expressLrsHandleTelemetryUpdate(void) void expressLrsSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload) { if (rcData && payload) { - rxExpressLrsSpiConfig()->switchMode == SM_HYBRID_WIDE ? unpackChannelDataHybridWide(rcData, payload) : unpackChannelDataHybridSwitch8(rcData, payload); + volatile elrsOtaPacket_t * const otaPktPtr = (elrsOtaPacket_t * const) payload; + rxExpressLrsSpiConfig()->switchMode == SM_WIDE ? unpackChannelDataHybridWide(rcData, otaPktPtr) : unpackChannelDataHybridSwitch8(rcData, otaPktPtr); } } @@ -1067,7 +1075,7 @@ void expressLrsDoTelem(void) expressLrsHandleTelemetryUpdate(); expressLrsSendTelemResp(); - if (rxExpressLrsSpiConfig()->domain != ISM2400 && !receiver.didFhss && !expressLrsTelemRespReq() && lqPeriodIsSet()) { + if (!domainIsTeam24() && !receiver.didFhss && !expressLrsTelemRespReq() && lqPeriodIsSet()) { // TODO No need to handle this on SX1280, but will on SX127x // TODO this needs to be DMA aswell, SX127x unlikely to work right now receiver.handleFreqCorrection(receiver.freqOffset, receiver.currentFreq); //corrects for RX freq offset @@ -1101,7 +1109,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); diff --git a/src/main/rx/expresslrs_common.c b/src/main/rx/expresslrs_common.c index 7448272e7f..c42b480176 100644 --- a/src/main/rx/expresslrs_common.c +++ b/src/main/rx/expresslrs_common.c @@ -40,10 +40,9 @@ STATIC_UNIT_TESTED uint16_t crc14tab[ELRS_CRC_LEN] = {0}; static uint8_t volatile fhssIndex = 0; STATIC_UNIT_TESTED uint8_t fhssSequence[ELRS_NR_SEQUENCE_ENTRIES] = {0}; -static const uint32_t *fhssFreqs; -static uint8_t numFreqs = 0; // The number of FHSS frequencies in the table -static uint8_t seqCount = 0; +static uint16_t seqCount = 0; static uint8_t syncChannel = 0; +static uint32_t freqSpread = 0; #define MS_TO_US(ms) (ms * 1000) @@ -54,18 +53,18 @@ static uint8_t syncChannel = 0; elrsModSettings_t airRateConfig[][ELRS_RATE_MAX] = { #ifdef USE_RX_SX127X { - {0, RATE_200HZ, SX127x_BW_500_00_KHZ, SX127x_SF_6, SX127x_CR_4_7, 5000, TLM_RATIO_1_64, 4, 8}, - {1, RATE_100HZ, SX127x_BW_500_00_KHZ, SX127x_SF_7, SX127x_CR_4_7, 10000, TLM_RATIO_1_64, 4, 8}, - {2, RATE_50HZ, SX127x_BW_500_00_KHZ, SX127x_SF_8, SX127x_CR_4_7, 20000, TLM_RATIO_NO_TLM, 4, 10}, - {3, RATE_25HZ, SX127x_BW_500_00_KHZ, SX127x_SF_9, SX127x_CR_4_7, 40000, TLM_RATIO_NO_TLM, 2, 10} + {0, RATE_LORA_200HZ, SX127x_BW_500_00_KHZ, SX127x_SF_6, SX127x_CR_4_7, 5000, TLM_RATIO_1_64, 4, 8}, + {1, RATE_LORA_100HZ, SX127x_BW_500_00_KHZ, SX127x_SF_7, SX127x_CR_4_7, 10000, TLM_RATIO_1_64, 4, 8}, + {2, RATE_LORA_50HZ, SX127x_BW_500_00_KHZ, SX127x_SF_8, SX127x_CR_4_7, 20000, TLM_RATIO_1_16, 4, 10}, + {3, RATE_LORA_25HZ, SX127x_BW_500_00_KHZ, SX127x_SF_9, SX127x_CR_4_7, 40000, TLM_RATIO_1_8, 2, 10} }, #endif #ifdef USE_RX_SX1280 { - {0, RATE_500HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF5, SX1280_LORA_CR_LI_4_6, 2000, TLM_RATIO_1_128, 4, 12}, - {1, RATE_250HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF6, SX1280_LORA_CR_LI_4_7, 4000, TLM_RATIO_1_64, 4, 14}, - {2, RATE_150HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF7, SX1280_LORA_CR_LI_4_7, 6666, TLM_RATIO_1_32, 4, 12}, - {3, RATE_50HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF9, SX1280_LORA_CR_LI_4_6, 20000, TLM_RATIO_NO_TLM, 2, 12} + {0, RATE_LORA_500HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF5, SX1280_LORA_CR_LI_4_6, 2000, TLM_RATIO_1_128, 4, 12}, + {1, RATE_LORA_250HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF6, SX1280_LORA_CR_LI_4_7, 4000, TLM_RATIO_1_64, 4, 14}, + {2, RATE_LORA_150HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF7, SX1280_LORA_CR_LI_4_7, 6666, TLM_RATIO_1_32, 4, 12}, + {3, RATE_LORA_50HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF8, SX1280_LORA_CR_LI_4_7, 20000, TLM_RATIO_1_16, 2, 12} }, #endif #if !defined(USE_RX_SX127X) && !defined(USE_RX_SX1280) @@ -76,18 +75,18 @@ elrsModSettings_t airRateConfig[][ELRS_RATE_MAX] = { elrsRfPerfParams_t rfPerfConfig[][ELRS_RATE_MAX] = { #ifdef USE_RX_SX127X { - {0, RATE_200HZ, -112, 4380, 3000, 2500, 600, 5000}, - {1, RATE_100HZ, -117, 8770, 3500, 2500, 600, 5000}, - {2, RATE_50HZ, -120, 17540, 4000, 2500, 600, 5000}, - {3, RATE_25HZ, -123, 17540, 6000, 4000, 0, 5000} + {0, RATE_LORA_200HZ, -112, 4380, 3000, 2500, 600, 5000}, + {1, RATE_LORA_100HZ, -117, 8770, 3500, 2500, 600, 5000}, + {2, RATE_LORA_50HZ, -120, 17540, 4000, 2500, 600, 5000}, + {3, RATE_LORA_25HZ, -123, 17540, 6000, 4000, 0, 5000} }, #endif #ifdef USE_RX_SX1280 { - {0, RATE_500HZ, -105, 1665, 2500, 2500, 3, 5000}, - {1, RATE_250HZ, -108, 3300, 3000, 2500, 6, 5000}, - {2, RATE_150HZ, -112, 5871, 3500, 2500, 10, 5000}, - {3, RATE_50HZ, -117, 18443, 4000, 2500, 0, 5000} + {0, RATE_LORA_500HZ, -105, 1665, 2500, 2500, 3, 5000}, + {1, RATE_LORA_250HZ, -108, 3300, 3000, 2500, 6, 5000}, + {2, RATE_LORA_150HZ, -112, 5871, 3500, 2500, 10, 5000}, + {3, RATE_LORA_50HZ, -115, 10798, 4000, 2500, 0, 5000} }, #endif #if !defined(USE_RX_SX127X) && !defined(USE_RX_SX1280) @@ -95,234 +94,25 @@ elrsRfPerfParams_t rfPerfConfig[][ELRS_RATE_MAX] = { #endif }; +const elrsFhssConfig_t fhssConfigs[] = { #ifdef USE_RX_SX127X -const uint32_t fhssFreqsAU433[] = { - FREQ_HZ_TO_REG_VAL_900(433420000), - FREQ_HZ_TO_REG_VAL_900(433920000), - FREQ_HZ_TO_REG_VAL_900(434420000)}; - -const uint32_t fhssFreqsAU915[] = { - FREQ_HZ_TO_REG_VAL_900(915500000), - FREQ_HZ_TO_REG_VAL_900(916100000), - FREQ_HZ_TO_REG_VAL_900(916700000), - FREQ_HZ_TO_REG_VAL_900(917300000), - - FREQ_HZ_TO_REG_VAL_900(917900000), - FREQ_HZ_TO_REG_VAL_900(918500000), - FREQ_HZ_TO_REG_VAL_900(919100000), - FREQ_HZ_TO_REG_VAL_900(919700000), - - FREQ_HZ_TO_REG_VAL_900(920300000), - FREQ_HZ_TO_REG_VAL_900(920900000), - FREQ_HZ_TO_REG_VAL_900(921500000), - FREQ_HZ_TO_REG_VAL_900(922100000), - - FREQ_HZ_TO_REG_VAL_900(922700000), - FREQ_HZ_TO_REG_VAL_900(923300000), - FREQ_HZ_TO_REG_VAL_900(923900000), - FREQ_HZ_TO_REG_VAL_900(924500000), - - FREQ_HZ_TO_REG_VAL_900(925100000), - FREQ_HZ_TO_REG_VAL_900(925700000), - FREQ_HZ_TO_REG_VAL_900(926300000), - FREQ_HZ_TO_REG_VAL_900(926900000)}; - -/* Frequency bands taken from https://wetten.overheid.nl/BWBR0036378/2016-12-28#Bijlagen - * Note: these frequencies fall in the license free H-band, but in combination with 500kHz - * LoRa modem bandwidth used by ExpressLRS (EU allows up to 125kHz modulation BW only) they - * will never pass RED certification and they are ILLEGAL to use. - * - * Therefore we simply maximize the usage of available spectrum so laboratory testing of the software won't disturb existing - * 868MHz ISM band traffic too much. - */ -const uint32_t fhssFreqsEU868[] = { - FREQ_HZ_TO_REG_VAL_900(863275000), // band H1, 863 - 865MHz, 0.1% duty cycle or CSMA techniques, 25mW EIRP - FREQ_HZ_TO_REG_VAL_900(863800000), - FREQ_HZ_TO_REG_VAL_900(864325000), - FREQ_HZ_TO_REG_VAL_900(864850000), - FREQ_HZ_TO_REG_VAL_900(865375000), // Band H2, 865 - 868.6MHz, 1.0% dutycycle or CSMA, 25mW EIRP - FREQ_HZ_TO_REG_VAL_900(865900000), - FREQ_HZ_TO_REG_VAL_900(866425000), - FREQ_HZ_TO_REG_VAL_900(866950000), - FREQ_HZ_TO_REG_VAL_900(867475000), - FREQ_HZ_TO_REG_VAL_900(868000000), - FREQ_HZ_TO_REG_VAL_900(868525000), // Band H3, 868.7-869.2MHz, 0.1% dutycycle or CSMA, 25mW EIRP - FREQ_HZ_TO_REG_VAL_900(869050000), - FREQ_HZ_TO_REG_VAL_900(869575000)}; - -/** - * India currently delicensed the 865-867 MHz band with a maximum of 1W Transmitter power, - * 4Watts Effective Radiated Power and 200Khz carrier bandwidth as per - * https://dot.gov.in/sites/default/files/Delicensing%20in%20865-867%20MHz%20band%20%5BGSR%20564%20%28E%29%5D_0.pdf . - * There is currently no mention of Direct-sequence spread spectrum, - * So these frequencies are a subset of Regulatory_Domain_EU_868 frequencies. - */ -const uint32_t fhssFreqsIN866[] = { - FREQ_HZ_TO_REG_VAL_900(865375000), - FREQ_HZ_TO_REG_VAL_900(865900000), - FREQ_HZ_TO_REG_VAL_900(866425000), - FREQ_HZ_TO_REG_VAL_900(866950000)}; - -/* Frequency band G, taken from https://wetten.overheid.nl/BWBR0036378/2016-12-28#Bijlagen - * Note: As is the case with the 868Mhz band, these frequencies only comply to the license free portion - * of the spectrum, nothing else. As such, these are likely illegal to use. - */ -const uint32_t fhssFreqsEU433[] = { - FREQ_HZ_TO_REG_VAL_900(433100000), - FREQ_HZ_TO_REG_VAL_900(433925000), - FREQ_HZ_TO_REG_VAL_900(434450000)}; - -/* Very definitely not fully checked. An initial pass at increasing the hops -*/ -const uint32_t fhssFreqsFCC915[] = { - FREQ_HZ_TO_REG_VAL_900(903500000), - FREQ_HZ_TO_REG_VAL_900(904100000), - FREQ_HZ_TO_REG_VAL_900(904700000), - FREQ_HZ_TO_REG_VAL_900(905300000), - - FREQ_HZ_TO_REG_VAL_900(905900000), - FREQ_HZ_TO_REG_VAL_900(906500000), - FREQ_HZ_TO_REG_VAL_900(907100000), - FREQ_HZ_TO_REG_VAL_900(907700000), - - FREQ_HZ_TO_REG_VAL_900(908300000), - FREQ_HZ_TO_REG_VAL_900(908900000), - FREQ_HZ_TO_REG_VAL_900(909500000), - FREQ_HZ_TO_REG_VAL_900(910100000), - - FREQ_HZ_TO_REG_VAL_900(910700000), - FREQ_HZ_TO_REG_VAL_900(911300000), - FREQ_HZ_TO_REG_VAL_900(911900000), - FREQ_HZ_TO_REG_VAL_900(912500000), - - FREQ_HZ_TO_REG_VAL_900(913100000), - FREQ_HZ_TO_REG_VAL_900(913700000), - FREQ_HZ_TO_REG_VAL_900(914300000), - FREQ_HZ_TO_REG_VAL_900(914900000), - - FREQ_HZ_TO_REG_VAL_900(915500000), // as per AU.. - FREQ_HZ_TO_REG_VAL_900(916100000), - FREQ_HZ_TO_REG_VAL_900(916700000), - FREQ_HZ_TO_REG_VAL_900(917300000), - - FREQ_HZ_TO_REG_VAL_900(917900000), - FREQ_HZ_TO_REG_VAL_900(918500000), - FREQ_HZ_TO_REG_VAL_900(919100000), - FREQ_HZ_TO_REG_VAL_900(919700000), - - FREQ_HZ_TO_REG_VAL_900(920300000), - FREQ_HZ_TO_REG_VAL_900(920900000), - FREQ_HZ_TO_REG_VAL_900(921500000), - FREQ_HZ_TO_REG_VAL_900(922100000), - - FREQ_HZ_TO_REG_VAL_900(922700000), - FREQ_HZ_TO_REG_VAL_900(923300000), - FREQ_HZ_TO_REG_VAL_900(923900000), - FREQ_HZ_TO_REG_VAL_900(924500000), - - FREQ_HZ_TO_REG_VAL_900(925100000), - FREQ_HZ_TO_REG_VAL_900(925700000), - FREQ_HZ_TO_REG_VAL_900(926300000), - FREQ_HZ_TO_REG_VAL_900(926900000)}; + {AU433, FREQ_HZ_TO_REG_VAL_900(433420000), FREQ_HZ_TO_REG_VAL_900(434420000), 3}, + {AU915, FREQ_HZ_TO_REG_VAL_900(915500000), FREQ_HZ_TO_REG_VAL_900(926900000), 20}, + {EU433, FREQ_HZ_TO_REG_VAL_900(433100000), FREQ_HZ_TO_REG_VAL_900(434450000), 3}, + {EU868, FREQ_HZ_TO_REG_VAL_900(865275000), FREQ_HZ_TO_REG_VAL_900(869575000), 13}, + {IN866, FREQ_HZ_TO_REG_VAL_900(865375000), FREQ_HZ_TO_REG_VAL_900(866950000), 4}, + {FCC915, FREQ_HZ_TO_REG_VAL_900(903500000), FREQ_HZ_TO_REG_VAL_900(926900000), 40}, #endif #ifdef USE_RX_SX1280 -const uint32_t fhssFreqsISM2400[] = { - FREQ_HZ_TO_REG_VAL_24(2400400000), - FREQ_HZ_TO_REG_VAL_24(2401400000), - FREQ_HZ_TO_REG_VAL_24(2402400000), - FREQ_HZ_TO_REG_VAL_24(2403400000), - FREQ_HZ_TO_REG_VAL_24(2404400000), - - FREQ_HZ_TO_REG_VAL_24(2405400000), - FREQ_HZ_TO_REG_VAL_24(2406400000), - FREQ_HZ_TO_REG_VAL_24(2407400000), - FREQ_HZ_TO_REG_VAL_24(2408400000), - FREQ_HZ_TO_REG_VAL_24(2409400000), - - FREQ_HZ_TO_REG_VAL_24(2410400000), - FREQ_HZ_TO_REG_VAL_24(2411400000), - FREQ_HZ_TO_REG_VAL_24(2412400000), - FREQ_HZ_TO_REG_VAL_24(2413400000), - FREQ_HZ_TO_REG_VAL_24(2414400000), - - FREQ_HZ_TO_REG_VAL_24(2415400000), - FREQ_HZ_TO_REG_VAL_24(2416400000), - FREQ_HZ_TO_REG_VAL_24(2417400000), - FREQ_HZ_TO_REG_VAL_24(2418400000), - FREQ_HZ_TO_REG_VAL_24(2419400000), - - FREQ_HZ_TO_REG_VAL_24(2420400000), - FREQ_HZ_TO_REG_VAL_24(2421400000), - FREQ_HZ_TO_REG_VAL_24(2422400000), - FREQ_HZ_TO_REG_VAL_24(2423400000), - FREQ_HZ_TO_REG_VAL_24(2424400000), - - FREQ_HZ_TO_REG_VAL_24(2425400000), - FREQ_HZ_TO_REG_VAL_24(2426400000), - FREQ_HZ_TO_REG_VAL_24(2427400000), - FREQ_HZ_TO_REG_VAL_24(2428400000), - FREQ_HZ_TO_REG_VAL_24(2429400000), - - FREQ_HZ_TO_REG_VAL_24(2430400000), - FREQ_HZ_TO_REG_VAL_24(2431400000), - FREQ_HZ_TO_REG_VAL_24(2432400000), - FREQ_HZ_TO_REG_VAL_24(2433400000), - FREQ_HZ_TO_REG_VAL_24(2434400000), - - FREQ_HZ_TO_REG_VAL_24(2435400000), - FREQ_HZ_TO_REG_VAL_24(2436400000), - FREQ_HZ_TO_REG_VAL_24(2437400000), - FREQ_HZ_TO_REG_VAL_24(2438400000), - FREQ_HZ_TO_REG_VAL_24(2439400000), - - FREQ_HZ_TO_REG_VAL_24(2440400000), - FREQ_HZ_TO_REG_VAL_24(2441400000), - FREQ_HZ_TO_REG_VAL_24(2442400000), - FREQ_HZ_TO_REG_VAL_24(2443400000), - FREQ_HZ_TO_REG_VAL_24(2444400000), - - FREQ_HZ_TO_REG_VAL_24(2445400000), - FREQ_HZ_TO_REG_VAL_24(2446400000), - FREQ_HZ_TO_REG_VAL_24(2447400000), - FREQ_HZ_TO_REG_VAL_24(2448400000), - FREQ_HZ_TO_REG_VAL_24(2449400000), - - FREQ_HZ_TO_REG_VAL_24(2450400000), - FREQ_HZ_TO_REG_VAL_24(2451400000), - FREQ_HZ_TO_REG_VAL_24(2452400000), - FREQ_HZ_TO_REG_VAL_24(2453400000), - FREQ_HZ_TO_REG_VAL_24(2454400000), - - FREQ_HZ_TO_REG_VAL_24(2455400000), - FREQ_HZ_TO_REG_VAL_24(2456400000), - FREQ_HZ_TO_REG_VAL_24(2457400000), - FREQ_HZ_TO_REG_VAL_24(2458400000), - FREQ_HZ_TO_REG_VAL_24(2459400000), - - FREQ_HZ_TO_REG_VAL_24(2460400000), - FREQ_HZ_TO_REG_VAL_24(2461400000), - FREQ_HZ_TO_REG_VAL_24(2462400000), - FREQ_HZ_TO_REG_VAL_24(2463400000), - FREQ_HZ_TO_REG_VAL_24(2464400000), - - FREQ_HZ_TO_REG_VAL_24(2465400000), - FREQ_HZ_TO_REG_VAL_24(2466400000), - FREQ_HZ_TO_REG_VAL_24(2467400000), - FREQ_HZ_TO_REG_VAL_24(2468400000), - FREQ_HZ_TO_REG_VAL_24(2469400000), - - FREQ_HZ_TO_REG_VAL_24(2470400000), - FREQ_HZ_TO_REG_VAL_24(2471400000), - FREQ_HZ_TO_REG_VAL_24(2472400000), - FREQ_HZ_TO_REG_VAL_24(2473400000), - FREQ_HZ_TO_REG_VAL_24(2474400000), - - FREQ_HZ_TO_REG_VAL_24(2475400000), - FREQ_HZ_TO_REG_VAL_24(2476400000), - FREQ_HZ_TO_REG_VAL_24(2477400000), - FREQ_HZ_TO_REG_VAL_24(2478400000), - FREQ_HZ_TO_REG_VAL_24(2479400000)}; + {ISM2400, FREQ_HZ_TO_REG_VAL_24(2400400000), FREQ_HZ_TO_REG_VAL_24(2479400000), 80}, + {CE2400, FREQ_HZ_TO_REG_VAL_24(2400400000), FREQ_HZ_TO_REG_VAL_24(2479400000), 80}, #endif +#if !defined(USE_RX_SX127X) && !defined(USE_RX_SX1280) + {0}, +#endif +}; + +const elrsFhssConfig_t *fhssConfig; void generateCrc14Table(void) { @@ -344,54 +134,14 @@ uint16_t calcCrc14(uint8_t *data, uint8_t len, uint16_t crc) return crc & 0x3FFF; } -static void initializeFhssFrequencies(const elrsFreqDomain_e dom) { - switch (dom) { -#ifdef USE_RX_SX127X - case AU433: - fhssFreqs = fhssFreqsAU433; - numFreqs = sizeof(fhssFreqsAU433) / sizeof(uint32_t); - break; - case AU915: - fhssFreqs = fhssFreqsAU915; - numFreqs = sizeof(fhssFreqsAU915) / sizeof(uint32_t); - break; - case EU433: - fhssFreqs = fhssFreqsEU433; - numFreqs = sizeof(fhssFreqsEU433) / sizeof(uint32_t); - break; - case EU868: - fhssFreqs = fhssFreqsEU868; - numFreqs = sizeof(fhssFreqsEU868) / sizeof(uint32_t); - break; - case IN866: - fhssFreqs = fhssFreqsIN866; - numFreqs = sizeof(fhssFreqsIN866) / sizeof(uint32_t); - break; - case FCC915: - fhssFreqs = fhssFreqsFCC915; - numFreqs = sizeof(fhssFreqsFCC915) / sizeof(uint32_t); - break; -#endif -#ifdef USE_RX_SX1280 - case ISM2400: - fhssFreqs = fhssFreqsISM2400; - numFreqs = sizeof(fhssFreqsISM2400) / sizeof(uint32_t); - break; -#endif - default: - fhssFreqs = NULL; - numFreqs = 0; - } -} - uint32_t fhssGetInitialFreq(const int32_t freqCorrection) { - return fhssFreqs[syncChannel] - freqCorrection; + return fhssConfig->freqStart + (syncChannel * freqSpread / ELRS_FREQ_SPREAD_SCALE) - freqCorrection; } uint8_t fhssGetNumEntries(void) { - return numFreqs; + return fhssConfig->freqCount; } uint8_t fhssGetCurrIndex(void) @@ -407,7 +157,8 @@ void fhssSetCurrIndex(const uint8_t value) uint32_t fhssGetNextFreq(const int32_t freqCorrection) { fhssIndex = (fhssIndex + 1) % seqCount; - return fhssFreqs[fhssSequence[fhssIndex]] - freqCorrection; + uint32_t freq = fhssConfig->freqStart + (freqSpread * fhssSequence[fhssIndex] / ELRS_FREQ_SPREAD_SCALE) - freqCorrection; + return freq; } static uint32_t seed = 0; @@ -437,30 +188,28 @@ Approach: */ void fhssGenSequence(const uint8_t UID[], const elrsFreqDomain_e dom) { - seed = ((long)UID[2] << 24) + ((long)UID[3] << 16) + ((long)UID[4] << 8) + UID[5]; - - initializeFhssFrequencies(dom); - - seqCount = (256 / MAX(numFreqs, 1)) * numFreqs; - - syncChannel = numFreqs / 2; + seed = (((long)UID[2] << 24) + ((long)UID[3] << 16) + ((long)UID[4] << 8) + UID[5]) ^ ELRS_OTA_VERSION_ID; + fhssConfig = &fhssConfigs[dom]; + seqCount = (256 / MAX(fhssConfig->freqCount, 1)) * fhssConfig->freqCount; + syncChannel = (fhssConfig->freqCount / 2) + 1; + freqSpread = (fhssConfig->freqStop - fhssConfig->freqStart) * ELRS_FREQ_SPREAD_SCALE / MAX((fhssConfig->freqCount - 1), 1); // initialize the sequence array - for (uint8_t i = 0; i < seqCount; i++) { - if (i % numFreqs == 0) { + for (uint16_t i = 0; i < seqCount; i++) { + if (i % fhssConfig->freqCount == 0) { fhssSequence[i] = syncChannel; - } else if (i % numFreqs == syncChannel) { + } else if (i % fhssConfig->freqCount == syncChannel) { fhssSequence[i] = 0; } else { - fhssSequence[i] = i % numFreqs; + fhssSequence[i] = i % fhssConfig->freqCount; } } - for (uint8_t i = 0; i < seqCount; i++) { + for (uint16_t i = 0; i < seqCount; i++) { // if it's not the sync channel - if (i % numFreqs != 0) { - uint8_t offset = (i / numFreqs) * numFreqs; // offset to start of current block - uint8_t rand = rngN(numFreqs - 1) + 1; // random number between 1 and numFreqs + if (i % fhssConfig->freqCount != 0) { + uint8_t offset = (i / fhssConfig->freqCount) * fhssConfig->freqCount; // offset to start of current block + uint8_t rand = rngN(fhssConfig->freqCount - 1) + 1; // random number between 1 and numFreqs // switch this entry and another random entry in the same block uint8_t temp = fhssSequence[i]; @@ -472,47 +221,27 @@ void fhssGenSequence(const uint8_t UID[], const elrsFreqDomain_e dom) uint8_t tlmRatioEnumToValue(const elrsTlmRatio_e enumval) { - switch (enumval) { - case TLM_RATIO_NO_TLM: + // !! TLM_RATIO_STD/TLM_RATIO_DISARMED should be converted by the caller !! + if (enumval == TLM_RATIO_NO_TLM) { return 1; - break; - case TLM_RATIO_1_2: - return 2; - break; - case TLM_RATIO_1_4: - return 4; - break; - case TLM_RATIO_1_8: - return 8; - break; - case TLM_RATIO_1_16: - return 16; - break; - case TLM_RATIO_1_32: - return 32; - break; - case TLM_RATIO_1_64: - return 64; - break; - case TLM_RATIO_1_128: - return 128; - break; - default: - return 0; } + + // 1 << (8 - (enumval - TLM_RATIO_NO_TLM)) + // 1_128 = 128, 1_64 = 64, 1_32 = 32, etc + return 1 << (8 + TLM_RATIO_NO_TLM - enumval); } uint16_t rateEnumToHz(const elrsRfRate_e eRate) { switch (eRate) { - case RATE_500HZ: return 500; - case RATE_250HZ: return 250; - case RATE_200HZ: return 200; - case RATE_150HZ: return 150; - case RATE_100HZ: return 100; - case RATE_50HZ: return 50; - case RATE_25HZ: return 25; - case RATE_4HZ: return 4; + case RATE_LORA_500HZ: return 500; + case RATE_LORA_250HZ: return 250; + case RATE_LORA_200HZ: return 200; + case RATE_LORA_150HZ: return 150; + case RATE_LORA_100HZ: return 100; + case RATE_LORA_50HZ: return 50; + case RATE_LORA_25HZ: return 25; + case RATE_LORA_4HZ: return 4; default: return 1; } } @@ -620,11 +349,6 @@ uint16_t convertSwitchNb(const uint16_t val, const uint16_t max) return (val > max) ? 1500 : val * 1000 / max + 1000; } -uint16_t convertAnalog(const uint16_t val) -{ - return CRSF_RC_CHANNEL_SCALE_LEGACY * val + 881; -} - uint8_t hybridWideNonceToSwitchIndex(const uint8_t nonce) { // Returns the sequence (0 to 7, then 0 to 7 rotated left by 1): @@ -637,4 +361,50 @@ uint8_t hybridWideNonceToSwitchIndex(const uint8_t nonce) return ((nonce & 0x07) + ((nonce >> 3) & 0x01)) % 8; } +uint8_t airRateIndexToIndex900(uint8_t airRate, uint8_t currentIndex) +{ + switch (airRate) { + case 0: + return 0; + case 1: + return currentIndex; + case 2: + return 1; + case 3: + return 2; + case 4: + return 3; + default: + return currentIndex; + } +} + +uint8_t airRateIndexToIndex24(uint8_t airRate, uint8_t currentIndex) +{ + switch (airRate) { + case 0: + return currentIndex; + case 1: + return currentIndex; + case 2: + return currentIndex; + case 3: + return currentIndex; + case 4: + return 0; + case 5: + return currentIndex; + case 6: + return 1; + case 7: + return 2; + case 8: + return currentIndex; + case 9: + return 3; + default: + return currentIndex; + } +} + #endif /* USE_RX_EXPRESSLRS */ diff --git a/src/main/rx/expresslrs_common.h b/src/main/rx/expresslrs_common.h index 91017a2fd4..0b6519c5ea 100644 --- a/src/main/rx/expresslrs_common.h +++ b/src/main/rx/expresslrs_common.h @@ -31,10 +31,15 @@ #include "drivers/io_types.h" #include "drivers/time.h" +#include "rx/expresslrs_telemetry.h" + +#define ELRS_OTA_VERSION_ID 3 + #define ELRS_CRC_LEN 256 #define ELRS_CRC14_POLY 0x2E57 #define ELRS_NR_SEQUENCE_ENTRIES 256 +#define ELRS_FREQ_SPREAD_SCALE 256 #define ELRS_RX_TX_BUFF_SIZE 8 @@ -74,6 +79,7 @@ typedef enum { #endif #ifdef USE_RX_SX1280 ISM2400, + CE2400, #endif #if !defined(USE_RX_SX127X) && !defined(USE_RX_SX1280) NONE, @@ -81,30 +87,38 @@ typedef enum { } elrsFreqDomain_e; typedef enum { - SM_HYBRID = 0, - SM_HYBRID_WIDE = 1 + SM_WIDE = 0, + SM_HYBRID = 1 } elrsSwitchMode_e; typedef enum { - TLM_RATIO_NO_TLM = 0, - TLM_RATIO_1_128 = 1, - TLM_RATIO_1_64 = 2, - TLM_RATIO_1_32 = 3, - TLM_RATIO_1_16 = 4, - TLM_RATIO_1_8 = 5, - TLM_RATIO_1_4 = 6, - TLM_RATIO_1_2 = 7, + TLM_RATIO_STD = 0, // Use suggested ratio from ModParams + TLM_RATIO_NO_TLM, + TLM_RATIO_1_128, + TLM_RATIO_1_64, + TLM_RATIO_1_32, + TLM_RATIO_1_16, + TLM_RATIO_1_8, + TLM_RATIO_1_4, + TLM_RATIO_1_2, + TLM_RATIO_DISARMED, // TLM_RATIO_STD when disarmed, TLM_RATIO_NO_TLM when armed } elrsTlmRatio_e; typedef enum { - RATE_500HZ = 0, - RATE_250HZ = 1, - RATE_200HZ = 2, - RATE_150HZ = 3, - RATE_100HZ = 4, - RATE_50HZ = 5, - RATE_25HZ = 6, - RATE_4HZ = 7, + RATE_LORA_4HZ = 0, + RATE_LORA_25HZ, + RATE_LORA_50HZ, + RATE_LORA_100HZ, + RATE_LORA_100HZ_8CH, + RATE_LORA_150HZ, + RATE_LORA_200HZ, + RATE_LORA_250HZ, + RATE_LORA_333HZ_8CH, + RATE_LORA_500HZ, + RATE_DVDA_250HZ, + RATE_DVDA_500HZ, + RATE_FLRC_500HZ, + RATE_FLRC_1000HZ, } elrsRfRate_e; // Max value of 16 since only 4 bits have been assigned in the sync package. typedef struct elrsModSettings_s { @@ -122,14 +136,71 @@ typedef struct elrsModSettings_s { typedef struct elrsRfPerfParams_s { int8_t index; elrsRfRate_e enumRate; // Max value of 16 since only 4 bits have been assigned in the sync package. - int32_t sensitivity; // expected RF sensitivity based on - uint32_t toa; // time on air in microseconds - uint32_t disconnectTimeoutMs; // Time without a packet before receiver goes to disconnected (ms) - uint32_t rxLockTimeoutMs; // Max time to go from tentative -> connected state on receiver (ms) - uint32_t syncPktIntervalDisconnected; // how often to send the SYNC_PACKET packet (ms) when there is no response from RX - uint32_t syncPktIntervalConnected; // how often to send the SYNC_PACKET packet (ms) when there we have a connection + int16_t sensitivity; // expected RF sensitivity based on + uint16_t toa; // time on air in microseconds + uint16_t disconnectTimeoutMs; // Time without a packet before receiver goes to disconnected (ms) + uint16_t rxLockTimeoutMs; // Max time to go from tentative -> connected state on receiver (ms) + uint16_t syncPktIntervalDisconnected; // how often to send the SYNC_PACKET packet (ms) when there is no response from RX + uint16_t syncPktIntervalConnected; // how often to send the SYNC_PACKET packet (ms) when there we have a connection } elrsRfPerfParams_t; +typedef struct elrsFhssConfig_s { + uint8_t domain; + uint32_t freqStart; + uint32_t freqStop; + uint8_t freqCount; +} elrsFhssConfig_t; + +typedef struct elrsOtaPacket_s { + // The packet type must always be the low two bits of the first byte of the + // packet to match the same placement in OTA_Packet8_s + uint8_t type : 2, + crcHigh : 6; + union { + /** PACKET_TYPE_RCDATA **/ + struct { + uint8_t ch[5]; + uint8_t switches : 7, + ch4 : 1; + } rc; + /** PACKET_TYPE_MSP **/ + struct { + uint8_t packageIndex; + uint8_t payload[ELRS_MSP_BYTES_PER_CALL]; + } msp_ul; + /** PACKET_TYPE_SYNC **/ + struct { + uint8_t fhssIndex; + uint8_t nonce; + uint8_t switchEncMode : 1, + newTlmRatio : 3, + rateIndex : 4; + uint8_t UID3; + uint8_t UID4; + uint8_t UID5; + } sync; + /** PACKET_TYPE_TLM **/ + struct { + uint8_t type : ELRS_TELEMETRY_SHIFT, + packageIndex : (8 - ELRS_TELEMETRY_SHIFT); + union { + struct { + uint8_t uplink_RSSI_1 : 7, + antenna : 1; + uint8_t uplink_RSSI_2 : 7, + modelMatch : 1; + uint8_t lq : 7, + mspConfirm : 1; + int8_t SNR; + uint8_t free; + } ul_link_stats; + uint8_t payload[ELRS_TELEMETRY_BYTES_PER_CALL]; + }; + } tlm_dl; + }; + uint8_t crcLow; +} __attribute__ ((__packed__)) elrsOtaPacket_t; + typedef bool (*elrsRxInitFnPtr)(IO_t resetPin, IO_t busyPin); typedef void (*elrsRxConfigFnPtr)(const uint8_t bw, const uint8_t sf, const uint8_t cr, const uint32_t freq, const uint8_t preambleLen, const bool iqInverted); typedef void (*elrsRxStartReceivingFnPtr)(void); @@ -170,5 +241,7 @@ uint8_t lqGet(void); uint16_t convertSwitch1b(const uint16_t val); uint16_t convertSwitch3b(const uint16_t val); uint16_t convertSwitchNb(const uint16_t val, const uint16_t max); -uint16_t convertAnalog(const uint16_t val); uint8_t hybridWideNonceToSwitchIndex(const uint8_t nonce); + +uint8_t airRateIndexToIndex24(uint8_t airRate, uint8_t currentIndex); +uint8_t airRateIndexToIndex900(uint8_t airRate, uint8_t currentIndex); diff --git a/src/main/rx/expresslrs_telemetry.c b/src/main/rx/expresslrs_telemetry.c index 2215064793..d015f3a8ee 100644 --- a/src/main/rx/expresslrs_telemetry.c +++ b/src/main/rx/expresslrs_telemetry.c @@ -31,6 +31,7 @@ #ifdef USE_RX_EXPRESSLRS +#include "common/maths.h" #include "config/feature.h" #include "fc/runtime_config.h" @@ -65,10 +66,10 @@ static crsfFrameType_e payloadTypes[] = { STATIC_UNIT_TESTED uint8_t tlmSensors = 0; STATIC_UNIT_TESTED uint8_t currentPayloadIndex; -static uint8_t *data; -static uint8_t length; -static uint8_t bytesPerCall; +static uint8_t *data = NULL; +static uint8_t length = 0; static uint8_t currentOffset; +static uint8_t bytesLastPayload; static uint8_t currentPackage; static bool waitUntilTelemetryConfirm; static uint16_t waitCount; @@ -77,33 +78,26 @@ static volatile stubbornSenderState_e senderState; static void telemetrySenderResetState(void) { - data = 0; + bytesLastPayload = 0; currentOffset = 0; - currentPackage = 0; - length = 0; + currentPackage = 1; waitUntilTelemetryConfirm = true; waitCount = 0; // 80 corresponds to UpdateTelemetryRate(ANY, 2, 1), which is what the TX uses in boost mode maxWaitCount = 80; senderState = ELRS_SENDER_IDLE; - bytesPerCall = 1; } /*** * Queues a message to send, will abort the current message if one is currently being transmitted ***/ -void setTelemetryDataToTransmit(const uint8_t lengthToTransmit, uint8_t* dataToTransmit, const uint8_t bpc) +void setTelemetryDataToTransmit(const uint8_t lengthToTransmit, uint8_t* dataToTransmit) { - if (lengthToTransmit / bpc >= ELRS_TELEMETRY_MAX_PACKAGES) { - return; - } - length = lengthToTransmit; data = dataToTransmit; currentOffset = 0; currentPackage = 1; waitCount = 0; - bytesPerCall = bpc; senderState = (senderState == ELRS_SENDER_IDLE) ? ELRS_SENDING : ELRS_RESYNC_THEN_SEND; } @@ -112,33 +106,37 @@ bool isTelemetrySenderActive(void) return senderState != ELRS_SENDER_IDLE; } -void getCurrentTelemetryPayload(uint8_t *packageIndex, uint8_t *count, uint8_t **currentData) +/*** + * Copy up to maxLen bytes from the current package to outData + * packageIndex + ***/ +uint8_t getCurrentTelemetryPayload(uint8_t *outData) { + uint8_t packageIndex; + + bytesLastPayload = 0; switch (senderState) { case ELRS_RESYNC: case ELRS_RESYNC_THEN_SEND: - *packageIndex = ELRS_TELEMETRY_MAX_PACKAGES; - *count = 0; - *currentData = 0; + packageIndex = ELRS_TELEMETRY_MAX_PACKAGES; break; case ELRS_SENDING: - *currentData = data + currentOffset; - *packageIndex = currentPackage; - if (bytesPerCall > 1) { - if (currentOffset + bytesPerCall <= length) { - *count = bytesPerCall; - } else { - *count = length - currentOffset; - } + bytesLastPayload = MIN((uint8_t)(length - currentOffset), ELRS_TELEMETRY_BYTES_PER_CALL); + // If this is the last data chunk, and there has been at least one other packet + // skip the blank packet needed for WAIT_UNTIL_NEXT_CONFIRM + if (currentPackage > 1 && (currentOffset + bytesLastPayload) >= length) { + packageIndex = 0; } else { - *count = 1; + packageIndex = currentPackage; } + memcpy(outData, &data[currentOffset], bytesLastPayload); + break; default: - *count = 0; - *currentData = 0; - *packageIndex = 0; + packageIndex = 0; } + + return packageIndex; } void confirmCurrentTelemetryPayload(const bool telemetryConfirmValue) @@ -156,15 +154,21 @@ void confirmCurrentTelemetryPayload(const bool telemetryConfirmValue) break; } - currentOffset += bytesPerCall; + currentOffset += bytesLastPayload; + if (currentOffset >= length) { + // A 0th packet is always requred so the reciver can + // differentiate a new send from a resend, if this is + // the first packet acked, send another, else IDLE + if (currentPackage == 1) { + nextSenderState = ELRS_WAIT_UNTIL_NEXT_CONFIRM; + } else { + nextSenderState = ELRS_SENDER_IDLE; + } + } + currentPackage++; waitUntilTelemetryConfirm = !waitUntilTelemetryConfirm; waitCount = 0; - - if (currentOffset >= length) { - nextSenderState = ELRS_WAIT_UNTIL_NEXT_CONFIRM; - } - break; case ELRS_RESYNC: @@ -191,10 +195,9 @@ void confirmCurrentTelemetryPayload(const bool telemetryConfirmValue) } #ifdef USE_MSP_OVER_TELEMETRY -static uint8_t *mspData; +static uint8_t *mspData = NULL; static volatile bool finishedData; -static volatile uint8_t mspLength; -static volatile uint8_t mspBytesPerCall; +static volatile uint8_t mspLength = 0; static volatile uint8_t mspCurrentOffset; static volatile uint8_t mspCurrentPackage; static volatile bool mspConfirm; @@ -203,11 +206,8 @@ STATIC_UNIT_TESTED volatile bool mspReplyPending; STATIC_UNIT_TESTED volatile bool deviceInfoReplyPending; void mspReceiverResetState(void) { - mspData = 0; - mspBytesPerCall = 1; mspCurrentOffset = 0; - mspCurrentPackage = 0; - mspLength = 0; + mspCurrentPackage = 1; mspConfirm = false; mspReplyPending = false; deviceInfoReplyPending = false; @@ -218,24 +218,18 @@ bool getCurrentMspConfirm(void) return mspConfirm; } -void setMspDataToReceive(const uint8_t maxLength, uint8_t* dataToReceive, const uint8_t bpc) +void setMspDataToReceive(const uint8_t maxLength, uint8_t* dataToReceive) { mspLength = maxLength; mspData = dataToReceive; mspCurrentPackage = 1; mspCurrentOffset = 0; finishedData = false; - mspBytesPerCall = bpc; } -void receiveMspData(const uint8_t packageIndex, const volatile uint8_t* receiveData) +void receiveMspData(const uint8_t packageIndex, const volatile uint8_t* const receiveData) { - if (packageIndex == 0 && mspCurrentPackage > 1) { - finishedData = true; - mspConfirm = !mspConfirm; - return; - } - + // Resync if (packageIndex == ELRS_MSP_MAX_PACKAGES) { mspConfirm = !mspConfirm; mspCurrentPackage = 1; @@ -248,17 +242,22 @@ void receiveMspData(const uint8_t packageIndex, const volatile uint8_t* receiveD return; } - if (packageIndex == mspCurrentPackage) { - for (uint8_t i = 0; i < mspBytesPerCall; i++) { - mspData[mspCurrentOffset++] = *(receiveData + i); - } - + bool acceptData = false; + if (packageIndex == 0 && mspCurrentPackage > 1) { + // PackageIndex 0 (the final packet) can also contain data + acceptData = true; + finishedData = true; + } else if (packageIndex == mspCurrentPackage) { + acceptData = true; mspCurrentPackage++; - mspConfirm = !mspConfirm; - return; } - return; + if (acceptData && (receiveData != NULL)) { + uint8_t len = MIN((uint8_t)(mspLength - mspCurrentOffset), ELRS_MSP_BYTES_PER_CALL); + memcpy(&mspData[mspCurrentOffset], (const uint8_t*) receiveData, len); + mspCurrentOffset += len; + mspConfirm = !mspConfirm; + } } bool hasFinishedMspData(void) diff --git a/src/main/rx/expresslrs_telemetry.h b/src/main/rx/expresslrs_telemetry.h index 8027d2981c..186aaf6652 100644 --- a/src/main/rx/expresslrs_telemetry.h +++ b/src/main/rx/expresslrs_telemetry.h @@ -45,16 +45,16 @@ typedef enum { void initTelemetry(void); bool getNextTelemetryPayload(uint8_t *nextPayloadSize, uint8_t **payloadData); -void setTelemetryDataToTransmit(const uint8_t lengthToTransmit, uint8_t* dataToTransmit, const uint8_t bpc); +void setTelemetryDataToTransmit(const uint8_t lengthToTransmit, uint8_t* dataToTransmit); bool isTelemetrySenderActive(void); -void getCurrentTelemetryPayload(uint8_t *packageIndex, uint8_t *count, uint8_t **currentData); +uint8_t getCurrentTelemetryPayload(uint8_t *outData); void confirmCurrentTelemetryPayload(const bool telemetryConfirmValue); void updateTelemetryRate(const uint16_t airRate, const uint8_t tlmRatio, const uint8_t tlmBurst); void mspReceiverResetState(void); bool getCurrentMspConfirm(void); -void setMspDataToReceive(const uint8_t maxLength, uint8_t* dataToReceive, const uint8_t bpc); -void receiveMspData(const uint8_t packageIndex, const volatile uint8_t* receiveData); +void setMspDataToReceive(const uint8_t maxLength, uint8_t* dataToReceive); +void receiveMspData(const uint8_t packageIndex, const volatile uint8_t* const receiveData); bool hasFinishedMspData(void); void mspReceiverUnlock(void); void processMspPacket(uint8_t *packet); diff --git a/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc b/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc index 417655e41f..b38824094e 100644 --- a/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc +++ b/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc @@ -84,30 +84,29 @@ TEST(RxSpiExpressLrsTelemetryUnitTest, TestInit) static void testSetDataToTransmit(uint8_t payloadSize, uint8_t *payload) { - uint8_t *data; - uint8_t maxLength; - uint8_t packageIndex; + uint8_t data[ELRS_TELEMETRY_BYTES_PER_CALL] = {0}; + uint8_t maxPackageIndex = (payloadSize - 1) / ELRS_TELEMETRY_BYTES_PER_CALL; + uint8_t nextPackageIndex; bool confirmValue = true; - setTelemetryDataToTransmit(payloadSize, payload, ELRS_TELEMETRY_BYTES_PER_CALL); + setTelemetryDataToTransmit(payloadSize, payload); - for (int j = 0; j < (1 + ((payloadSize - 1) / ELRS_TELEMETRY_BYTES_PER_CALL)); j++) { - getCurrentTelemetryPayload(&packageIndex, &maxLength, &data); - EXPECT_LE(maxLength, 5); - EXPECT_EQ(1 + j, packageIndex); - for(int i = 0; i < maxLength; i++) { + for (int j = 0; j <= maxPackageIndex; j++) { + nextPackageIndex = getCurrentTelemetryPayload(data); + if (j != maxPackageIndex) { + EXPECT_EQ(1 + j, nextPackageIndex); + } else { + EXPECT_EQ(0, nextPackageIndex); //back to start + } + uint8_t maxLength = (j == maxPackageIndex) ? payloadSize % ELRS_TELEMETRY_BYTES_PER_CALL : ELRS_TELEMETRY_BYTES_PER_CALL; + for (int i = 0; i < maxLength; i++) { EXPECT_EQ(payload[i + j * ELRS_TELEMETRY_BYTES_PER_CALL], data[i]); } + EXPECT_EQ(true, isTelemetrySenderActive()); confirmCurrentTelemetryPayload(confirmValue); confirmValue = !confirmValue; } - getCurrentTelemetryPayload(&packageIndex, &maxLength, &data); - EXPECT_EQ(0, packageIndex); - EXPECT_EQ(true, isTelemetrySenderActive()); - confirmCurrentTelemetryPayload(!confirmValue); - EXPECT_EQ(true, isTelemetrySenderActive()); - confirmCurrentTelemetryPayload(confirmValue); EXPECT_EQ(false, isTelemetrySenderActive()); } @@ -240,7 +239,7 @@ TEST(RxSpiExpressLrsTelemetryUnitTest, TestMspVersionRequest) initTelemetry(); initSharedMsp(); - setMspDataToReceive(15, mspBuffer, ELRS_MSP_BYTES_PER_CALL); + setMspDataToReceive(15, mspBuffer); receiveMspData(data1[0], data1 + 1); receiveMspData(data2[0], data2 + 1); receiveMspData(data3[0], data3 + 1); @@ -279,7 +278,7 @@ TEST(RxSpiExpressLrsTelemetryUnitTest, TestMspPidRequest) initTelemetry(); initSharedMsp(); - setMspDataToReceive(sizeof(mspBuffer), mspBuffer, ELRS_MSP_BYTES_PER_CALL); + setMspDataToReceive(sizeof(mspBuffer), mspBuffer); receiveMspData(data1[0], data1 + 1); EXPECT_FALSE(hasFinishedMspData()); receiveMspData(data2[0], data2 + 1); @@ -328,7 +327,7 @@ TEST(RxSpiExpressLrsTelemetryUnitTest, TestMspVtxRequest) initTelemetry(); initSharedMsp(); - setMspDataToReceive(sizeof(mspBuffer), mspBuffer, ELRS_MSP_BYTES_PER_CALL); + setMspDataToReceive(sizeof(mspBuffer), mspBuffer); receiveMspData(data1[0], data1 + 1); receiveMspData(data2[0], data2 + 1); receiveMspData(data3[0], data3 + 1); @@ -364,10 +363,10 @@ TEST(RxSpiExpressLrsTelemetryUnitTest, TestDeviceInfoResp) initTelemetry(); initSharedMsp(); - setMspDataToReceive(sizeof(mspBuffer), mspBuffer, ELRS_MSP_BYTES_PER_CALL); + setMspDataToReceive(sizeof(mspBuffer), mspBuffer); receiveMspData(pingData[0], pingData + 1); EXPECT_FALSE(hasFinishedMspData()); - receiveMspData(0, 0); + receiveMspData(0, pingData + 1); EXPECT_TRUE(hasFinishedMspData()); EXPECT_FALSE(deviceInfoReplyPending); diff --git a/src/test/unit/rx_spi_expresslrs_unittest.cc b/src/test/unit/rx_spi_expresslrs_unittest.cc index a5ac4880ff..8f2d9fff5d 100644 --- a/src/test/unit/rx_spi_expresslrs_unittest.cc +++ b/src/test/unit/rx_spi_expresslrs_unittest.cc @@ -105,71 +105,84 @@ TEST(RxSpiExpressLrsUnitTest, TestCrc14) } } +static void printFhssSequence(uint8_t *seq) +{ + for (int i = 0; i < ELRS_NR_SEQUENCE_ENTRIES; i++) { + printf("%d, ", seq[i]); + if (i % 10 == 9) { + printf("\n"); + } + } + printf("\n\n"); +} + TEST(RxSpiExpressLrsUnitTest, TestFHSSTable) { const uint8_t UID[6] = {1, 2, 3, 4, 5, 6}; const uint8_t expectedSequence[2][ELRS_NR_SEQUENCE_ENTRIES] = { { - 40, 43, 39, 18, 52, 19, 36, 7, 1, 12, - 71, 5, 42, 46, 50, 28, 49, 33, 76, 51, - 60, 70, 47, 61, 0, 55, 72, 37, 53, 25, - 3, 11, 41, 13, 35, 27, 9, 75, 48, 77, - 73, 74, 69, 58, 14, 31, 10, 59, 66, 4, - 78, 17, 44, 54, 29, 57, 21, 64, 22, 67, - 62, 56, 15, 79, 6, 34, 23, 30, 32, 2, - 68, 8, 63, 65, 45, 20, 24, 26, 16, 38, - 40, 8, 52, 29, 57, 10, 6, 26, 19, 75, - 21, 24, 1, 9, 50, 32, 69, 67, 2, 59, - 28, 48, 77, 60, 41, 49, 68, 4, 5, 3, - 44, 78, 58, 31, 16, 62, 35, 45, 73, 11, - 33, 46, 42, 36, 64, 7, 34, 53, 17, 25, - 37, 38, 54, 55, 15, 76, 18, 43, 23, 12, - 39, 51, 22, 79, 74, 63, 27, 66, 65, 47, - 70, 0, 30, 61, 13, 56, 14, 72, 71, 20, - 40, 71, 68, 12, 57, 45, 10, 53, 21, 15, - 69, 26, 54, 55, 73, 47, 35, 77, 1, 31, - 20, 0, 38, 76, 5, 60, 6, 79, 3, 16, - 50, 17, 52, 62, 18, 46, 28, 39, 29, 51, - 43, 34, 49, 56, 32, 61, 74, 58, 25, 44, - 2, 19, 65, 4, 13, 67, 11, 30, 66, 64, - 36, 24, 75, 33, 59, 7, 41, 70, 48, 14, - 42, 37, 8, 23, 78, 63, 22, 9, 72, 27 + 41, 39, 72, 45, 76, 48, 3, 79, 55, 66, + 68, 13, 65, 20, 15, 43, 21, 54, 46, 37, + 60, 29, 50, 40, 34, 61, 69, 64, 10, 70, + 16, 2, 0, 38, 36, 30, 47, 8, 59, 35, + 4, 71, 73, 11, 9, 51, 32, 1, 18, 12, + 22, 26, 49, 25, 6, 7, 77, 62, 42, 57, + 53, 74, 14, 31, 28, 75, 78, 23, 24, 27, + 17, 44, 67, 33, 19, 5, 52, 56, 58, 63, + 41, 24, 21, 5, 58, 3, 70, 76, 59, 67, + 15, 78, 26, 10, 2, 27, 7, 23, 48, 31, + 72, 13, 56, 45, 51, 50, 44, 54, 39, 53, + 22, 71, 9, 47, 55, 36, 49, 43, 4, 40, + 52, 25, 60, 28, 34, 74, 6, 29, 62, 14, + 8, 0, 19, 17, 79, 46, 42, 77, 37, 18, + 66, 38, 30, 75, 32, 63, 1, 33, 69, 12, + 61, 57, 35, 65, 11, 64, 20, 73, 68, 16, + 41, 75, 76, 17, 26, 22, 25, 60, 53, 43, + 72, 50, 38, 24, 79, 77, 49, 33, 15, 8, + 14, 59, 42, 64, 1, 30, 29, 35, 39, 56, + 40, 36, 74, 18, 47, 73, 62, 13, 61, 58, + 44, 71, 0, 37, 19, 16, 48, 63, 65, 20, + 69, 54, 52, 70, 31, 3, 12, 21, 57, 10, + 5, 7, 28, 55, 27, 6, 11, 9, 78, 45, + 68, 66, 2, 67, 51, 32, 34, 23, 4, 46 }, { - 20, 37, 1, 3, 7, 26, 36, 29, 15, 35, - 33, 24, 10, 34, 13, 31, 22, 9, 28, 23, - 17, 38, 6, 27, 0, 32, 11, 5, 18, 25, - 2, 4, 12, 19, 16, 8, 30, 14, 21, 39, - 20, 2, 14, 7, 13, 33, 32, 28, 21, 11, - 25, 17, 22, 9, 3, 4, 0, 31, 35, 38, - 10, 34, 26, 39, 36, 6, 19, 16, 30, 27, - 15, 24, 18, 1, 23, 37, 29, 8, 12, 5, - 20, 19, 24, 29, 27, 2, 22, 14, 0, 3, - 23, 13, 12, 35, 4, 25, 38, 18, 33, 36, - 21, 16, 5, 31, 9, 32, 11, 1, 6, 7, - 10, 15, 26, 34, 39, 37, 28, 17, 30, 8, - 20, 7, 4, 24, 19, 16, 8, 13, 15, 10, - 14, 36, 34, 0, 17, 12, 28, 21, 39, 22, - 3, 2, 32, 33, 27, 6, 37, 18, 31, 38, - 23, 25, 26, 30, 9, 1, 35, 5, 11, 29, - 20, 1, 35, 22, 0, 10, 11, 27, 18, 37, - 21, 31, 9, 19, 30, 17, 5, 38, 29, 36, - 3, 2, 25, 34, 23, 6, 15, 4, 16, 26, - 12, 24, 14, 13, 39, 8, 32, 7, 28, 33, - 20, 36, 13, 5, 39, 37, 15, 8, 9, 4, - 22, 12, 1, 6, 32, 25, 17, 18, 27, 28, - 23, 19, 26, 3, 38, 16, 2, 34, 14, 30, - 10, 11, 7, 0, 35, 24, 21, 33, 31, 29 + 21, 7, 12, 6, 11, 27, 35, 9, 18, 28, + 13, 1, 24, 32, 30, 38, 14, 5, 2, 23, + 37, 26, 17, 3, 25, 29, 39, 36, 0, 4, + 33, 10, 16, 31, 34, 22, 8, 19, 15, 20, + 21, 33, 11, 13, 26, 6, 1, 7, 2, 0, + 9, 28, 19, 18, 23, 10, 29, 14, 25, 3, + 30, 15, 35, 38, 5, 39, 22, 8, 20, 24, + 34, 27, 37, 36, 31, 12, 4, 16, 32, 17, + 21, 20, 30, 37, 11, 5, 26, 3, 18, 32, + 35, 13, 38, 9, 15, 2, 16, 34, 22, 29, + 7, 24, 10, 39, 28, 8, 31, 1, 23, 0, + 12, 36, 19, 4, 27, 17, 6, 25, 33, 14, + 21, 8, 32, 16, 15, 20, 30, 29, 5, 7, + 31, 33, 28, 9, 36, 34, 13, 1, 0, 12, + 35, 26, 25, 39, 22, 19, 11, 2, 37, 23, + 10, 14, 6, 3, 17, 27, 18, 38, 4, 24, + 21, 12, 25, 32, 9, 8, 6, 28, 38, 17, + 2, 31, 10, 16, 20, 24, 13, 22, 39, 29, + 26, 5, 7, 18, 19, 11, 14, 30, 35, 4, + 37, 15, 0, 34, 33, 23, 27, 1, 36, 3, + 21, 36, 5, 31, 17, 32, 10, 34, 1, 3, + 7, 2, 28, 38, 13, 4, 22, 29, 0, 23, + 20, 26, 25, 16, 30, 11, 35, 6, 19, 24, + 8, 18, 33, 15, 37, 12, 14, 39, 27, 9 } }; fhssGenSequence(UID, ISM2400); + printFhssSequence(fhssSequence); for (int i = 0; i < ELRS_NR_SEQUENCE_ENTRIES; i++) { EXPECT_EQ(expectedSequence[0][i], fhssSequence[i]); } fhssGenSequence(UID, FCC915); + printFhssSequence(fhssSequence); for (int i = 0; i < ELRS_NR_SEQUENCE_ENTRIES; i++) { EXPECT_EQ(expectedSequence[1][i], fhssSequence[i]); } @@ -194,13 +207,13 @@ TEST(RxSpiExpressLrsUnitTest, TestInitUnbound) EXPECT_EQ(0, receiver.lastValidPacketMs); const uint32_t initialFrequencies[7] = { - FREQ_HZ_TO_REG_VAL_900(433920000), - FREQ_HZ_TO_REG_VAL_900(921500000), - FREQ_HZ_TO_REG_VAL_900(433925000), - FREQ_HZ_TO_REG_VAL_900(866425000), - FREQ_HZ_TO_REG_VAL_900(866425000), - FREQ_HZ_TO_REG_VAL_900(915500000), - FREQ_HZ_TO_REG_VAL_24(2440400000) + FREQ_HZ_TO_REG_VAL_900(434420000), + FREQ_HZ_TO_REG_VAL_900(922100000), + FREQ_HZ_TO_REG_VAL_900(434450000), + FREQ_HZ_TO_REG_VAL_900(867783300), + FREQ_HZ_TO_REG_VAL_900(866949900), + FREQ_HZ_TO_REG_VAL_900(916100000), + FREQ_HZ_TO_REG_VAL_24(2441400000) }; for (int i = 0; i < 7; i++) { @@ -245,7 +258,7 @@ TEST(RxSpiExpressLrsUnitTest, TestInitUnbound) expressLrsSpiInit(&injectedConfig, &config, &extiConfig); EXPECT_EQ(16, config.channelCount); receiver = empty; - rxExpressLrsSpiConfigMutable()->switchMode = SM_HYBRID_WIDE; + rxExpressLrsSpiConfigMutable()->switchMode = SM_WIDE; expressLrsSpiInit(&injectedConfig, &config, &extiConfig); EXPECT_EQ(16, config.channelCount); } @@ -363,13 +376,6 @@ TEST(RxSpiExpressLrsUnitTest, Test4bSwitchDecode) EXPECT_EQ(1500, convertSwitchNb(255, 15)); } -TEST(RxSpiExpressLrsUnitTest, TestAnalogDecode) -{ - EXPECT_EQ(988, convertAnalog(172)); - EXPECT_EQ(1500, convertAnalog(992)); - EXPECT_EQ(2012, convertAnalog(1811)); -} - // STUBS extern "C" { @@ -419,6 +425,7 @@ extern "C" { } void sx1280AdjustFrequency(int32_t , const uint32_t ) {} bool sx1280Init(IO_t , IO_t ) { return true; } + void sx1280SetOutputPower(const int8_t ) {} void sx127xConfig(const sx127xBandwidth_e , const sx127xSpreadingFactor_e , const sx127xCodingRate_e , const uint32_t , const uint8_t , const bool ) {} void sx127xStartReceiving(void) {} @@ -464,9 +471,13 @@ extern "C" { void initTelemetry(void) {} bool getNextTelemetryPayload(uint8_t *, uint8_t **) { return false; } - void setTelemetryDataToTransmit(const uint8_t , uint8_t* , const uint8_t ) {} + void setTelemetryDataToTransmit(const uint8_t , uint8_t* ) {} bool isTelemetrySenderActive(void) { return false; } - void getCurrentTelemetryPayload(uint8_t *, uint8_t *, uint8_t **) {} + uint8_t getCurrentTelemetryPayload(uint8_t * ) { return 0; } void confirmCurrentTelemetryPayload(const bool ) {} void updateTelemetryRate(const uint16_t , const uint8_t , const uint8_t ) {} + + void meanAccumulatorAdd(meanAccumulator_t * , const int8_t ) {}; + int8_t meanAccumulatorCalc(meanAccumulator_t * , const int8_t defaultValue) { return defaultValue; }; + void meanAccumulatorInit(meanAccumulator_t * ) {}; }