From dfe1e8d3aa11d98c7849050b255df85e488ad4eb Mon Sep 17 00:00:00 2001 From: phobos- Date: Wed, 12 Dec 2018 15:53:21 +0100 Subject: [PATCH] Implemented FRSKY X EU LBT --- src/main/drivers/rx/rx_spi.h | 2 +- src/main/interface/cli.c | 1 + src/main/interface/settings.c | 1 + src/main/rx/cc2500_frsky_shared.c | 25 ++++- src/main/rx/cc2500_frsky_x.c | 171 ++++++++++++++++-------------- src/main/rx/cc2500_frsky_x.h | 2 +- src/main/rx/rx_spi.c | 1 + src/main/rx/rx_spi.h | 1 + 8 files changed, 120 insertions(+), 84 deletions(-) diff --git a/src/main/drivers/rx/rx_spi.h b/src/main/drivers/rx/rx_spi.h index 7c92e55946..a151546597 100644 --- a/src/main/drivers/rx/rx_spi.h +++ b/src/main/drivers/rx/rx_spi.h @@ -22,7 +22,7 @@ #include -#define RX_SPI_MAX_PAYLOAD_SIZE 32 +#define RX_SPI_MAX_PAYLOAD_SIZE 35 struct rxSpiConfig_s; diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index 95c3e23618..ba5607c6a7 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -2614,6 +2614,7 @@ void cliRxSpiBind(char *cmdline){ #endif #if defined(USE_RX_FRSKY_SPI_X) case RX_SPI_FRSKY_X: + case RX_SPI_FRSKY_X_LBT: #endif #endif // USE_RX_FRSKY_SPI #ifdef USE_RX_SFHSS_SPI diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 90d39c1828..3c22dcb845 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -227,6 +227,7 @@ static const char * const lookupTableRxSpi[] = { "INAV", "FRSKY_D", "FRSKY_X", + "FRSKY_X_LBT", "FLYSKY", "FLYSKY_2A", "KN", diff --git a/src/main/rx/cc2500_frsky_shared.c b/src/main/rx/cc2500_frsky_shared.c index e78cc0aff4..9b57571158 100644 --- a/src/main/rx/cc2500_frsky_shared.c +++ b/src/main/rx/cc2500_frsky_shared.c @@ -85,14 +85,11 @@ PG_RESET_TEMPLATE(rxFrSkySpiConfig_t, rxFrSkySpiConfig, static void initialise() { cc2500Reset(); cc2500WriteReg(CC2500_02_IOCFG0, 0x01); - cc2500WriteReg(CC2500_17_MCSM1, 0x0C); cc2500WriteReg(CC2500_18_MCSM0, 0x18); cc2500WriteReg(CC2500_07_PKTCTRL1, 0x04); cc2500WriteReg(CC2500_3E_PATABLE, 0xFF); cc2500WriteReg(CC2500_0C_FSCTRL0, 0x00); cc2500WriteReg(CC2500_0D_FREQ2, 0x5C); - cc2500WriteReg(CC2500_0E_FREQ1, 0x76); - cc2500WriteReg(CC2500_0F_FREQ0, 0x27); cc2500WriteReg(CC2500_13_MDMCFG1, 0x23); cc2500WriteReg(CC2500_14_MDMCFG0, 0x7A); cc2500WriteReg(CC2500_19_FOCCFG, 0x16); @@ -115,6 +112,9 @@ static void initialise() { switch (spiProtocol) { case RX_SPI_FRSKY_D: + cc2500WriteReg(CC2500_17_MCSM1, 0x0C); + cc2500WriteReg(CC2500_0E_FREQ1, 0x76); + cc2500WriteReg(CC2500_0F_FREQ0, 0x27); cc2500WriteReg(CC2500_06_PKTLEN, 0x19); cc2500WriteReg(CC2500_08_PKTCTRL0, 0x05); cc2500WriteReg(CC2500_0B_FSCTRL1, 0x08); @@ -125,6 +125,9 @@ static void initialise() { break; case RX_SPI_FRSKY_X: + cc2500WriteReg(CC2500_17_MCSM1, 0x0C); + cc2500WriteReg(CC2500_0E_FREQ1, 0x76); + cc2500WriteReg(CC2500_0F_FREQ0, 0x27); cc2500WriteReg(CC2500_06_PKTLEN, 0x1E); cc2500WriteReg(CC2500_08_PKTCTRL0, 0x01); cc2500WriteReg(CC2500_0B_FSCTRL1, 0x0A); @@ -133,6 +136,19 @@ static void initialise() { cc2500WriteReg(CC2500_12_MDMCFG2, 0x13); cc2500WriteReg(CC2500_15_DEVIATN, 0x51); + break; + case RX_SPI_FRSKY_X_LBT: + cc2500WriteReg(CC2500_17_MCSM1, 0x0E); + cc2500WriteReg(CC2500_0E_FREQ1, 0x80); + cc2500WriteReg(CC2500_0F_FREQ0, 0x00); + cc2500WriteReg(CC2500_06_PKTLEN, 0x23); + cc2500WriteReg(CC2500_08_PKTCTRL0, 0x01); + cc2500WriteReg(CC2500_0B_FSCTRL1, 0x08); + cc2500WriteReg(CC2500_10_MDMCFG4, 0x7B); + cc2500WriteReg(CC2500_11_MDMCFG3, 0xF8); + cc2500WriteReg(CC2500_12_MDMCFG2, 0x03); + cc2500WriteReg(CC2500_15_DEVIATN, 0x53); + break; default: @@ -420,11 +436,12 @@ bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntime break; case RX_SPI_FRSKY_X: + case RX_SPI_FRSKY_X_LBT: rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT_FRSKY_X; handlePacket = frSkyXHandlePacket; setRcData = frSkyXSetRcData; - frSkyXInit(); + frSkyXInit(spiProtocol); break; default: diff --git a/src/main/rx/cc2500_frsky_x.c b/src/main/rx/cc2500_frsky_x.c index 05ace2121c..1f6001577b 100644 --- a/src/main/rx/cc2500_frsky_x.c +++ b/src/main/rx/cc2500_frsky_x.c @@ -152,8 +152,10 @@ static bool telemetryEnabled = false; #endif #endif // USE_RX_FRSKY_SPI_TELEMETRY +static uint8_t packetLength; +static uint16_t telemetryDelayUs; -static uint16_t calculateCrc(uint8_t *data, uint8_t len) { +static uint16_t calculateCrc(const uint8_t *data, uint8_t len) { uint16_t crc = 0; for (unsigned i = 0; i < len; i++) { crc = (crc << 8) ^ (crcTable[((uint8_t)(crc >> 8) ^ *data++) & 0xFF]); @@ -290,6 +292,19 @@ void frSkyXSetRcData(uint16_t *rcData, const uint8_t *packet) } } +bool isValidPacket(const uint8_t *packet) +{ + uint16_t lcrc = calculateCrc(&packet[3], (packetLength - 7)); + if ((lcrc >> 8) == packet[packetLength - 4] && (lcrc & 0x00FF) == packet[packetLength - 3] && + (packet[0] == packetLength - 3) && + (packet[1] == rxFrSkySpiConfig()->bindTxId[0]) && + (packet[2] == rxFrSkySpiConfig()->bindTxId[1]) && + (rxFrSkySpiConfig()->rxNum == 0 || packet[6] == 0 || packet[6] == rxFrSkySpiConfig()->rxNum)) { + return true; + } + return false; +} + rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const protocolState) { static unsigned receiveTelemetryRetryCount = 0; @@ -343,89 +358,78 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro case STATE_DATA: if (cc2500getGdo() && (frameReceived == false)){ uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; - ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; // read 2 times to avoid reading errors - if (ccLen > 32) { - ccLen = 32; - } - if (ccLen) { - cc2500ReadFifo(packet, ccLen); - uint16_t lcrc= calculateCrc(&packet[3], (ccLen - 7)); - if((lcrc >> 8) == packet[ccLen-4] && (lcrc&0x00FF) == packet[ccLen - 3]){ // check calculateCrc - if (packet[0] == 0x1D) { - if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) && - (packet[2] == rxFrSkySpiConfig()->bindTxId[1]) && - (rxFrSkySpiConfig()->rxNum == 0 || packet[6] == 0 || packet[6] == rxFrSkySpiConfig()->rxNum)) { - missingPackets = 0; - timeoutUs = 1; - receiveDelayUs = 0; - rxSpiLedOn(); - if (skipChannels) { - channelsToSkip = packet[5] << 2; - if (packet[4] >= listLength) { - if (packet[4] < (64 + listLength)) { - channelsToSkip += 1; - } else if (packet[4] < (128 + listLength)) { - channelsToSkip += 2; - } else if (packet[4] < (192 + listLength)) { - channelsToSkip += 3; - } - } - telemetryReceived = true; // now telemetry can be sent - skipChannels = false; + if (ccLen >= packetLength) { + cc2500ReadFifo(packet, packetLength); + if (isValidPacket(packet)) { + missingPackets = 0; + timeoutUs = 1; + receiveDelayUs = 0; + rxSpiLedOn(); + if (skipChannels) { + channelsToSkip = packet[5] << 2; + if (packet[4] >= listLength) { + if (packet[4] < (64 + listLength)) { + channelsToSkip += 1; + } else if (packet[4] < (128 + listLength)) { + channelsToSkip += 2; + } else if (packet[4] < (192 + listLength)) { + channelsToSkip += 3; } - cc2500setRssiDbm(packet[ccLen - 2]); + } + telemetryReceived = true; // now telemetry can be sent + skipChannels = false; + } + cc2500setRssiDbm(packet[packetLength - 2]); - telemetrySequenceMarker_t *inFrameMarker = (telemetrySequenceMarker_t *)&packet[21]; + telemetrySequenceMarker_t *inFrameMarker = (telemetrySequenceMarker_t *)&packet[21]; - uint8_t remoteNewPacketId = inFrameMarker->data.packetSequenceId; - memcpy(&telemetryRxBuffer[remoteNewPacketId].data, &packet[22], TELEMETRY_FRAME_SIZE); - telemetryRxBuffer[remoteNewPacketId].needsProcessing = true; + uint8_t remoteNewPacketId = inFrameMarker->data.packetSequenceId; + memcpy(&telemetryRxBuffer[remoteNewPacketId].data, &packet[22], TELEMETRY_FRAME_SIZE); + telemetryRxBuffer[remoteNewPacketId].needsProcessing = true; - responseToSend.raw = 0; - uint8_t remoteToAckId = (remoteAckId + 1) % TELEMETRY_SEQUENCE_LENGTH; - if (remoteNewPacketId != remoteToAckId) { - while (remoteToAckId != remoteNewPacketId) { - if (!telemetryRxBuffer[remoteToAckId].needsProcessing) { - responseToSend.data.ackSequenceId = remoteToAckId; - responseToSend.data.retransmissionRequested = 1; + responseToSend.raw = 0; + uint8_t remoteToAckId = (remoteAckId + 1) % TELEMETRY_SEQUENCE_LENGTH; + if (remoteNewPacketId != remoteToAckId) { + while (remoteToAckId != remoteNewPacketId) { + if (!telemetryRxBuffer[remoteToAckId].needsProcessing) { + responseToSend.data.ackSequenceId = remoteToAckId; + responseToSend.data.retransmissionRequested = 1; - receiveTelemetryRetryCount++; + receiveTelemetryRetryCount++; - break; - } - - remoteToAckId = (remoteToAckId + 1) % TELEMETRY_SEQUENCE_LENGTH; - } + break; } - if (!responseToSend.data.retransmissionRequested) { - receiveTelemetryRetryCount = 0; - - remoteToAckId = (remoteAckId + 1) % TELEMETRY_SEQUENCE_LENGTH; - uint8_t remoteNextAckId; - while (telemetryRxBuffer[remoteToAckId].needsProcessing && remoteToAckId != remoteAckId) { - remoteNextAckId = remoteToAckId; - remoteToAckId = (remoteToAckId + 1) % TELEMETRY_SEQUENCE_LENGTH; - } - remoteAckId = remoteNextAckId; - responseToSend.data.ackSequenceId = remoteAckId; - } - - if (receiveTelemetryRetryCount >= 5) { - remoteProcessedId = TELEMETRY_SEQUENCE_LENGTH - 1; - remoteAckId = TELEMETRY_SEQUENCE_LENGTH - 1; - for (unsigned i = 0; i < TELEMETRY_SEQUENCE_LENGTH; i++) { - telemetryRxBuffer[i].needsProcessing = false; - } - - receiveTelemetryRetryCount = 0; - } - - packetTimerUs = micros(); - frameReceived = true; // no need to process frame again. + remoteToAckId = (remoteToAckId + 1) % TELEMETRY_SEQUENCE_LENGTH; } } - } + + if (!responseToSend.data.retransmissionRequested) { + receiveTelemetryRetryCount = 0; + + remoteToAckId = (remoteAckId + 1) % TELEMETRY_SEQUENCE_LENGTH; + uint8_t remoteNextAckId = remoteToAckId; + while (telemetryRxBuffer[remoteToAckId].needsProcessing && remoteToAckId != remoteAckId) { + remoteNextAckId = remoteToAckId; + remoteToAckId = (remoteToAckId + 1) % TELEMETRY_SEQUENCE_LENGTH; + } + remoteAckId = remoteNextAckId; + responseToSend.data.ackSequenceId = remoteAckId; + } + + if (receiveTelemetryRetryCount >= 5) { + remoteProcessedId = TELEMETRY_SEQUENCE_LENGTH - 1; + remoteAckId = TELEMETRY_SEQUENCE_LENGTH - 1; + for (unsigned i = 0; i < TELEMETRY_SEQUENCE_LENGTH; i++) { + telemetryRxBuffer[i].needsProcessing = false; + } + + receiveTelemetryRetryCount = 0; + } + + packetTimerUs = micros(); + frameReceived = true; // no need to process frame again. + } if (!frameReceived) { packetErrors++; DEBUG_SET(DEBUG_RX_FRSKY_SPI, DEBUG_DATA_BAD_FRAME, packetErrors); @@ -433,7 +437,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro } } if (telemetryReceived) { - if(cmpTimeUs(micros(), packetTimerUs) > receiveDelayUs) { // if received or not received in this time sent telemetry data + if (cmpTimeUs(micros(), packetTimerUs) > receiveDelayUs) { // if received or not received in this time sent telemetry data *protocolState = STATE_TELEMETRY; buildTelemetryFrame(packet); } @@ -449,7 +453,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro break; #ifdef USE_RX_FRSKY_SPI_TELEMETRY case STATE_TELEMETRY: - if(cmpTimeUs(micros(), packetTimerUs) >= receiveDelayUs + 400) { // if received or not received in this time sent telemetry data + if (cmpTimeUs(micros(), packetTimerUs) >= receiveDelayUs + telemetryDelayUs) { // if received or not received in this time sent telemetry data cc2500Strobe(CC2500_SIDLE); cc2500SetPower(6); cc2500Strobe(CC2500_SFRX); @@ -520,7 +524,6 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro } missingPackets++; DEBUG_SET(DEBUG_RX_FRSKY_SPI, DEBUG_DATA_MISSING_PACKETS, missingPackets); - *protocolState = STATE_DATA; } break; @@ -529,8 +532,20 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro return ret; } -void frSkyXInit(void) +void frSkyXInit(const rx_spi_protocol_e spiProtocol) { + switch(spiProtocol) { + case RX_SPI_FRSKY_X: + packetLength = 32; + telemetryDelayUs = 400; + break; + case RX_SPI_FRSKY_X_LBT: + packetLength = 35; + telemetryDelayUs = 1400; + break; + default: + break; + } #if defined(USE_TELEMETRY_SMARTPORT) if (featureIsEnabled(FEATURE_TELEMETRY)) { telemetryEnabled = initSmartPortTelemetryExternal(frSkyXTelemetryWriteFrame); diff --git a/src/main/rx/cc2500_frsky_x.h b/src/main/rx/cc2500_frsky_x.h index 2d77d28901..37792ac7b1 100644 --- a/src/main/rx/cc2500_frsky_x.h +++ b/src/main/rx/cc2500_frsky_x.h @@ -27,5 +27,5 @@ void frSkyXSetRcData(uint16_t *rcData, const uint8_t *payload); -void frSkyXInit(void); +void frSkyXInit(const rx_spi_protocol_e spiProtocol); rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const protocolState); diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c index 0810a42120..e84fc32f61 100644 --- a/src/main/rx/rx_spi.c +++ b/src/main/rx/rx_spi.c @@ -132,6 +132,7 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol) #endif #if defined(USE_RX_FRSKY_SPI_X) case RX_SPI_FRSKY_X: + case RX_SPI_FRSKY_X_LBT: #endif protocolInit = frSkySpiInit; protocolDataReceived = frSkySpiDataReceived; diff --git a/src/main/rx/rx_spi.h b/src/main/rx/rx_spi.h index 0c3ab282f5..2a6c600930 100644 --- a/src/main/rx/rx_spi.h +++ b/src/main/rx/rx_spi.h @@ -36,6 +36,7 @@ typedef enum { RX_SPI_NRF24_INAV, RX_SPI_FRSKY_D, RX_SPI_FRSKY_X, + RX_SPI_FRSKY_X_LBT, RX_SPI_A7105_FLYSKY, RX_SPI_A7105_FLYSKY_2A, RX_SPI_NRF24_KN,