From 1aca26c1a3de2b148a26a4c141d5de6811d6badb Mon Sep 17 00:00:00 2001 From: MJ666 Date: Sun, 5 Apr 2020 12:33:11 +0200 Subject: [PATCH] Initial FrSky ACCST V2.1.x support --- src/main/cli/settings.c | 6 +- src/main/pg/rx_spi_cc2500.c | 4 +- src/main/pg/rx_spi_cc2500.h | 2 +- src/main/rx/cc2500_frsky_shared.c | 224 +++++++++++++++++------------- src/main/rx/cc2500_frsky_shared.h | 11 +- src/main/rx/cc2500_frsky_x.c | 75 ++++------ src/main/rx/cc2500_frsky_x.h | 2 +- src/main/rx/rx_bind.c | 2 + src/main/rx/rx_spi.c | 2 + src/main/rx/rx_spi.h | 2 + 10 files changed, 179 insertions(+), 151 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 9ee11c8fb..673129808 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -256,7 +256,9 @@ static const char * const lookupTableRxSpi[] = { "SFHSS", "SPEKTRUM", "FRSKY_X_LBT", - "REDPINE" + "REDPINE", + "FRSKY_X_V2", + "FRSKY_X_LBT_V2", }; #endif @@ -1475,7 +1477,7 @@ const clivalue_t valueTable[] = { #ifdef USE_RX_FRSKY_SPI { "frsky_spi_autobind", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, autoBind) }, - { "frsky_spi_tx_id", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 2, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, bindTxId) }, + { "frsky_spi_tx_id", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 3, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, bindTxId) }, { "frsky_spi_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -127, 127 }, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, bindOffset) }, { "frsky_spi_bind_hop_data", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 50, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, bindHopData) }, { "frsky_x_rx_num", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, rxNum) }, diff --git a/src/main/pg/rx_spi_cc2500.c b/src/main/pg/rx_spi_cc2500.c index 07a945e67..f575fe472 100644 --- a/src/main/pg/rx_spi_cc2500.c +++ b/src/main/pg/rx_spi_cc2500.c @@ -29,7 +29,7 @@ #include "rx_spi_cc2500.h" -PG_REGISTER_WITH_RESET_TEMPLATE(rxCc2500SpiConfig_t, rxCc2500SpiConfig, PG_RX_CC2500_SPI_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(rxCc2500SpiConfig_t, rxCc2500SpiConfig, PG_RX_CC2500_SPI_CONFIG, 2); #if defined(RX_CC2500_SPI_DISABLE_CHIP_DETECTION) #define CC2500_SPI_CHIP_DETECTION false @@ -39,7 +39,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(rxCc2500SpiConfig_t, rxCc2500SpiConfig, PG_RX_CC PG_RESET_TEMPLATE(rxCc2500SpiConfig_t, rxCc2500SpiConfig, .autoBind = false, - .bindTxId = {0, 0}, + .bindTxId = {0, 0, 0}, .bindOffset = 0, .bindHopData = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, diff --git a/src/main/pg/rx_spi_cc2500.h b/src/main/pg/rx_spi_cc2500.h index e76d0522f..1baf0d2e2 100644 --- a/src/main/pg/rx_spi_cc2500.h +++ b/src/main/pg/rx_spi_cc2500.h @@ -32,7 +32,7 @@ typedef enum { typedef struct rxCc2500SpiConfig_s { uint8_t autoBind; - uint8_t bindTxId[2]; + uint8_t bindTxId[3]; int8_t bindOffset; uint8_t bindHopData[50]; uint8_t rxNum; diff --git a/src/main/rx/cc2500_frsky_shared.c b/src/main/rx/cc2500_frsky_shared.c index fc914f942..62c516b4b 100644 --- a/src/main/rx/cc2500_frsky_shared.c +++ b/src/main/rx/cc2500_frsky_shared.c @@ -48,9 +48,10 @@ #include "cc2500_frsky_shared.h" -static rx_spi_protocol_e spiProtocol; +rx_spi_protocol_e spiProtocol; static timeMs_t start_time; +static uint8_t packetLength; static uint8_t protocolState; uint32_t missingPackets; @@ -59,8 +60,8 @@ timeDelta_t timeoutUs; static uint8_t calData[255][3]; static timeMs_t timeTunedMs; uint8_t listLength; -static uint8_t bindIdx; -static int8_t bindOffset; +static uint16_t bindState; +static int8_t bindOffset, bindOffset_min, bindOffset_max; typedef uint8_t handlePacketFn(uint8_t * const packet, uint8_t * const protocolState); typedef rx_spi_received_e processFrameFn(uint8_t * const packet); @@ -74,7 +75,7 @@ const cc2500RegisterConfigElement_t cc2500FrskyBaseConfig[] = { { CC2500_02_IOCFG0, 0x01 }, { CC2500_18_MCSM0, 0x18 }, - { CC2500_07_PKTCTRL1, 0x04 }, + { CC2500_07_PKTCTRL1, 0x05 }, { CC2500_3E_PATABLE, 0xFF }, { CC2500_0C_FSCTRL0, 0x00 }, { CC2500_0D_FREQ2, 0x5C }, @@ -141,6 +142,18 @@ const cc2500RegisterConfigElement_t cc2500FrskyXLbtConfig[] = { CC2500_15_DEVIATN, 0x53 } }; +const cc2500RegisterConfigElement_t cc2500FrskyXV2Config[] = +{ + { CC2500_17_MCSM1, 0x0E }, + { CC2500_08_PKTCTRL0, 0x05 }, + { CC2500_11_MDMCFG3, 0x84 }, +}; + +const cc2500RegisterConfigElement_t cc2500FrskyXLbtV2Config[] = +{ + { CC2500_08_PKTCTRL0, 0x05 }, +}; + static void initialise() { cc2500Reset(); @@ -158,6 +171,16 @@ static void initialise() { case RX_SPI_FRSKY_X_LBT: cc2500ApplyRegisterConfig(cc2500FrskyXLbtConfig, sizeof(cc2500FrskyXLbtConfig)); + break; + case RX_SPI_FRSKY_X_V2: + cc2500ApplyRegisterConfig(cc2500FrskyXConfig, sizeof(cc2500FrskyXConfig)); + cc2500ApplyRegisterConfig(cc2500FrskyXV2Config, sizeof(cc2500FrskyXV2Config)); + + break; + case RX_SPI_FRSKY_X_LBT_V2: + cc2500ApplyRegisterConfig(cc2500FrskyXLbtConfig, sizeof(cc2500FrskyXLbtConfig)); + cc2500ApplyRegisterConfig(cc2500FrskyXLbtV2Config, sizeof(cc2500FrskyXLbtV2Config)); + break; default: @@ -207,29 +230,34 @@ static void initTuneRx(void) cc2500Strobe(CC2500_SRX); } -static bool tuneRx(uint8_t *packet) +static bool isValidBindPacket(uint8_t *packet) { - if (bindOffset >= 126) { - bindOffset = -126; + if (spiProtocol == RX_SPI_FRSKY_D || spiProtocol == RX_SPI_FRSKY_X_V2 || spiProtocol == RX_SPI_FRSKY_X_LBT_V2) { + if (!(packet[packetLength - 1] & 0x80)) { + return false; + } } - if ((millis() - timeTunedMs) > 50) { + if ((packet[0] == packetLength - 3) && (packet[1] == 0x03) && (packet[2] == 0x01)) { + return true; + } + + return false; +} + +static bool tuneRx(uint8_t *packet, int8_t inc) +{ + if ((millis() - timeTunedMs) > 50 || bindOffset == 126 || bindOffset == -126) { timeTunedMs = millis(); - bindOffset += 5; + bindOffset += inc; cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset); + cc2500Strobe(CC2500_SRX); } if (rxSpiGetExtiState()) { uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; - if (ccLen) { - cc2500ReadFifo(packet, ccLen); - if (packet[ccLen - 1] & 0x80) { - if (packet[2] == 0x01) { - uint8_t Lqi = packet[ccLen - 1] & 0x7F; - // higher lqi represent better link quality - if (Lqi > 50) { - rxCc2500SpiConfigMutable()->bindOffset = bindOffset; - return true; - } - } + if (ccLen >= packetLength) { + cc2500ReadFifo(packet, packetLength); + if (isValidBindPacket(packet)) { + return true; } } } @@ -249,29 +277,73 @@ static void initGetBind(void) cc2500Strobe(CC2500_SRX); listLength = 0; - bindIdx = 0x05; + bindState = 0; } -static bool getBind1(uint8_t *packet) +static void generateV2HopData(uint16_t id) +{ + uint8_t inc = (id % 46) + 1; // Increment + if (inc == 12 || inc == 35) inc++; // Exception list from dumps + uint8_t offset = id % 5; // Start offset + + uint8_t channel; + for (uint8_t i = 0; i < 47; i++) { + channel = 5 * ((uint16_t)(inc * i) % 47) + offset; // Exception list from dumps + if (spiProtocol == RX_SPI_FRSKY_X_LBT_V2) { // LBT or FCC + // LBT + if (channel <=1 || channel == 43 || channel == 44 || channel == 87 || channel == 88 || channel == 129 || channel == 130 || channel == 173 || channel == 174) { + channel += 2; + } + else if (channel == 216 || channel == 217 || channel == 218) { + channel += 3; + } + } else { + // FCC + if (channel == 3 || channel == 4 || channel == 46 || channel == 47 || channel == 90 || channel == 91 || channel == 133 || channel == 134 || channel == 176 || channel == 177 || channel == 220 || channel == 221) { + channel += 2; + } + } + rxCc2500SpiConfigMutable()->bindHopData[i] = channel; // Store + } + rxCc2500SpiConfigMutable()->bindHopData[47] = 0; //Bind freq + rxCc2500SpiConfigMutable()->bindHopData[48] = 0; + rxCc2500SpiConfigMutable()->bindHopData[49] = 0; +} + +static bool getBind(uint8_t *packet) { // len|bind |tx // id|03|01|idx|h0|h1|h2|h3|h4|00|00|00|00|00|00|00|00|00|00|00|00|00|00|00|CHK1|CHK2|RSSI|LQI/CRC| - // Start by getting bind packet 0 and the txid if (rxSpiGetExtiState()) { uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; - if (ccLen) { - cc2500ReadFifo(packet, ccLen); - if (packet[ccLen - 1] & 0x80) { - if (packet[2] == 0x01) { + if (ccLen >= packetLength) { + cc2500ReadFifo(packet, packetLength); + if (isValidBindPacket(packet)) { + if (spiProtocol == RX_SPI_FRSKY_X_V2 || spiProtocol == RX_SPI_FRSKY_X_LBT_V2) { + for (uint8_t i = 3; i < packetLength - 4; i++) { + packet[i] ^= 0xA7; + } + rxCc2500SpiConfigMutable()->bindTxId[0] = packet[3]; + rxCc2500SpiConfigMutable()->bindTxId[1] = packet[4]; + rxCc2500SpiConfigMutable()->bindTxId[2] = packet[5]; + rxCc2500SpiConfigMutable()->rxNum = packet[6]; + generateV2HopData((packet[4] << 8) + packet[3]); + listLength = 47; + + return true; + } else { if (packet[5] == 0x00) { rxCc2500SpiConfigMutable()->bindTxId[0] = packet[3]; rxCc2500SpiConfigMutable()->bindTxId[1] = packet[4]; - for (uint8_t n = 0; n < 5; n++) { - rxCc2500SpiConfigMutable()->bindHopData[packet[5] + n] = - packet[6 + n]; - } - + rxCc2500SpiConfigMutable()->bindTxId[2] = packet[11]; rxCc2500SpiConfigMutable()->rxNum = packet[12]; + } + for (uint8_t n = 0; n < 5; n++) { + rxCc2500SpiConfigMutable()->bindHopData[packet[5] + n] = (packet[5] + n) >= 47 ? 0 : packet[6 + n]; + } + bindState |= 1 << (packet[5] / 5); + if (bindState == 0x3FF) { + listLength = 47; return true; } @@ -283,57 +355,6 @@ static bool getBind1(uint8_t *packet) return false; } -static bool getBind2(uint8_t *packet) -{ - if (bindIdx <= 120) { - if (rxSpiGetExtiState()) { - uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; - if (ccLen) { - cc2500ReadFifo(packet, ccLen); - if (packet[ccLen - 1] & 0x80) { - if (packet[2] == 0x01) { - if ((packet[3] == rxCc2500SpiConfig()->bindTxId[0]) && - (packet[4] == rxCc2500SpiConfig()->bindTxId[1])) { - if (packet[5] == bindIdx) { -#if defined(DJTS) - if (packet[5] == 0x2D) { - for (uint8_t i = 0; i < 2; i++) { - rxCc2500SpiConfigMutable()->bindHopData[packet[5] + i] = packet[6 + i]; - } - listLength = 47; - - return true; - } -#endif - - for (uint8_t n = 0; n < 5; n++) { - if (packet[6 + n] == packet[ccLen - 3] || (packet[6 + n] == 0)) { - if (bindIdx >= 0x2D) { - listLength = packet[5] + n; - - return true; - } - } - - rxCc2500SpiConfigMutable()->bindHopData[packet[5] + n] = packet[6 + n]; - } - - bindIdx = bindIdx + 5; - - return false; - } - } - } - } - } - } - - return false; - } else { - return true; - } -} - rx_spi_received_e frSkySpiDataReceived(uint8_t *packet) { rx_spi_received_e ret = RX_SPI_RECEIVED_NONE; @@ -352,29 +373,38 @@ rx_spi_received_e frSkySpiDataReceived(uint8_t *packet) rxSpiLedOn(); initTuneRx(); - protocolState = STATE_BIND_TUNING; + protocolState = STATE_BIND_TUNING_LOW; } else { protocolState = STATE_STARTING; } break; - case STATE_BIND_TUNING: - if (tuneRx(packet)) { + case STATE_BIND_TUNING_LOW: + if (tuneRx(packet, 2)) { + bindOffset_min = bindOffset; + bindOffset = 126; + + protocolState = STATE_BIND_TUNING_HIGH; + } + + break; + case STATE_BIND_TUNING_HIGH: + if (tuneRx(packet, -2)) { + bindOffset_max = bindOffset; + bindOffset = ((int16_t)bindOffset_max + (int16_t)bindOffset_min) / 2; + rxCc2500SpiConfigMutable()->bindOffset = bindOffset; initGetBind(); initialiseData(true); - protocolState = STATE_BIND_BINDING1; + if(bindOffset_min < bindOffset_max) + protocolState = STATE_BIND_BINDING; + else + protocolState = STATE_BIND; } break; - case STATE_BIND_BINDING1: - if (getBind1(packet)) { - protocolState = STATE_BIND_BINDING2; - } - - break; - case STATE_BIND_BINDING2: - if (getBind2(packet)) { + case STATE_BIND_BINDING: + if (getBind(packet)) { cc2500Strobe(CC2500_SIDLE); protocolState = STATE_BIND_COMPLETE; @@ -462,6 +492,8 @@ bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeS break; case RX_SPI_FRSKY_X: case RX_SPI_FRSKY_X_LBT: + case RX_SPI_FRSKY_X_V2: + case RX_SPI_FRSKY_X_LBT_V2: rxRuntimeState->channelCount = RC_CHANNEL_COUNT_FRSKY_X; handlePacket = frSkyXHandlePacket; @@ -469,7 +501,7 @@ bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeS processFrame = frSkyXProcessFrame; #endif setRcData = frSkyXSetRcData; - frSkyXInit(spiProtocol); + packetLength = frSkyXInit(); break; default: diff --git a/src/main/rx/cc2500_frsky_shared.h b/src/main/rx/cc2500_frsky_shared.h index 16754ac72..e6ea51a6d 100644 --- a/src/main/rx/cc2500_frsky_shared.h +++ b/src/main/rx/cc2500_frsky_shared.h @@ -33,12 +33,16 @@ #define MAX_MISSING_PKT 100 +#define FRSKY_RX_D16FCC_LENGTH 0x1d + 3 +#define FRSKY_RX_D16LBT_LENGTH 0x20 + 3 +#define FRSKY_RX_D16v2_LENGTH 0x1d + 3 + enum { STATE_INIT = 0, STATE_BIND, - STATE_BIND_TUNING, - STATE_BIND_BINDING1, - STATE_BIND_BINDING2, + STATE_BIND_TUNING_LOW, + STATE_BIND_TUNING_HIGH, + STATE_BIND_BINDING, STATE_BIND_COMPLETE, STATE_STARTING, STATE_UPDATE, @@ -47,6 +51,7 @@ enum { STATE_RESUME, }; +extern rx_spi_protocol_e spiProtocol; extern uint8_t listLength; extern uint32_t missingPackets; extern timeDelta_t timeoutUs; diff --git a/src/main/rx/cc2500_frsky_x.c b/src/main/rx/cc2500_frsky_x.c index 48f020265..c9155dbad 100644 --- a/src/main/rx/cc2500_frsky_x.c +++ b/src/main/rx/cc2500_frsky_x.c @@ -59,39 +59,9 @@ #include "cc2500_frsky_x.h" -const uint16_t crcTable[] = { +const uint16_t crcTable_Short[] = { 0x0000,0x1189,0x2312,0x329b,0x4624,0x57ad,0x6536,0x74bf, 0x8c48,0x9dc1,0xaf5a,0xbed3,0xca6c,0xdbe5,0xe97e,0xf8f7, - 0x1081,0x0108,0x3393,0x221a,0x56a5,0x472c,0x75b7,0x643e, - 0x9cc9,0x8d40,0xbfdb,0xae52,0xdaed,0xcb64,0xf9ff,0xe876, - 0x2102,0x308b,0x0210,0x1399,0x6726,0x76af,0x4434,0x55bd, - 0xad4a,0xbcc3,0x8e58,0x9fd1,0xeb6e,0xfae7,0xc87c,0xd9f5, - 0x3183,0x200a,0x1291,0x0318,0x77a7,0x662e,0x54b5,0x453c, - 0xbdcb,0xac42,0x9ed9,0x8f50,0xfbef,0xea66,0xd8fd,0xc974, - 0x4204,0x538d,0x6116,0x709f,0x0420,0x15a9,0x2732,0x36bb, - 0xce4c,0xdfc5,0xed5e,0xfcd7,0x8868,0x99e1,0xab7a,0xbaf3, - 0x5285,0x430c,0x7197,0x601e,0x14a1,0x0528,0x37b3,0x263a, - 0xdecd,0xcf44,0xfddf,0xec56,0x98e9,0x8960,0xbbfb,0xaa72, - 0x6306,0x728f,0x4014,0x519d,0x2522,0x34ab,0x0630,0x17b9, - 0xef4e,0xfec7,0xcc5c,0xddd5,0xa96a,0xb8e3,0x8a78,0x9bf1, - 0x7387,0x620e,0x5095,0x411c,0x35a3,0x242a,0x16b1,0x0738, - 0xffcf,0xee46,0xdcdd,0xcd54,0xb9eb,0xa862,0x9af9,0x8b70, - 0x8408,0x9581,0xa71a,0xb693,0xc22c,0xd3a5,0xe13e,0xf0b7, - 0x0840,0x19c9,0x2b52,0x3adb,0x4e64,0x5fed,0x6d76,0x7cff, - 0x9489,0x8500,0xb79b,0xa612,0xd2ad,0xc324,0xf1bf,0xe036, - 0x18c1,0x0948,0x3bd3,0x2a5a,0x5ee5,0x4f6c,0x7df7,0x6c7e, - 0xa50a,0xb483,0x8618,0x9791,0xe32e,0xf2a7,0xc03c,0xd1b5, - 0x2942,0x38cb,0x0a50,0x1bd9,0x6f66,0x7eef,0x4c74,0x5dfd, - 0xb58b,0xa402,0x9699,0x8710,0xf3af,0xe226,0xd0bd,0xc134, - 0x39c3,0x284a,0x1ad1,0x0b58,0x7fe7,0x6e6e,0x5cf5,0x4d7c, - 0xc60c,0xd785,0xe51e,0xf497,0x8028,0x91a1,0xa33a,0xb2b3, - 0x4a44,0x5bcd,0x6956,0x78df,0x0c60,0x1de9,0x2f72,0x3efb, - 0xd68d,0xc704,0xf59f,0xe416,0x90a9,0x8120,0xb3bb,0xa232, - 0x5ac5,0x4b4c,0x79d7,0x685e,0x1ce1,0x0d68,0x3ff3,0x2e7a, - 0xe70e,0xf687,0xc41c,0xd595,0xa12a,0xb0a3,0x8238,0x93b1, - 0x6b46,0x7acf,0x4854,0x59dd,0x2d62,0x3ceb,0x0e70,0x1ff9, - 0xf78f,0xe606,0xd49d,0xc514,0xb1ab,0xa022,0x92b9,0x8330, - 0x7bc7,0x6a4e,0x58d5,0x495c,0x3de3,0x2c6a,0x1ef1,0x0f78 }; #define TELEMETRY_OUT_BUFFER_SIZE 64 @@ -164,10 +134,17 @@ static uint8_t remoteToProcessIndex = 0; static uint8_t packetLength; static uint16_t telemetryDelayUs; +static uint16_t crcTable(uint8_t val) { + uint16_t word; + word = (*(&crcTable_Short[val & 0x0f])); + val /= 16; + return word ^ (0x1081 * val); +} + 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]); + crc = (crc << 8) ^ crcTable((uint8_t)(crc >> 8) ^ *data++); } return crc; } @@ -200,7 +177,7 @@ static void buildTelemetryFrame(uint8_t *packet) frame[0] = 0x0E;//length frame[1] = rxCc2500SpiConfig()->bindTxId[0]; frame[2] = rxCc2500SpiConfig()->bindTxId[1]; - frame[3] = packet[3]; + frame[3] = rxCc2500SpiConfig()->bindTxId[2]; if (evenRun) { frame[4] = (uint8_t)cc2500getRssiDbm() | 0x80; @@ -314,11 +291,17 @@ void frSkyXSetRcData(uint16_t *rcData, const uint8_t *packet) bool isValidPacket(const uint8_t *packet) { + if (spiProtocol == RX_SPI_FRSKY_X_V2 || spiProtocol == RX_SPI_FRSKY_X_LBT_V2) { + if (!(packet[packetLength - 1] & 0x80)) { + return false; + } + } 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] == rxCc2500SpiConfig()->bindTxId[0]) && (packet[2] == rxCc2500SpiConfig()->bindTxId[1]) && + (packet[3] == rxCc2500SpiConfig()->bindTxId[2]) && (rxCc2500SpiConfig()->rxNum == 0 || packet[6] == 0 || packet[6] == rxCc2500SpiConfig()->rxNum)) { return true; } @@ -380,16 +363,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro 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; - } - } + channelsToSkip = (packet[5] << 2) | (packet[4] >> 6); telemetryReceived = true; // now telemetry can be sent skipChannels = false; } @@ -569,17 +543,25 @@ rx_spi_received_e frSkyXProcessFrame(uint8_t * const packet) } #endif -void frSkyXInit(const rx_spi_protocol_e spiProtocol) +uint8_t frSkyXInit(void) { switch(spiProtocol) { case RX_SPI_FRSKY_X: - packetLength = 32; + packetLength = FRSKY_RX_D16FCC_LENGTH; telemetryDelayUs = 400; break; case RX_SPI_FRSKY_X_LBT: - packetLength = 35; + packetLength = FRSKY_RX_D16LBT_LENGTH; telemetryDelayUs = 1400; break; + case RX_SPI_FRSKY_X_V2: + packetLength = FRSKY_RX_D16v2_LENGTH; + telemetryDelayUs = 400; + break; + case RX_SPI_FRSKY_X_LBT_V2: + packetLength = FRSKY_RX_D16v2_LENGTH; + telemetryDelayUs = 1500; + break; default: break; } @@ -588,6 +570,7 @@ void frSkyXInit(const rx_spi_protocol_e spiProtocol) telemetryEnabled = initSmartPortTelemetryExternal(frSkyXTelemetryWriteFrame); } #endif + return packetLength; } #endif diff --git a/src/main/rx/cc2500_frsky_x.h b/src/main/rx/cc2500_frsky_x.h index 5a464d0b5..8afde23e3 100644 --- a/src/main/rx/cc2500_frsky_x.h +++ b/src/main/rx/cc2500_frsky_x.h @@ -27,6 +27,6 @@ void frSkyXSetRcData(uint16_t *rcData, const uint8_t *payload); -void frSkyXInit(const rx_spi_protocol_e spiProtocol); +uint8_t frSkyXInit(void); rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const protocolState); rx_spi_received_e frSkyXProcessFrame(uint8_t * const packet); diff --git a/src/main/rx/rx_bind.c b/src/main/rx/rx_bind.c index c7657632a..c0729b22e 100644 --- a/src/main/rx/rx_bind.c +++ b/src/main/rx/rx_bind.c @@ -59,6 +59,8 @@ static bool doRxBind(bool doBind) #if defined(USE_RX_FRSKY_SPI_X) case RX_SPI_FRSKY_X: case RX_SPI_FRSKY_X_LBT: + case RX_SPI_FRSKY_X_V2: + case RX_SPI_FRSKY_X_LBT_V2: #endif #if defined(USE_RX_REDPINE_SPI) case RX_SPI_REDPINE: diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c index e6deffb2b..7b41ad1c2 100644 --- a/src/main/rx/rx_spi.c +++ b/src/main/rx/rx_spi.c @@ -136,6 +136,8 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol) #if defined(USE_RX_FRSKY_SPI_X) case RX_SPI_FRSKY_X: case RX_SPI_FRSKY_X_LBT: + case RX_SPI_FRSKY_X_V2: + case RX_SPI_FRSKY_X_LBT_V2: protocolInit = frSkySpiInit; protocolDataReceived = frSkySpiDataReceived; protocolSetRcDataFromPayload = frSkySpiSetRcData; diff --git a/src/main/rx/rx_spi.h b/src/main/rx/rx_spi.h index 02f5e926c..1db790215 100644 --- a/src/main/rx/rx_spi.h +++ b/src/main/rx/rx_spi.h @@ -46,6 +46,8 @@ typedef enum { RX_SPI_CYRF6936_DSM, RX_SPI_FRSKY_X_LBT, RX_SPI_REDPINE, + RX_SPI_FRSKY_X_V2, + RX_SPI_FRSKY_X_LBT_V2, RX_SPI_PROTOCOL_COUNT } rx_spi_protocol_e;