diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index ec8ec58fc3..dd082f6254 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -519,7 +519,7 @@ static const char* const lookupTableFreqDomain[] = { }; static const char* const lookupTableSwitchMode[] = { - "HYBRID", "WIDE", + "WIDE", "HYBRID", }; #endif diff --git a/src/main/rx/expresslrs.c b/src/main/rx/expresslrs.c index 95ed4c82dc..2de2ba7816 100644 --- a/src/main/rx/expresslrs.c +++ b/src/main/rx/expresslrs.c @@ -192,12 +192,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 +224,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 +258,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) { @@ -288,8 +277,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); @@ -387,23 +375,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 = receiver.snr; #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 +404,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) @@ -594,10 +578,10 @@ 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]; @@ -608,12 +592,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 +609,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,25 +629,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] & 0xE0) >> 5) - 3; - uint8_t switchEncMode = ((packet[3] & 0x03) >> 0) - 1; //spi implementation uses 0 based index for hybrid + receiver.nextRateIndex = (rxExpressLrsSpiConfig()->domain == ISM2400) ? 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) { @@ -672,7 +656,7 @@ static bool processRFSyncPacket(volatile uint8_t *packet, const uint32_t timeSta } // Update TLM ratio - uint8_t tlmRateIn = ((packet[3] & 0x1C) >> 2) + TLM_RATIO_NO_TLM; + uint8_t tlmRateIn = otaPktPtr->sync.newTlmRatio + TLM_RATIO_NO_TLM; uint8_t tlmDenom = tlmRatioEnumToValue(tlmRateIn); if (currTlmDenom != tlmDenom) { currTlmDenom = tlmDenom; @@ -681,11 +665,11 @@ static bool processRFSyncPacket(volatile uint8_t *packet, const uint32_t timeSta // 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; @@ -698,22 +682,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_HYBRID_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; } @@ -724,7 +711,7 @@ 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 @@ -732,22 +719,22 @@ rx_spi_received_e processRFPacket(volatile uint8_t *payload, uint32_t timeStampU if (rxExpressLrsSpiConfig()->switchMode == SM_HYBRID_WIDE) { wideSwitchIndex = hybridWideNonceToSwitchIndex(receiver.nonceRX); if ((currTlmDenom < 8) || wideSwitchIndex == 7) { - confirmCurrentTelemetryPayload((dmaBuffer[6] & 0x40) >> 6); + 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; @@ -926,7 +913,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 +1026,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 +1034,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_HYBRID_WIDE ? unpackChannelDataHybridWide(rcData, otaPktPtr) : unpackChannelDataHybridSwitch8(rcData, otaPktPtr); } } diff --git a/src/main/rx/expresslrs_common.c b/src/main/rx/expresslrs_common.c index 30a3487709..e72d3576b2 100644 --- a/src/main/rx/expresslrs_common.c +++ b/src/main/rx/expresslrs_common.c @@ -190,7 +190,7 @@ 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]; fhssConfig = &fhssConfigs[dom]; seqCount = (256 / MAX(fhssConfig->freqCount, 1)) * fhssConfig->freqCount; - syncChannel = fhssConfig->freqCount / 2; + syncChannel = (fhssConfig->freqCount / 2) + 1; freqSpread = (fhssConfig->freqStop - fhssConfig->freqStart) * ELRS_FREQ_SPREAD_SCALE / MAX((fhssConfig->freqCount - 1), 1); // initialize the sequence array @@ -348,11 +348,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): @@ -365,4 +360,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 f3acc36254..ac54c84180 100644 --- a/src/main/rx/expresslrs_common.h +++ b/src/main/rx/expresslrs_common.h @@ -31,6 +31,8 @@ #include "drivers/io_types.h" #include "drivers/time.h" +#include "rx/expresslrs_telemetry.h" + #define ELRS_CRC_LEN 256 #define ELRS_CRC14_POLY 0x2E57 @@ -82,8 +84,8 @@ typedef enum { } elrsFreqDomain_e; typedef enum { - SM_HYBRID = 0, - SM_HYBRID_WIDE = 1 + SM_HYBRID_WIDE = 0, + SM_HYBRID = 1 } elrsSwitchMode_e; typedef enum { @@ -125,12 +127,12 @@ 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 { @@ -140,6 +142,56 @@ typedef struct elrsFhssConfig_s { 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); @@ -180,5 +232,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..728e995abc 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,39 @@ 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); + for (unsigned n = 0; n < bytesLastPayload; ++n) + { + outData[n] = data[currentOffset + n]; + } + // 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; } break; default: - *count = 0; - *currentData = 0; - *packageIndex = 0; + packageIndex = 0; } + + return packageIndex; } void confirmCurrentTelemetryPayload(const bool telemetryConfirmValue) @@ -156,15 +156,20 @@ 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 +196,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 +207,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 +219,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 +243,23 @@ 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) { + uint8_t len = MIN((uint8_t)(mspLength - mspCurrentOffset), ELRS_MSP_BYTES_PER_CALL); + for (unsigned i = 0; i < len; i++) { + mspData[mspCurrentOffset++] = receiveData[i]; + } + 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);