diff --git a/src/main/build/debug.c b/src/main/build/debug.c index c2ecc276c..bbe22206d 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -77,4 +77,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "RC_SMOOTHING_RATE", "ANTI_GRAVITY", "DYN_LPF", + "RX_SPEKTRUM_SPI", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 5958dec11..0cc50a66f 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -95,6 +95,7 @@ typedef enum { DEBUG_RC_SMOOTHING_RATE, DEBUG_ANTI_GRAVITY, DEBUG_DYN_LPF, + DEBUG_RX_SPEKTRUM_SPI, DEBUG_COUNT } debugType_e; diff --git a/src/main/drivers/rx/rx_cyrf6936.c b/src/main/drivers/rx/rx_cyrf6936.c new file mode 100644 index 000000000..4b2527ad3 --- /dev/null +++ b/src/main/drivers/rx/rx_cyrf6936.c @@ -0,0 +1,180 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_RX_SPEKTRUM + +#include "drivers/bus_spi.h" +#include "drivers/exti.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/nvic.h" +#include "drivers/rx/rx_cyrf6936.h" +#include "drivers/rx/rx_spi.h" +#include "drivers/time.h" + +static IO_t rxIntIO = IO_NONE; +static extiCallbackRec_t cyrf6936extiCallbackRec; +static volatile uint32_t timeEvent = 0; +static volatile bool occurEvent = false; +volatile bool isError = false; + +void cyrf6936ExtiHandler(extiCallbackRec_t *cb) +{ + UNUSED(cb); + + if (IORead(rxIntIO) == 0) { + timeEvent = micros(); + occurEvent = true; + } +} + +bool cyrf6936RxFinished(uint32_t *timeStamp) +{ + if (occurEvent) { + if (timeStamp) { + *timeStamp = timeEvent; + } + + uint8_t rxIrqStatus = cyrf6936ReadRegister(CYRF6936_RX_IRQ_STATUS); + if ((rxIrqStatus & CYRF6936_RXC_IRQ) || (rxIrqStatus & CYRF6936_RXE_IRQ)) { + isError = (rxIrqStatus & CYRF6936_RXE_IRQ) > 0x0; + } + + occurEvent = false; + return true; + } + return false; +} + +bool cyrf6936Init(void) +{ + spiDeviceByInstance(RX_SPI_INSTANCE); + rxIntIO = IOGetByTag(IO_TAG(RX_IRQ_PIN)); + IOInit(rxIntIO, OWNER_RX_SPI_CS, 0); + EXTIHandlerInit(&cyrf6936extiCallbackRec, cyrf6936ExtiHandler); + EXTIConfig(rxIntIO, &cyrf6936extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IPD, EXTI_TRIGGER_FALLING); + EXTIEnable(rxIntIO, false); + + uint16_t timeout = 1000; + do { // Check if chip has waken up + cyrf6936WriteRegister(CYRF6936_XACT_CFG, 0x82); + } while ((cyrf6936ReadRegister(CYRF6936_XACT_CFG) != 0x82) && timeout--); + + // Soft reset + cyrf6936WriteRegister(CYRF6936_MODE_OVERRIDE, CYRF6936_RST); + + // Verify the CYRF chip is responding + return cyrf6936ReadRegister(CYRF6936_FRAMING_CFG) == 0xA5; +} + +void cyrf6936WriteRegister(const uint8_t address, const uint8_t data) +{ + rxSpiWriteCommand(CYRF6936_DIR | address, data); +} + +void cyrf6936WriteBlock(const uint8_t address, const uint8_t *data, const uint8_t length) +{ + rxSpiWriteCommandMulti(CYRF6936_DIR | address, &data[0], length); +} + +uint8_t cyrf6936ReadRegister(const uint8_t address) +{ + return rxSpiReadCommand(address, 0xFF); +} + +void cyrf6936ReadBlock(const uint8_t address, uint8_t data[], const uint8_t length) +{ + rxSpiReadCommandMulti(address, 0xFF, &data[0], length); +} + +uint8_t cyrf6936GetRssi(void) +{ + return cyrf6936ReadRegister(CYRF6936_RSSI) & 0x1F; //5 bit value 0 - 31 +} + +uint8_t cyrf6936GetRxStatus(void) +{ + return cyrf6936ReadRegister(CYRF6936_RX_STATUS); +} + +void cyrf6936SetConfigLen(const uint8_t config[][2], const uint8_t length) +{ + for (unsigned i = 0; i < length; i++) { + cyrf6936WriteRegister(config[i][0], config[i][1]); + } +} + +void cyrf6936SetChannel(const uint8_t chan) +{ + cyrf6936WriteRegister(CYRF6936_CHANNEL, chan); +} + +void cyrf6936SetMode(const uint8_t mode, const bool force) +{ + if (force) { + cyrf6936WriteRegister(CYRF6936_XACT_CFG, mode | CYRF6936_FRC_END); + } else { + cyrf6936WriteRegister(CYRF6936_XACT_CFG, mode); + } +} + +void cyrf6936SetCrcSeed(const uint16_t crc) +{ + cyrf6936WriteRegister(CYRF6936_CRC_SEED_LSB, crc & 0xff); + cyrf6936WriteRegister(CYRF6936_CRC_SEED_MSB, crc >> 8); +} + +void cyrf6936SetSopCode(const uint8_t *sopcode) +{ + cyrf6936WriteBlock(CYRF6936_SOP_CODE, sopcode, 8); +} + +void cyrf6936SetDataCode(const uint8_t *datacode) +{ + cyrf6936WriteBlock(CYRF6936_DATA_CODE, datacode, 16); +} + +void cyrf6936SendLen(const uint8_t *data, const uint8_t length) +{ + cyrf6936WriteRegister(CYRF6936_TX_LENGTH, length); + cyrf6936WriteRegister(CYRF6936_TX_CTRL, CYRF6936_TX_CLR); + cyrf6936WriteBlock(CYRF6936_TX_BUFFER, data, length); + cyrf6936WriteRegister(CYRF6936_TX_CTRL, CYRF6936_TX_GO); +} + +void cyrf6936StartRecv(void) +{ + cyrf6936WriteRegister(CYRF6936_RX_IRQ_STATUS, CYRF6936_RXOW_IRQ); + cyrf6936WriteRegister(CYRF6936_RX_CTRL, CYRF6936_RX_GO | CYRF6936_RXC_IRQEN | CYRF6936_RXE_IRQEN); + EXTIEnable(rxIntIO, true); +} + +void cyrf6936RecvLen(uint8_t *data, const uint8_t length) +{ + cyrf6936ReadBlock(CYRF6936_RX_BUFFER, data, length); +} + +#endif /* USE_RX_SPEKTRUM */ \ No newline at end of file diff --git a/src/main/drivers/rx/rx_cyrf6936.h b/src/main/drivers/rx/rx_cyrf6936.h new file mode 100644 index 000000000..99a2c728a --- /dev/null +++ b/src/main/drivers/rx/rx_cyrf6936.h @@ -0,0 +1,226 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +enum { + CYRF6936_CHANNEL = 0x00, + CYRF6936_TX_LENGTH = 0x01, + CYRF6936_TX_CTRL = 0x02, + CYRF6936_TX_CFG = 0x03, + CYRF6936_TX_IRQ_STATUS = 0x04, + CYRF6936_RX_CTRL = 0x05, + CYRF6936_RX_CFG = 0x06, + CYRF6936_RX_IRQ_STATUS = 0x07, + CYRF6936_RX_STATUS = 0x08, + CYRF6936_RX_COUNT = 0x09, + CYRF6936_RX_LENGTH = 0x0A, + CYRF6936_PWR_CTRL = 0x0B, + CYRF6936_XTAL_CTRL = 0x0C, + CYRF6936_IO_CFG = 0x0D, + CYRF6936_GPIO_CTRL = 0x0E, + CYRF6936_XACT_CFG = 0x0F, + CYRF6936_FRAMING_CFG = 0x10, + CYRF6936_DATA32_THOLD = 0x11, + CYRF6936_DATA64_THOLD = 0x12, + CYRF6936_RSSI = 0x13, + CYRF6936_EOP_CTRL = 0x14, + CYRF6936_CRC_SEED_LSB = 0x15, + CYRF6936_CRC_SEED_MSB = 0x16, + CYRF6936_TX_CRC_LSB = 0x17, + CYRF6936_TX_CRC_MSB = 0x18, + CYRF6936_RX_CRC_LSB = 0x19, + CYRF6936_RX_CRC_MSB = 0x1A, + CYRF6936_TX_OFFSET_LSB = 0x1B, + CYRF6936_TX_OFFSET_MSB = 0x1C, + CYRF6936_MODE_OVERRIDE = 0x1D, + CYRF6936_RX_OVERRIDE = 0x1E, + CYRF6936_TX_OVERRIDE = 0x1F, + CYRF6936_TX_BUFFER = 0x20, + CYRF6936_RX_BUFFER = 0x21, + CYRF6936_SOP_CODE = 0x22, + CYRF6936_DATA_CODE = 0x23, + CYRF6936_PREAMBLE = 0x24, + CYRF6936_MFG_ID = 0x25, + CYRF6936_XTAL_CFG = 0x26, + CYRF6936_CLK_OFFSET = 0x27, + CYRF6936_CLK_EN = 0x28, + CYRF6936_RX_ABORT = 0x29, + CYRF6936_AUTO_CAL_TIME = 0x32, + CYRF6936_AUTO_CAL_OFFSET = 0x35, + CYRF6936_ANALOG_CTRL = 0x39, +}; +// ENABLE WRITING +#define CYRF6936_DIR (1<<7) + +// CYRF6936_MODE_OVERRIDE +#define CYRF6936_RST (1<<0) + +// CYRF6936_CLK_EN +#define CYRF6936_RXF (1<<1) + +// CYRF6936_XACT_CFG +enum { + CYRF6936_MODE_SLEEP = (0x0 << 2), + CYRF6936_MODE_IDLE = (0x1 << 2), + CYRF6936_MODE_SYNTH_TX = (0x2 << 2), + CYRF6936_MODE_SYNTH_RX = (0x3 << 2), + CYRF6936_MODE_RX = (0x4 << 2), +}; +#define CYRF6936_FRC_END (1<<5) +#define CYRF6936_ACK_EN (1<<7) + +// CYRF6936_IO_CFG +#define CYRF6936_IRQ_GPIO (1<<0) +#define CYRF6936_SPI_3PIN (1<<1) +#define CYRF6936_PACTL_GPIO (1<<2) +#define CYRF6936_PACTL_OD (1<<3) +#define CYRF6936_XOUT_OD (1<<4) +#define CYRF6936_MISO_OD (1<<5) +#define CYRF6936_IRQ_POL (1<<6) +#define CYRF6936_IRQ_OD (1<<7) + +// CYRF6936_FRAMING_CFG +#define CYRF6936_LEN_EN (1<<5) +#define CYRF6936_SOP_LEN (1<<6) +#define CYRF6936_SOP_EN (1<<7) + +// CYRF6936_RX_STATUS +enum { + CYRF6936_RX_DATA_MODE_GFSK = 0x00, + CYRF6936_RX_DATA_MODE_8DR = 0x01, + CYRF6936_RX_DATA_MODE_DDR = 0x10, + CYRF6936_RX_DATA_MODE_NV = 0x11, +}; +#define CYRF6936_RX_CODE (1<<2) +#define CYRF6936_BAD_CRC (1<<3) +#define CYRF6936_CRC0 (1<<4) +#define CYRF6936_EOP_ERR (1<<5) +#define CYRF6936_PKT_ERR (1<<6) +#define CYRF6936_RX_ACK (1<<7) + +// CYRF6936_TX_IRQ_STATUS +#define CYRF6936_TXE_IRQ (1<<0) +#define CYRF6936_TXC_IRQ (1<<1) +#define CYRF6936_TXBERR_IRQ (1<<2) +#define CYRF6936_TXB0_IRQ (1<<3) +#define CYRF6936_TXB8_IRQ (1<<4) +#define CYRF6936_TXB15_IRQ (1<<5) +#define CYRF6936_LV_IRQ (1<<6) +#define CYRF6936_OS_IRQ (1<<7) + +// CYRF6936_RX_IRQ_STATUS +#define CYRF6936_RXE_IRQ (1<<0) +#define CYRF6936_RXC_IRQ (1<<1) +#define CYRF6936_RXBERR_IRQ (1<<2) +#define CYRF6936_RXB1_IRQ (1<<3) +#define CYRF6936_RXB8_IRQ (1<<4) +#define CYRF6936_RXB16_IRQ (1<<5) +#define CYRF6936_SOPDET_IRQ (1<<6) +#define CYRF6936_RXOW_IRQ (1<<7) + +// CYRF6936_TX_CTRL +#define CYRF6936_TXE_IRQEN (1<<0) +#define CYRF6936_TXC_IRQEN (1<<1) +#define CYRF6936_TXBERR_IRQEN (1<<2) +#define CYRF6936_TXB0_IRQEN (1<<3) +#define CYRF6936_TXB8_IRQEN (1<<4) +#define CYRF6936_TXB15_IRQEN (1<<5) +#define CYRF6936_TX_CLR (1<<6) +#define CYRF6936_TX_GO (1<<7) + +// CYRF6936_RX_CTRL +#define CYRF6936_RXE_IRQEN (1<<0) +#define CYRF6936_RXC_IRQEN (1<<1) +#define CYRF6936_RXBERR_IRQEN (1<<2) +#define CYRF6936_RXB1_IRQEN (1<<3) +#define CYRF6936_RXB8_IRQEN (1<<4) +#define CYRF6936_RXB16_IRQEN (1<<5) +#define CYRF6936_RSVD (1<<6) +#define CYRF6936_RX_GO (1<<7) + +// CYRF6936_RX_OVERRIDE +#define CYRF6936_ACE (1<<1) +#define CYRF6936_DIS_RXCRC (1<<2) +#define CYRF6936_DIS_CRC0 (1<<3) +#define CYRF6936_FRC_RXDR (1<<4) +#define CYRF6936_MAN_RXACK (1<<5) +#define CYRF6936_RXTX_DLY (1<<6) +#define CYRF6936_ACK_RX (1<<7) + +// CYRF6936_TX_OVERRIDE +#define CYRF6936_TX_INV (1<<0) +#define CYRF6936_DIS_TXCRC (1<<2) +#define CYRF6936_OVRD_ACK (1<<3) +#define CYRF6936_MAN_TXACK (1<<4) +#define CYRF6936_FRC_PRE (1<<6) +#define CYRF6936_ACK_TX (1<<7) + +// CYRF6936_RX_CFG +#define CYRF6936_VLD_EN (1<<0) +#define CYRF6936_RXOW_EN (1<<1) +#define CYRF6936_FAST_TURN_EN (1<<3) +#define CYRF6936_HILO (1<<4) +#define CYRF6936_ATT (1<<5) +#define CYRF6936_LNA (1<<6) +#define CYRF6936_AGC_EN (1<<7) + +// CYRF6936_TX_CFG +enum { + CYRF6936_PA_M35 = 0x0, + CYRF6936_PA_M30 = 0x1, + CYRF6936_PA_M24 = 0x2, + CYRF6936_PA_M18 = 0x3, + CYRF6936_PA_M13 = 0x4, + CYRF6936_PA_M5 = 0x5, + CYRF6936_PA_0 = 0x6, + CYRF6936_PA_4 = 0x7, +}; +enum { + CYRF6936_DATA_MODE_GFSK = (0x0 << 3), + CYRF6936_DATA_MODE_8DR = (0x1 << 3), + CYRF6936_DATA_MODE_DDR = (0x2 << 3), + CYRF6936_DATA_MODE_SDR = (0x3 << 3), +}; +#define CYRF6936_DATA_CODE_LENGTH (1<<5) + +extern volatile bool isError; + +bool cyrf6936Init(void); + +bool cyrf6936RxFinished(uint32_t *timeStamp); + +void cyrf6936WriteRegister(const uint8_t address, const uint8_t data); +void cyrf6936WriteBlock(const uint8_t address, const uint8_t *data, const uint8_t length); +uint8_t cyrf6936ReadRegister(const uint8_t address); +void cyrf6936ReadBlock(const uint8_t address, uint8_t *data, const uint8_t length); + +uint8_t cyrf6936GetRssi(void); +uint8_t cyrf6936GetRxStatus(void); +void cyrf6936SetConfigLen(const uint8_t config[][2], const uint8_t length); +void cyrf6936SetChannel(const uint8_t chan); +void cyrf6936SetMode(const uint8_t mode, const bool force); +void cyrf6936SetCrcSeed(const uint16_t crc); +void cyrf6936SetSopCode(const uint8_t *sopcode); +void cyrf6936SetDataCode(const uint8_t *datacode); + +void cyrf6936SendLen(const uint8_t *data, const uint8_t length); +void cyrf6936StartRecv(void); +void cyrf6936RecvLen(uint8_t *data, const uint8_t length); \ No newline at end of file diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index 166c3f4bf..711bd6b8e 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -2616,7 +2616,10 @@ void cliRxSpiBind(char *cmdline){ case RX_SPI_A7105_FLYSKY: case RX_SPI_A7105_FLYSKY_2A: #endif -#if defined(USE_RX_FRSKY_SPI) || defined(USE_RX_SFHSS_SPI) || defined(USE_RX_FLYSKY) +#ifdef USE_RX_SPEKTRUM + case RX_SPI_CYRF6936_DSM: +#endif +#if defined(USE_RX_FRSKY_SPI) || defined(USE_RX_SFHSS_SPI) || defined(USE_RX_FLYSKY) || defined(USE_RX_SPEKTRUM) rxSpiBind(); cliPrint("Binding..."); break; diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 01f34ef42..eff9fa81d 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -90,6 +90,7 @@ #include "rx/cc2500_frsky_common.h" #include "rx/cc2500_sfhss.h" #include "rx/spektrum.h" +#include "rx/cyrf6936_spektrum.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" @@ -227,7 +228,8 @@ static const char * const lookupTableRxSpi[] = { "FLYSKY", "FLYSKY_2A", "KN", - "SFHSS" + "SFHSS", + "SPEKTRUM" }; #endif @@ -1259,6 +1261,11 @@ const clivalue_t valueTable[] = { #ifdef USE_MCO { "mco2_on_pc9", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MCO_CONFIG, offsetof(mcoConfig_t, enabled[1]) }, #endif +#ifdef USE_RX_SPEKTRUM + { "spektrum_spi_protocol", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, PG_RX_SPEKTRUM_SPI_CONFIG, offsetof(spektrumConfig_t, protocol) }, + { "spektrum_spi_mfg_id", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 4, PG_RX_SPEKTRUM_SPI_CONFIG, offsetof(spektrumConfig_t, mfgId) }, + { "spektrum_spi_num_channels", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, DSM_MAX_CHANNEL_COUNT }, PG_RX_SPEKTRUM_SPI_CONFIG, offsetof(spektrumConfig_t, numChannels) }, +#endif }; const uint16_t valueTableEntryCount = ARRAYLEN(valueTable); diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h index 7794f7464..495cb240f 100644 --- a/src/main/pg/pg_ids.h +++ b/src/main/pg/pg_ids.h @@ -136,7 +136,8 @@ #define PG_RCDEVICE_CONFIG 539 #define PG_GYRO_DEVICE_CONFIG 540 #define PG_MCO_CONFIG 541 -#define PG_BETAFLIGHT_END 541 +#define PG_RX_SPEKTRUM_SPI_CONFIG 542 +#define PG_BETAFLIGHT_END 542 // OSD configuration (subject to change) diff --git a/src/main/rx/cyrf6936_spektrum.c b/src/main/rx/cyrf6936_spektrum.c new file mode 100644 index 000000000..063c44bbd --- /dev/null +++ b/src/main/rx/cyrf6936_spektrum.c @@ -0,0 +1,610 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include "platform.h" + +#ifdef USE_RX_SPEKTRUM + +#include "build/debug.h" + +#include "drivers/io.h" +#include "drivers/rx/rx_cyrf6936.h" +#include "drivers/system.h" +#include "drivers/time.h" + +#include "sensors/battery.h" + +#include "fc/config.h" +#include "config/feature.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" +#include "pg/rx_spi.h" + +#include "rx/rx.h" +#include "rx/rx_spi.h" +#include "rx/rx_spi_common.h" + +#include "rx/cyrf6936_spektrum.h" + +static const uint8_t pnCodes[5][9][8] = { +{ /* Row 0 */ + /* Col 0 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8}, + /* Col 1 */ {0x88, 0x17, 0x13, 0x3B, 0x2D, 0xBF, 0x06, 0xD6}, + /* Col 2 */ {0xF1, 0x94, 0x30, 0x21, 0xA1, 0x1C, 0x88, 0xA9}, + /* Col 3 */ {0xD0, 0xD2, 0x8E, 0xBC, 0x82, 0x2F, 0xE3, 0xB4}, + /* Col 4 */ {0x8C, 0xFA, 0x47, 0x9B, 0x83, 0xA5, 0x66, 0xD0}, + /* Col 5 */ {0x07, 0xBD, 0x9F, 0x26, 0xC8, 0x31, 0x0F, 0xB8}, + /* Col 6 */ {0xEF, 0x03, 0x95, 0x89, 0xB4, 0x71, 0x61, 0x9D}, + /* Col 7 */ {0x40, 0xBA, 0x97, 0xD5, 0x86, 0x4F, 0xCC, 0xD1}, + /* Col 8 */ {0xD7, 0xA1, 0x54, 0xB1, 0x5E, 0x89, 0xAE, 0x86} +}, +{ /* Row 1 */ + /* Col 0 */ {0x83, 0xF7, 0xA8, 0x2D, 0x7A, 0x44, 0x64, 0xD3}, + /* Col 1 */ {0x3F, 0x2C, 0x4E, 0xAA, 0x71, 0x48, 0x7A, 0xC9}, + /* Col 2 */ {0x17, 0xFF, 0x9E, 0x21, 0x36, 0x90, 0xC7, 0x82}, + /* Col 3 */ {0xBC, 0x5D, 0x9A, 0x5B, 0xEE, 0x7F, 0x42, 0xEB}, + /* Col 4 */ {0x24, 0xF5, 0xDD, 0xF8, 0x7A, 0x77, 0x74, 0xE7}, + /* Col 5 */ {0x3D, 0x70, 0x7C, 0x94, 0xDC, 0x84, 0xAD, 0x95}, + /* Col 6 */ {0x1E, 0x6A, 0xF0, 0x37, 0x52, 0x7B, 0x11, 0xD4}, + /* Col 7 */ {0x62, 0xF5, 0x2B, 0xAA, 0xFC, 0x33, 0xBF, 0xAF}, + /* Col 8 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97} +}, +{ /* Row 2 */ + /* Col 0 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97}, + /* Col 1 */ {0x8E, 0x4A, 0xD0, 0xA9, 0xA7, 0xFF, 0x20, 0xCA}, + /* Col 2 */ {0x4C, 0x97, 0x9D, 0xBF, 0xB8, 0x3D, 0xB5, 0xBE}, + /* Col 3 */ {0x0C, 0x5D, 0x24, 0x30, 0x9F, 0xCA, 0x6D, 0xBD}, + /* Col 4 */ {0x50, 0x14, 0x33, 0xDE, 0xF1, 0x78, 0x95, 0xAD}, + /* Col 5 */ {0x0C, 0x3C, 0xFA, 0xF9, 0xF0, 0xF2, 0x10, 0xC9}, + /* Col 6 */ {0xF4, 0xDA, 0x06, 0xDB, 0xBF, 0x4E, 0x6F, 0xB3}, + /* Col 7 */ {0x9E, 0x08, 0xD1, 0xAE, 0x59, 0x5E, 0xE8, 0xF0}, + /* Col 8 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E} +}, +{ /* Row 3 */ + /* Col 0 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E}, + /* Col 1 */ {0x80, 0x69, 0x26, 0x80, 0x08, 0xF8, 0x49, 0xE7}, + /* Col 2 */ {0x7D, 0x2D, 0x49, 0x54, 0xD0, 0x80, 0x40, 0xC1}, + /* Col 3 */ {0xB6, 0xF2, 0xE6, 0x1B, 0x80, 0x5A, 0x36, 0xB4}, + /* Col 4 */ {0x42, 0xAE, 0x9C, 0x1C, 0xDA, 0x67, 0x05, 0xF6}, + /* Col 5 */ {0x9B, 0x75, 0xF7, 0xE0, 0x14, 0x8D, 0xB5, 0x80}, + /* Col 6 */ {0xBF, 0x54, 0x98, 0xB9, 0xB7, 0x30, 0x5A, 0x88}, + /* Col 7 */ {0x35, 0xD1, 0xFC, 0x97, 0x23, 0xD4, 0xC9, 0x88}, + /* Col 8 */ {0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40, 0x93} +}, +{ /* Row 4 */ + /* Col 0 */ {0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40, 0x93}, + /* Col 1 */ {0xDC, 0x68, 0x08, 0x99, 0x97, 0xAE, 0xAF, 0x8C}, + /* Col 2 */ {0xC3, 0x0E, 0x01, 0x16, 0x0E, 0x32, 0x06, 0xBA}, + /* Col 3 */ {0xE0, 0x83, 0x01, 0xFA, 0xAB, 0x3E, 0x8F, 0xAC}, + /* Col 4 */ {0x5C, 0xD5, 0x9C, 0xB8, 0x46, 0x9C, 0x7D, 0x84}, + /* Col 5 */ {0xF1, 0xC6, 0xFE, 0x5C, 0x9D, 0xA5, 0x4F, 0xB7}, + /* Col 6 */ {0x58, 0xB5, 0xB3, 0xDD, 0x0E, 0x28, 0xF1, 0xB0}, + /* Col 7 */ {0x5F, 0x30, 0x3B, 0x56, 0x96, 0x45, 0xF4, 0xA1}, + /* Col 8 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8} +}, +}; + +static const uint8_t cyrf6936Config[][2] = { + {CYRF6936_CLK_EN, CYRF6936_RXF}, // Enable the clock + {CYRF6936_AUTO_CAL_TIME, 0x3C}, // From manual, needed for initialization + {CYRF6936_AUTO_CAL_OFFSET, 0x14}, // From manual, needed for initialization + {CYRF6936_RX_CFG, CYRF6936_LNA | CYRF6936_FAST_TURN_EN}, // Enable low noise amplifier and fast turning + {CYRF6936_TX_OFFSET_LSB, 0x55}, // From manual, typical configuration + {CYRF6936_TX_OFFSET_MSB, 0x05}, // From manual, typical configuration + {CYRF6936_XACT_CFG, CYRF6936_MODE_SYNTH_RX | CYRF6936_FRC_END}, // Force in Synth RX mode + {CYRF6936_TX_CFG, CYRF6936_DATA_CODE_LENGTH | CYRF6936_DATA_MODE_SDR | CYRF6936_PA_4}, // Enable 64 chip codes, SDR mode and amplifier +4dBm + {CYRF6936_DATA64_THOLD, 0x0E}, // From manual, typical configuration +}; +static const uint8_t cyrf6936BindConfig[][2] = { + {CYRF6936_TX_CFG, CYRF6936_DATA_CODE_LENGTH | CYRF6936_DATA_MODE_SDR | CYRF6936_PA_4}, // Enable 64 chip codes, SDR mode and amplifier +4dBm + {CYRF6936_FRAMING_CFG, CYRF6936_SOP_LEN | 0xE}, // Set SOP CODE to 64 chips and SOP Correlator Threshold to 0xE + {CYRF6936_RX_OVERRIDE, CYRF6936_FRC_RXDR | CYRF6936_DIS_RXCRC}, // Force receive data rate and disable receive CRC checker + {CYRF6936_EOP_CTRL, 0x02}, // Only enable EOP symbol count of 2 + {CYRF6936_TX_OVERRIDE, CYRF6936_DIS_TXCRC}, // Disable transmit CRC generate +}; +static const uint8_t cyrf6936TransferConfig[][2] = { + {CYRF6936_TX_CFG, CYRF6936_DATA_CODE_LENGTH | CYRF6936_DATA_MODE_8DR | CYRF6936_PA_4}, // Enable 64 chip codes, 8DR mode and amplifier +4dBm + {CYRF6936_FRAMING_CFG, CYRF6936_SOP_EN | CYRF6936_SOP_LEN | CYRF6936_LEN_EN | 0xE}, // Set SOP CODE enable, SOP CODE to 64 chips, Packet length enable, and SOP Correlator Threshold to 0xE + {CYRF6936_TX_OVERRIDE, 0x00}, // Reset TX overrides + {CYRF6936_RX_OVERRIDE, 0x00}, // Reset RX overrides +}; + +typedef enum { + DSM2_22 = 0x01, + DSM2_11 = 0x02, + DSM2_11_DX8 = 0x12, + DSMX_22 = 0xA2, + DSMX_11 = 0xB2, +} dsm_protocol_e; + +#define IS_DSM2(x) (x == DSM2_22 || x == DSM2_11 || x == DSM2_11_DX8) +#define IS_DSMX(x) (!IS_DSM2(x)) + +#define CHECK_MFG_ID(protocol, packet, id) ((IS_DSM2(protocol) && packet[0] == (~id[2]&0xFF) && packet[1] == (~id[3]&0xFF)) || \ + (IS_DSMX(protocol) && packet[0] == id[2] && packet[1] == id[3])) + +typedef enum { + DSM_RECEIVER_BIND = 0x0, + DSM_RECEIVER_SYNC_A = 0x1, + DSM_RECEIVER_SYNC_B = 0x2, + DSM_RECEIVER_RECV = 0x3, +#ifdef USE_RX_SPEKTRUM_TELEMETRY + DSM_RECEIVER_TLM = 0x4, +#endif +} dsm_receiver_status_e; + +typedef struct dsmReceiver_s { + dsm_receiver_status_e status; + dsm_protocol_e protocol; + + uint8_t mfgId[4]; + + uint8_t rfChannel; + uint8_t rfChannelIdx; + uint8_t rfChannels[23]; + + uint8_t sopCol; + uint8_t dataCol; + uint16_t crcSeed; + + uint8_t missedPackets; + uint8_t numChannels; + + bool bound; + + uint32_t timeout; + uint32_t timeLastPacket; + +#ifdef USE_RX_SPEKTRUM_TELEMETRY + uint32_t timeLastTelemetry; + bool sendTelemetry; +#endif +} dsmReceiver_t; + +STATIC_UNIT_TESTED dsmReceiver_t dsmReceiver; + +PG_REGISTER_WITH_RESET_TEMPLATE(spektrumConfig_t, spektrumConfig, PG_RX_SPEKTRUM_SPI_CONFIG, 0); +PG_RESET_TEMPLATE(spektrumConfig_t, spektrumConfig, .protocol = 0, .mfgId = {0, 0, 0, 0}, .numChannels = 0); + +static void dsmGenerateDsmxChannels(void) +{ + unsigned idx = 0; + const uint32_t id = ~((dsmReceiver.mfgId[0] << 24) | (dsmReceiver.mfgId[1] << 16) | (dsmReceiver.mfgId[2] << 8) | (dsmReceiver.mfgId[3] << 0)); + uint32_t idTmp = id; + + while (idx < 23) { + unsigned i; + unsigned count3To27 = 0, count28To51 = 0, count52To76 = 0; + + idTmp = idTmp * 0x0019660D + 0x3C6EF35F; // Randomization + const uint8_t nextCh = ((idTmp >> 8) % 0x49) + 3; + if (((nextCh ^ id) & 0x01) == 0) { + continue; + } + + for (i = 0; i < idx; i++) { + if (dsmReceiver.rfChannels[i] == nextCh) { + break; + } + + if (dsmReceiver.rfChannels[i] <= 27) { + count3To27++; + } else if (dsmReceiver.rfChannels[i] <= 51) { + count28To51++; + } else { + count52To76++; + } + } + + if (i != idx) { + continue; + } + + if ((nextCh < 28 && count3To27 < 8) + || (nextCh >= 28 && nextCh < 52 && count28To51 < 7) + || (nextCh >= 52 && count52To76 < 8)) { + dsmReceiver.rfChannels[idx++] = nextCh; + } + } +} + +static void dsmSetChannel(const uint8_t channel, const uint8_t sopCol, const uint8_t dataCol, const uint16_t crcSeed) +{ + const uint8_t pnRow = IS_DSM2(dsmReceiver.protocol) ? channel % 5 : (channel - 2) % 5; + + cyrf6936SetCrcSeed(crcSeed); + cyrf6936SetSopCode(pnCodes[pnRow][sopCol]); + cyrf6936SetDataCode(pnCodes[pnRow][dataCol]); + + cyrf6936SetChannel(channel); +} + +static void dsmReceiverSetNextSyncChannel(void) +{ + dsmReceiver.crcSeed = ~dsmReceiver.crcSeed; + dsmReceiver.rfChannel = (dsmReceiver.rfChannel + 1) % DSM_MAX_RF_CHANNEL; + dsmSetChannel(dsmReceiver.rfChannel, dsmReceiver.sopCol, dsmReceiver.dataCol, dsmReceiver.crcSeed); +} + +static void dsmReceiverSetNextChannel(void) +{ + dsmReceiver.rfChannelIdx = IS_DSM2(dsmReceiver.protocol) ? (dsmReceiver.rfChannelIdx + 1) % 2 : (dsmReceiver.rfChannelIdx + 1) % 23; + dsmReceiver.crcSeed = ~dsmReceiver.crcSeed; + dsmReceiver.rfChannel = dsmReceiver.rfChannels[dsmReceiver.rfChannelIdx]; + dsmSetChannel(dsmReceiver.rfChannel, dsmReceiver.sopCol, dsmReceiver.dataCol, dsmReceiver.crcSeed); +} + +static void resetReceiveTimeout(const uint32_t timeStamp) +{ + dsmReceiver.timeLastPacket = timeStamp; + if (dsmReceiver.crcSeed == ((dsmReceiver.mfgId[0] << 8) + dsmReceiver.mfgId[1])) { + dsmReceiver.timeout = DSM_RECV_SHORT_TIMEOUT_US; + } else { + dsmReceiver.timeout = (dsmReceiver.numChannels < 8 ? DSM_RECV_LONG_TIMEOUT_US : DSM_RECV_MID_TIMEOUT_US); + } +} + +static void checkTimeout(void) +{ + const uint32_t time = micros(); + +#ifdef USE_RX_SPEKTRUM_TELEMETRY + if (featureIsEnabled(FEATURE_TELEMETRY) && (time - dsmReceiver.timeLastTelemetry) > DSM_TELEMETRY_TIME_US) { + dsmReceiver.timeLastTelemetry = time; + dsmReceiver.sendTelemetry = true; + } +#endif + + if ((time - dsmReceiver.timeLastPacket) > dsmReceiver.timeout) { + cyrf6936SetMode(CYRF6936_MODE_SYNTH_RX, true); + cyrf6936WriteRegister(CYRF6936_RX_ABORT, 0x00); + + dsmReceiver.timeLastPacket += dsmReceiver.timeout; + + switch (dsmReceiver.status) { + case DSM_RECEIVER_BIND: + dsmReceiver.rfChannel = (dsmReceiver.rfChannel + 2) % DSM_MAX_RF_CHANNEL; + cyrf6936SetChannel(dsmReceiver.rfChannel); + dsmReceiver.timeout = DSM_BIND_TIMEOUT_US; + cyrf6936StartRecv(); + break; + case DSM_RECEIVER_SYNC_A: + case DSM_RECEIVER_SYNC_B: + IS_DSM2(dsmReceiver.protocol) ? dsmReceiverSetNextSyncChannel() : dsmReceiverSetNextChannel(); + dsmReceiver.timeout = DSM_SYNC_TIMEOUT_US; + cyrf6936StartRecv(); + break; + case DSM_RECEIVER_RECV: + dsmReceiver.missedPackets++; + DEBUG_SET(DEBUG_RX_SPEKTRUM_SPI, 0, dsmReceiver.missedPackets); + if (dsmReceiver.missedPackets < DSM_MAX_MISSED_PACKETS) { + dsmReceiverSetNextChannel(); + if (dsmReceiver.crcSeed == ((dsmReceiver.mfgId[0] << 8) + dsmReceiver.mfgId[1])) { + dsmReceiver.timeout = DSM_RECV_SHORT_TIMEOUT_US; + } else { + dsmReceiver.timeout = DSM_RECV_TIMEOUT_OFFSET_US + (dsmReceiver.numChannels < 8 ? DSM_RECV_LONG_TIMEOUT_US : DSM_RECV_MID_TIMEOUT_US); + } + cyrf6936StartRecv(); + } else { + setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL); + dsmReceiver.status = DSM_RECEIVER_SYNC_A; + dsmReceiver.timeout = DSM_SYNC_TIMEOUT_US; + } + break; +#ifdef USE_RX_SPEKTRUM_TELEMETRY + case DSM_RECEIVER_TLM: + DEBUG_SET(DEBUG_RX_SPEKTRUM_SPI, 2, (cyrf6936ReadRegister(CYRF6936_TX_IRQ_STATUS) & CYRF6936_TXC_IRQ) == 0); + dsmReceiverSetNextChannel(); + dsmReceiver.status = DSM_RECEIVER_RECV; + dsmReceiver.timeout = (dsmReceiver.numChannels < 8 ? DSM_RECV_LONG_TIMEOUT_US : DSM_RECV_MID_TIMEOUT_US) - DSM_TELEMETRY_TIMEOUT_US; + cyrf6936StartRecv(); + break; +#endif + default: + break; + } + } +} + +static void dsmReceiverStartBind(void) +{ + uint8_t dataCode[16]; + dsmReceiver.status = DSM_RECEIVER_BIND; + + cyrf6936SetConfigLen(cyrf6936BindConfig, ARRAYLEN(cyrf6936BindConfig)); + + memcpy(dataCode, pnCodes[0][8], 8); + memcpy(dataCode + 8, pnCodes[0][8], 8); + cyrf6936SetDataCode(dataCode); + + dsmReceiver.rfChannel = DSM_INITIAL_BIND_CHANNEL; + cyrf6936SetChannel(dsmReceiver.rfChannel); + dsmReceiver.timeLastPacket = micros(); + dsmReceiver.timeout = DSM_BIND_TIMEOUT_US; + cyrf6936StartRecv(); +} + +static void dsmReceiverStartTransfer(void) +{ + dsmReceiver.status = DSM_RECEIVER_SYNC_A; + dsmReceiver.rfChannelIdx = 0; + dsmReceiver.missedPackets = 0; + + cyrf6936SetConfigLen(cyrf6936TransferConfig, ARRAYLEN(cyrf6936TransferConfig)); + + dsmReceiver.numChannels = spektrumConfig()->numChannels; + dsmReceiver.protocol = spektrumConfig()->protocol; + + dsmReceiver.crcSeed = ~((dsmReceiver.mfgId[0] << 8) + dsmReceiver.mfgId[1]); + dsmReceiver.sopCol = (dsmReceiver.mfgId[0] + dsmReceiver.mfgId[1] + dsmReceiver.mfgId[2] + 2) & 0x07; + dsmReceiver.dataCol = 7 - dsmReceiver.sopCol; + + if (IS_DSMX(dsmReceiver.protocol)) { + dsmGenerateDsmxChannels(); + dsmReceiver.rfChannelIdx = 22; + dsmReceiverSetNextChannel(); + } else { + memset(dsmReceiver.rfChannels, 0, 23); + dsmReceiverSetNextSyncChannel(); + } + + dsmReceiver.timeLastPacket = micros(); + dsmReceiver.timeout = DSM_SYNC_TIMEOUT_US << 2; + cyrf6936StartRecv(); +} + +bool spektrumSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig) +{ + rxSpiCommonIOInit(rxConfig); + + rxRuntimeConfig->channelCount = DSM_MAX_CHANNEL_COUNT; + + if (!cyrf6936Init()) { + return false; + } + + if (rssiSource == RSSI_SOURCE_NONE) { + rssiSource = RSSI_SOURCE_RX_PROTOCOL; + } + + cyrf6936SetConfigLen(cyrf6936Config, ARRAYLEN(cyrf6936Config)); + +#ifdef USE_RX_SPEKTRUM_TELEMETRY + dsmReceiver.timeLastTelemetry = micros() + DSM_TELEMETRY_TIME_US; + dsmReceiver.sendTelemetry = false; +#endif + + if (spektrumConfig()->mfgId[0] || spektrumConfig()->mfgId[1] + || spektrumConfig()->mfgId[2] || spektrumConfig()->mfgId[3]) { + dsmReceiver.bound = true; + memcpy(dsmReceiver.mfgId, spektrumConfig()->mfgId, 4); + dsmReceiverStartTransfer(); + } else { + dsmReceiver.bound = false; + dsmReceiverStartBind(); + } + + return true; +} + +#ifdef USE_RX_SPEKTRUM_TELEMETRY +static void dsmSendTelemetryPacket(void) +{ + uint8_t packet[9]; + const uint16_t voltage = getBatteryVoltage() * 10; + + packet[0] = DSM_TELEMETRY_FRAME_RPM; + packet[1] = 0xFF; //sid + packet[2] = 0xFF; //rpm + packet[3] = 0xFF; //rpm + packet[4] = voltage >> 8; + packet[5] = voltage & 0xFF; + packet[6] = 0x7F; //temperature + packet[7] = 0xFF; //temperature + packet[8] = getRssiPercent(); + cyrf6936SetMode(CYRF6936_MODE_IDLE, true); + cyrf6936SendLen(packet, 9); +} +#endif + +void spektrumSpiSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload) +{ + if (rcData && payload) { + const uint8_t divider = (uint8_t) IS_DSMX(dsmReceiver.protocol); + const uint8_t bitShift = 10 + divider; + const uint16_t valueMax = IS_DSMX(dsmReceiver.protocol) ? 0x7FF : 0x3FF; + + for (unsigned i = 0; i < 7; i++) { + const uint16_t tmp = (payload[2 * i] << 8) + payload[2 * i + 1]; + const uint8_t chan = (tmp >> bitShift) & 0x0F; + const int16_t val = (tmp & valueMax) >> divider; + + if (chan < dsmReceiver.numChannels) { + rcData[chan] = 988 + val; + } + } + } +} + +static bool isValidPacket(const uint8_t *packet) +{ + DEBUG_SET(DEBUG_RX_SPEKTRUM_SPI, 1, isError); + if (isError) { + if (dsmReceiver.status != DSM_RECEIVER_RECV && (cyrf6936GetRxStatus() & CYRF6936_BAD_CRC)) { + dsmReceiver.crcSeed = ~dsmReceiver.crcSeed; + } else { + return false; + } + } + if (!CHECK_MFG_ID(dsmReceiver.protocol, packet, dsmReceiver.mfgId)) { + return false; + } + return true; +} + +rx_spi_received_e spektrumReadPacket(uint8_t *payload, const uint32_t timeStamp) +{ + rx_spi_received_e result = RX_SPI_RECEIVED_NONE; + + uint8_t packetLength, packet[16]; + + packetLength = cyrf6936ReadRegister(CYRF6936_RX_COUNT); + cyrf6936RecvLen(packet, packetLength); + + cyrf6936WriteRegister(CYRF6936_XACT_CFG, CYRF6936_MODE_SYNTH_RX | CYRF6936_FRC_END); + cyrf6936WriteRegister(CYRF6936_RX_ABORT, 0x00); + + if (packetLength < 2) { + return result; + } + + switch (dsmReceiver.status) { + case DSM_RECEIVER_BIND: + if (packet[0] != packet[4] || packet[1] != packet[5] + || packet[2] != packet[6] || packet[3] != packet[7]) { + + dsmReceiver.timeLastPacket = timeStamp; + dsmReceiver.timeout = DSM_BIND_TIMEOUT_US; + break; + } + + unsigned i; + uint16_t bindSum = 384 - 0x10; + for (i = 0; i < 8; i++) { + bindSum += packet[i]; + } + + if (packet[8] != bindSum >> 8 || packet[9] != (bindSum & 0xFF)) { + break; + } + + for (i = 8; i < 14; i++) { + bindSum += packet[i]; + } + + if (packet[14] != bindSum >> 8 || packet[15] != (bindSum & 0xFF)) { + break; + } + + dsmReceiver.mfgId[0] = ~packet[0]; + dsmReceiver.mfgId[1] = ~packet[1]; + dsmReceiver.mfgId[2] = ~packet[2]; + dsmReceiver.mfgId[3] = ~packet[3]; + dsmReceiver.numChannels = packet[11]; + dsmReceiver.protocol = packet[12]; + memcpy(spektrumConfigMutable()->mfgId, dsmReceiver.mfgId, 4); + spektrumConfigMutable()->numChannels = dsmReceiver.numChannels; + spektrumConfigMutable()->protocol = dsmReceiver.protocol; + writeEEPROM(); + + dsmReceiverStartTransfer(); + + dsmReceiver.bound = true; + result = RX_SPI_RECEIVED_BIND; + break; + case DSM_RECEIVER_SYNC_A: + if (isValidPacket(packet)) { + if (IS_DSM2(dsmReceiver.protocol)) { + dsmReceiver.rfChannels[0] = dsmReceiver.rfChannel; + dsmReceiver.rfChannels[1] = dsmReceiver.rfChannel; + + dsmReceiver.status = DSM_RECEIVER_SYNC_B; + } else { + dsmReceiver.status = DSM_RECEIVER_RECV; + dsmReceiver.missedPackets = 0; + } + + IS_DSM2(dsmReceiver.protocol) ? dsmReceiverSetNextSyncChannel() : dsmReceiverSetNextChannel(); + resetReceiveTimeout(timeStamp); + cyrf6936StartRecv(); + } + break; + case DSM_RECEIVER_SYNC_B: + if (isValidPacket(packet)) { + if (dsmReceiver.crcSeed != ((dsmReceiver.mfgId[0] << 8) + dsmReceiver.mfgId[1])) { + dsmReceiver.rfChannels[0] = dsmReceiver.rfChannel; + } else { + dsmReceiver.rfChannels[1] = dsmReceiver.rfChannel; + } + + if (dsmReceiver.rfChannels[0] != dsmReceiver.rfChannels[1]) { + dsmReceiver.status = DSM_RECEIVER_RECV; + dsmReceiver.missedPackets = 0; + dsmReceiverSetNextChannel(); + resetReceiveTimeout(timeStamp); + cyrf6936StartRecv(); + } + } + break; + case DSM_RECEIVER_RECV: + if (isValidPacket(packet)) { + setRssi(403 + cyrf6936GetRssi() * 20, RSSI_SOURCE_RX_PROTOCOL); + dsmReceiver.missedPackets = 0; + DEBUG_SET(DEBUG_RX_SPEKTRUM_SPI, 0, dsmReceiver.missedPackets); + memcpy(payload, &packet[2], 14); +#ifdef USE_RX_SPEKTRUM_TELEMETRY + if (dsmReceiver.sendTelemetry && dsmReceiver.crcSeed == ((dsmReceiver.mfgId[0] << 8) + dsmReceiver.mfgId[1])) { + dsmSendTelemetryPacket(); + dsmReceiver.sendTelemetry = false; + dsmReceiver.timeLastPacket = timeStamp; + dsmReceiver.timeout = DSM_TELEMETRY_TIMEOUT_US; + dsmReceiver.status = DSM_RECEIVER_TLM; + } else { +#endif + dsmReceiverSetNextChannel(); + resetReceiveTimeout(timeStamp); + cyrf6936StartRecv(); +#ifdef USE_RX_SPEKTRUM_TELEMETRY + } +#endif + result = RX_SPI_RECEIVED_DATA; + } + break; + default: + break; + } + + return result; +} + +rx_spi_received_e spektrumSpiDataReceived(uint8_t *payload) +{ + rx_spi_received_e result = RX_SPI_RECEIVED_NONE; + uint32_t timeStamp; + + if (rxSpiCheckBindRequested(true)) { + dsmReceiver.bound = false; + dsmReceiverStartBind(); + } + + if (cyrf6936RxFinished(&timeStamp)) { + result = spektrumReadPacket(payload, timeStamp); + } + + checkTimeout(); + + dsmReceiver.bound ? rxSpiLedBlinkRxLoss(result) : rxSpiLedBlinkBind(); + + return result; +} + +#endif /* USE_RX_SPEKTRUM */ \ No newline at end of file diff --git a/src/main/rx/cyrf6936_spektrum.h b/src/main/rx/cyrf6936_spektrum.h new file mode 100644 index 000000000..548b66cef --- /dev/null +++ b/src/main/rx/cyrf6936_spektrum.h @@ -0,0 +1,53 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include +#include + +#define DSM_BIND_TIMEOUT_US 10000 +#define DSM_SYNC_TIMEOUT_US 20000 +#define DSM_RECV_LONG_TIMEOUT_US 18010 +#define DSM_RECV_MID_TIMEOUT_US 7000 +#define DSM_RECV_SHORT_TIMEOUT_US 4010 +#define DSM_RECV_TIMEOUT_OFFSET_US 1000 + +#define DSM_MAX_MISSED_PACKETS 31 + +#define DSM_MAX_RF_CHANNEL 0x4F +#define DSM_MAX_CHANNEL_COUNT 12 +#define DSM_INITIAL_BIND_CHANNEL 0x0D + +#define DSM_TELEMETRY_FRAME_RPM 0x7E +#define DSM_TELEMETRY_TIMEOUT_US 1500 +#define DSM_TELEMETRY_TIME_US 176000 + +typedef struct spektrumConfig_s { + uint8_t protocol; + uint8_t mfgId[4]; + uint8_t numChannels; +} spektrumConfig_t; + +PG_DECLARE(spektrumConfig_t, spektrumConfig); + +bool spektrumSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); +void spektrumSpiSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload); +rx_spi_received_e spektrumSpiDataReceived(uint8_t *payload); \ No newline at end of file diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c index e69e6a204..0810a4212 100644 --- a/src/main/rx/rx_spi.c +++ b/src/main/rx/rx_spi.c @@ -48,6 +48,7 @@ #include "rx/nrf24_kn.h" #include "rx/flysky.h" #include "rx/cc2500_sfhss.h" +#include "rx/cyrf6936_spektrum.h" uint16_t rxSpiRcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; @@ -152,6 +153,13 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol) protocolDataReceived = sfhssSpiDataReceived; protocolSetRcDataFromPayload = sfhssSpiSetRcData; break; +#endif +#ifdef USE_RX_SPEKTRUM + case RX_SPI_CYRF6936_DSM: + protocolInit = spektrumSpiInit; + protocolDataReceived = spektrumSpiDataReceived; + protocolSetRcDataFromPayload = spektrumSpiSetRcDataFromPayload; + break; #endif } return true; diff --git a/src/main/rx/rx_spi.h b/src/main/rx/rx_spi.h index d94554749..0c3ab282f 100644 --- a/src/main/rx/rx_spi.h +++ b/src/main/rx/rx_spi.h @@ -40,6 +40,7 @@ typedef enum { RX_SPI_A7105_FLYSKY_2A, RX_SPI_NRF24_KN, RX_SPI_SFHSS, + RX_SPI_CYRF6936_DSM, RX_SPI_PROTOCOL_COUNT } rx_spi_protocol_e; diff --git a/src/test/Makefile b/src/test/Makefile index 88d007e4a..fa87d9ee4 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -353,6 +353,13 @@ vtx_unittest_DEFINES := \ USE_VTX_CONTROL= \ USE_VTX_SMARTAUDIO= +rx_spi_spektrum_unittest_SRC := \ + $(USER_DIR)/rx/cyrf6936_spektrum.c + +rx_spi_spektrum_unittest_DEFINES := \ + USE_RX_SPI \ + USE_RX_SPEKTRUM + # Please tweak the following variable definitions as needed by your # project, except GTEST_HEADERS, which you can use in your own targets # but shouldn't modify. diff --git a/src/test/unit/rx_spi_spektrum_unittest.cc b/src/test/unit/rx_spi_spektrum_unittest.cc new file mode 100644 index 000000000..6b0045e24 --- /dev/null +++ b/src/test/unit/rx_spi_spektrum_unittest.cc @@ -0,0 +1,391 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include + +extern "C" { + #include "platform.h" + + #include "pg/pg.h" + #include "pg/pg_ids.h" + #include "pg/rx_spi.h" + + #include "rx/rx_spi.h" + #include "rx/cyrf6936_spektrum.h" + + typedef enum { + DSM2_22 = 0x01, + DSM2_11 = 0x02, + DSM2_11_DX8 = 0x12, + DSMX_22 = 0xA2, + DSMX_11 = 0xB2, + } dsm_protocol_e; + + #define IS_DSM2(x) (x == DSM2_22 || x == DSM2_11 || x == DSM2_11_DX8) + #define IS_DSMX(x) (!IS_DSM2(x)) + + typedef enum { + DSM_RECEIVER_BIND = 0x0, + DSM_RECEIVER_SYNC_A = 0x1, + DSM_RECEIVER_SYNC_B = 0x2, + DSM_RECEIVER_RECV = 0x3, + #ifdef USE_RX_SPEKTRUM_TELEMETRY + DSM_RECEIVER_TLM = 0x4, + #endif + } dsm_receiver_status_e; + + typedef struct dsmReceiver_s { + dsm_receiver_status_e status; + dsm_protocol_e protocol; + + uint8_t mfgId[4]; + + uint8_t rfChannel; + uint8_t rfChannelIdx; + uint8_t rfChannels[23]; + + uint8_t sopCol; + uint8_t dataCol; + uint16_t crcSeed; + + uint8_t missedPackets; + uint8_t numChannels; + + bool bound; + + uint32_t timeout; + uint32_t timeLastPacket; + + #ifdef USE_RX_SPEKTRUM_TELEMETRY + uint32_t timeLastTelemetry; + bool sendTelemetry; + #endif + } dsmReceiver_t; + + extern dsmReceiver_t dsmReceiver; + extern bool isError = false; + + static const dsmReceiver_t empty = dsmReceiver_t(); + static rxRuntimeConfig_t config = rxRuntimeConfig_t(); + static uint8_t packetLen; + static uint8_t packet[16]; + static uint16_t rssi = 0; + static uint8_t cyrfRssi; + + /* DeviationTx code */ + #define CHAN_MAX_VALUE 10000 + #define CHAN_MIN_VALUE -10000 + static void buildDataPacket(bool upper, const int16_t *sticks, uint8_t *pkt) + { + const uint8_t chMap7[] = {1, 5, 2, 3, 0, 4, 6}; + const uint8_t chMap12[] = {1, 5, 2, 4, 6, 10, 0xff, 0, 7, 3, 8, 9, 11, 0xff}; + const uint8_t chMap10[] = {1, 5, 2, 3, 4, 6, 8, 1, 5, 2, 3, 0, 7, 9}; + const uint8_t *chMap; + + uint8_t bits = IS_DSMX(dsmReceiver.protocol) ? 11 : 10; + if (dsmReceiver.numChannels < 8) { + chMap = chMap7; + } else if (dsmReceiver.numChannels < 11) { + chMap = chMap10; + } else { + chMap = chMap12; + } + uint16_t max = 1 << bits; + uint16_t pct100 = (uint32_t) max * 100 / 150; + for (uint32_t i = 0; i < 7; i++) { + uint8_t idx = chMap[upper * 7 + i]; + int32_t value; + if ((chMap[upper * 7 + i] == 0xff) || ((dsmReceiver.numChannels > 7) && (chMap[upper * 7 + i] > dsmReceiver.numChannels - 1))) { + value = 0xffff; + } else { + value = (int32_t) sticks[idx] * (pct100 / 2) / CHAN_MAX_VALUE + (max / 2); + if (value >= max) { + value = max - 1; + } else if (value < 0) { + value = 0; + } + value = (upper && i == 0 ? 0x8000 : 0) | (chMap[upper * 7 + i] << bits) | value; + } + pkt[i * 2] = (value >> 8) & 0xff; + pkt[i * 2 + 1] = (value >> 0) & 0xff; + } + } +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +//make clean test_rx_spi_spektrum_unittest +TEST(RxSpiSpektrumUnitTest, TestInitUnbound) +{ + dsmReceiver = empty; + spektrumSpiInit(nullptr, &config); + EXPECT_FALSE(dsmReceiver.bound); + EXPECT_EQ(DSM_RECEIVER_BIND, dsmReceiver.status); + EXPECT_EQ(DSM_INITIAL_BIND_CHANNEL, dsmReceiver.rfChannel); +} + +TEST(RxSpiSpektrumUnitTest, TestInitBound) +{ + const uint8_t validMfgId[4] = {0xd4, 0x62, 0xd6, 0xad}; + const uint16_t validCrcSeed = (uint16_t) ~((validMfgId[0] << 8) + validMfgId[1]); + const uint16_t changedSeed = ~validCrcSeed; + + dsmReceiver = empty; + memcpy(spektrumConfigMutable()->mfgId, validMfgId, 4); + spektrumConfigMutable()->numChannels = 7; + + spektrumConfigMutable()->protocol = DSMX_11; + + bool result = spektrumSpiInit(nullptr, &config); + + EXPECT_TRUE(result); + EXPECT_TRUE(dsmReceiver.bound); + for (int i = 0; i < 4; i++) { + EXPECT_EQ(validMfgId[i], dsmReceiver.mfgId[i]); + } + EXPECT_EQ(7, dsmReceiver.numChannels); + EXPECT_EQ(DSMX_11, dsmReceiver.protocol); + EXPECT_EQ(DSM_RECEIVER_SYNC_A, dsmReceiver.status); + EXPECT_EQ(changedSeed, dsmReceiver.crcSeed); + EXPECT_EQ(6, dsmReceiver.sopCol); + EXPECT_EQ(1, dsmReceiver.dataCol); + EXPECT_EQ(0, dsmReceiver.rfChannelIdx); + EXPECT_TRUE(dsmReceiver.rfChannels[22] != 0); + EXPECT_EQ(dsmReceiver.rfChannels[dsmReceiver.rfChannelIdx], dsmReceiver.rfChannel); + + dsmReceiver = empty; + spektrumConfigMutable()->protocol = DSM2_11; + + spektrumSpiInit(nullptr, &config); + + EXPECT_TRUE(dsmReceiver.bound); + EXPECT_EQ(DSM2_11, dsmReceiver.protocol); + EXPECT_TRUE(dsmReceiver.rfChannels[22] == 0); //uninitialized for dsm2 + EXPECT_EQ(1, dsmReceiver.rfChannel); +} + +TEST(RxSpiSpektrumUnitTest, TestDecodeBindPacket) +{ + const uint8_t bindPacket[] = {0xD7, 0xA1, 0x54, 0xB1, 0xD7, 0xA1, 0x54, 0xB1, 0x06, 0x6A, 0x01, 7, DSMX_22, 0x00, 0x07, 0x84}; + packetLen = sizeof(packet); + memcpy(packet, bindPacket, packetLen); + dsmReceiver = empty; + dsmReceiver.status = DSM_RECEIVER_BIND; + + rx_spi_received_e result = spektrumSpiDataReceived(nullptr); + EXPECT_EQ(RX_SPI_RECEIVED_BIND, result); + EXPECT_EQ(7, dsmReceiver.numChannels); + EXPECT_EQ(DSMX_22, dsmReceiver.protocol); + EXPECT_EQ(0x4E, dsmReceiver.mfgId[3]); + EXPECT_EQ(spektrumConfig()->numChannels, dsmReceiver.numChannels); + EXPECT_EQ(spektrumConfig()->protocol, dsmReceiver.protocol); + for (int i = 0; i < 4; i++) { + EXPECT_EQ(spektrumConfig()->mfgId[i], dsmReceiver.mfgId[i]); + } +} + +TEST(RxSpiSpektrumUnitTest, TestReceiverSyncLogic) +{ + const uint8_t mfgId[4] = {0xAA, 0xBB, 0xCC, 0xDD}; + packetLen = 2; + + //DSMX SYNCA -> RECV + dsmReceiver = empty; + memcpy(dsmReceiver.mfgId, mfgId, 4); + dsmReceiver.status = DSM_RECEIVER_SYNC_A; + dsmReceiver.protocol = DSMX_11; + dsmReceiver.crcSeed = 12345; + dsmReceiver.missedPackets = 1; + dsmReceiver.rfChannel = 10; + dsmReceiver.rfChannelIdx = 1; + dsmReceiver.rfChannels[2] = 5; + packet[0] = mfgId[2]; + packet[1] = mfgId[3]; + + spektrumSpiDataReceived(nullptr); + EXPECT_EQ(DSM_RECEIVER_RECV, dsmReceiver.status); + EXPECT_EQ(53190, dsmReceiver.crcSeed); + EXPECT_EQ(0, dsmReceiver.missedPackets); + EXPECT_EQ(2, dsmReceiver.rfChannelIdx); + EXPECT_EQ(5, dsmReceiver.rfChannel); + + //DSM2 SYNCA -> SYNCB + dsmReceiver = empty; + memcpy(dsmReceiver.mfgId, mfgId, 4); + dsmReceiver.status = DSM_RECEIVER_SYNC_A; + dsmReceiver.protocol = DSM2_22; + dsmReceiver.crcSeed = 12345; + dsmReceiver.missedPackets = 1; + dsmReceiver.rfChannel = 10; + dsmReceiver.rfChannelIdx = 1; + packet[0] = (~mfgId[2]&0xFF); + packet[1] = (~mfgId[3]&0xFF); + + spektrumSpiDataReceived(nullptr); + EXPECT_EQ(DSM_RECEIVER_SYNC_B, dsmReceiver.status); + EXPECT_EQ(53190, dsmReceiver.crcSeed); + EXPECT_EQ(dsmReceiver.rfChannel, dsmReceiver.rfChannels[0] + 1); + EXPECT_EQ(dsmReceiver.rfChannel, dsmReceiver.rfChannels[1] + 1); + EXPECT_EQ(1, dsmReceiver.missedPackets); + EXPECT_EQ(1, dsmReceiver.rfChannelIdx); + + //DSM2 SYNCB -> RECV + dsmReceiver.rfChannel = dsmReceiver.rfChannel+1; + spektrumSpiDataReceived((uint8_t *) packet); + EXPECT_EQ(DSM_RECEIVER_RECV, dsmReceiver.status); + EXPECT_EQ(12345, dsmReceiver.crcSeed); + EXPECT_EQ(0, dsmReceiver.missedPackets); + EXPECT_EQ(0, dsmReceiver.rfChannelIdx); + EXPECT_EQ(12, dsmReceiver.rfChannel); +} + +TEST(RxSpiSpektrumUnitTest, TestReceiveData) +{ + const uint8_t mfgId[4] = {0xAA, 0xBB, 0xCC, 0xDD}; + cyrfRssi = 31; + packetLen = 16; + dsmReceiver = empty; + memcpy(dsmReceiver.mfgId, mfgId, 4); + dsmReceiver.status = DSM_RECEIVER_RECV; + dsmReceiver.protocol = DSMX_11; + dsmReceiver.crcSeed = 12345; + dsmReceiver.numChannels = 14; + dsmReceiver.missedPackets = 1; + dsmReceiver.rfChannelIdx = 22; + dsmReceiver.rfChannels[0] = 5; + const uint8_t dataPacket[16] = {mfgId[2], mfgId[3], 0x01, 0x12, 0x23, 0x34, 0x45, 0x56, 0x67, 0x78, 0x89, 0x9A, 0xAB, 0xBC, 0xCD, 0xDE}; + memcpy(packet, dataPacket, packetLen); + + uint8_t data[16]; + rx_spi_received_e result = spektrumSpiDataReceived((uint8_t *) data); + EXPECT_EQ(RX_SPI_RECEIVED_DATA, result); + EXPECT_EQ(0, dsmReceiver.missedPackets); + EXPECT_EQ(0, dsmReceiver.rfChannelIdx); + EXPECT_EQ(5, dsmReceiver.rfChannel); + EXPECT_EQ(53190, dsmReceiver.crcSeed); + EXPECT_EQ(1023, rssi); + for (int i = 0; i < 14; i++) { + EXPECT_EQ(packet[i+2], data[i]); + } +} + +TEST(RxSpiSpektrumUnitTest, TestConvertDataPacket) +{ + dsmReceiver = empty; + + const int16_t sticks[12] = {CHAN_MAX_VALUE, CHAN_MIN_VALUE, 0, -7500, 7500, -5000, 5000, -2500, 2500, 6666, -3333, 250}; + const uint16_t bfSticks[12] = {1841, 1159, 1500, 1244, 1755, 1329, 1670, 1415, 1585, 1727, 1386, 1508}; + const uint8_t testNumChan[3] = {7, 10, 12}; + + uint8_t dataPacket[16]; + uint16_t rcData[16]; + + for (int i = 0; i < 3; i++) { + dsmReceiver.numChannels = testNumChan[i]; + memset(rcData, 0, sizeof(rcData)); + memset(dataPacket, 0, sizeof(dataPacket)); + + if (dsmReceiver.numChannels > 7) { //we need two packets to update all channels + buildDataPacket(false, sticks, dataPacket); + spektrumSpiSetRcDataFromPayload((uint16_t *) rcData, (uint8_t *) dataPacket); + memset(dataPacket, 0, sizeof(dataPacket)); + buildDataPacket(true, sticks, dataPacket); + spektrumSpiSetRcDataFromPayload((uint16_t *) rcData, (uint8_t *) dataPacket); + } else { // we need only one packet + buildDataPacket(false, sticks, dataPacket); + spektrumSpiSetRcDataFromPayload((uint16_t *) rcData, (uint8_t *) dataPacket); + } + for (int k = 0; k < 16; k++) { + if (k