diff --git a/Makefile b/Makefile
index 3852953ff0..c930eb33ca 100644
--- a/Makefile
+++ b/Makefile
@@ -411,10 +411,14 @@ COMMON_SRC = \
drivers/buf_writer.c \
drivers/bus_i2c_soft.c \
drivers/bus_spi.c \
+ drivers/bus_spi_soft.c \
drivers/exti.c \
drivers/gyro_sync.c \
drivers/io.c \
drivers/light_led.c \
+ drivers/rx_nrf24l01.c \
+ drivers/rx_spi.c \
+ drivers/rx_xn297.c \
drivers/pwm_mapping.c \
drivers/pwm_output.c \
drivers/pwm_rx.c \
@@ -444,8 +448,14 @@ COMMON_SRC = \
rx/ibus.c \
rx/jetiexbus.c \
rx/msp.c \
+ rx/nrf24_cx10.c \
+ rx/nrf24_inav.c \
+ rx/nrf24_h8_3d.c \
+ rx/nrf24_syma.c \
+ rx/nrf24_v202.c \
rx/pwm.c \
rx/rx.c \
+ rx/rx_spi.c \
rx/sbus.c \
rx/spektrum.c \
rx/sumd.c \
diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index ee1b65ce67..1dd55519a1 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -336,3 +336,17 @@ int16_t qMultiply(fix12_t q, int16_t input) {
fix12_t qConstruct(int16_t num, int16_t den) {
return (num << 12) / den;
}
+
+uint16_t crc16_ccitt(uint16_t crc, unsigned char a)
+{
+ crc ^= a << 8;
+ for (int ii = 0; ii < 8; ++ii) {
+ if (crc & 0x8000) {
+ crc = (crc << 1) ^ 0x1021;
+ } else {
+ crc = crc << 1;
+ }
+ }
+ return crc;
+}
+
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index b47a19ae7d..a672fba3a1 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -134,3 +134,5 @@ static inline float constrainf(float amt, float low, float high)
else
return amt;
}
+uint16_t crc16_ccitt(uint16_t crc, unsigned char a);
+
diff --git a/src/main/config/config.h b/src/main/config/config.h
index d0e85f3612..56f66645a8 100644
--- a/src/main/config/config.h
+++ b/src/main/config/config.h
@@ -51,7 +51,7 @@ typedef enum {
FEATURE_AIRMODE = 1 << 22,
//FEATURE_SUPEREXPO_RATES = 1 << 23,
FEATURE_VTX = 1 << 24,
- FEATURE_RX_NRF24 = 1 << 25,
+ FEATURE_RX_SPI = 1 << 25,
FEATURE_SOFTSPI = 1 << 26,
} features_e;
diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h
index 58c76c9d78..3d00aee52f 100644
--- a/src/main/drivers/bus_spi.h
+++ b/src/main/drivers/bus_spi.h
@@ -79,4 +79,5 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len
uint16_t spiGetErrorCounter(SPI_TypeDef *instance);
void spiResetErrorCounter(SPI_TypeDef *instance);
+SPIDevice spiDeviceByInstance(SPI_TypeDef *instance);
diff --git a/src/main/drivers/bus_spi_soft.c b/src/main/drivers/bus_spi_soft.c
new file mode 100644
index 0000000000..6207f6591e
--- /dev/null
+++ b/src/main/drivers/bus_spi_soft.c
@@ -0,0 +1,88 @@
+/*
+ * 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
+
+#ifdef USE_SOFTSPI
+
+#include "build/build_config.h"
+
+
+#include "io.h"
+#include "io_impl.h"
+#include "bus_spi.h"
+#include "bus_spi_soft.h"
+
+
+void softSpiInit(const softSPIDevice_t *dev)
+{
+ // SCK as output
+ IOInit(IOGetByTag(dev->sckTag), OWNER_SOFTSPI, RESOURCE_SPI_SCK, SOFT_SPIDEV_1 + 1);
+#if defined(STM32F10X)
+ IOConfigGPIO(IOGetByTag(dev->sckTag), IO_CONFIG(GPIO_Mode_Out_PP, GPIO_Speed_50MHz));
+#elif defined(STM32F3) || defined(STM32F4)
+ IOConfigGPIOAF(IOGetByTag(dev->sckTag), SPI_IO_AF_CFG, 0);
+#endif
+
+ // MOSI as output
+ IOInit(IOGetByTag(dev->mosiTag), OWNER_SOFTSPI, RESOURCE_SPI_MOSI, SOFT_SPIDEV_1 + 1);
+#if defined(STM32F10X)
+ IOConfigGPIO(IOGetByTag(dev->mosiTag), IO_CONFIG(GPIO_Mode_Out_PP, GPIO_Speed_50MHz));
+#elif defined(STM32F3) || defined(STM32F4)
+ IOConfigGPIOAF(IOGetByTag(dev->mosiTag), SPI_IO_AF_CFG, 0);
+#endif
+
+ // MISO as input
+ IOInit(IOGetByTag(dev->misoTag), OWNER_SOFTSPI, RESOURCE_SPI_MISO, SOFT_SPIDEV_1 + 1);
+#if defined(STM32F10X)
+ IOConfigGPIO(IOGetByTag(dev->misoTag), IO_CONFIG(GPIO_Mode_IN_FLOATING, GPIO_Speed_50MHz));
+#elif defined(STM32F3) || defined(STM32F4)
+ IOConfigGPIOAF(IOGetByTag(dev->misoTag), SPI_IO_AF_CFG, 0);
+#endif
+
+ // NSS as output
+ if (dev->nssTag != IOTAG_NONE) {
+ IOInit(IOGetByTag(dev->nssTag), OWNER_SOFTSPI, RESOURCE_SPI_CS, SOFT_SPIDEV_1 + 1);
+#if defined(STM32F10X)
+ IOConfigGPIO(IOGetByTag(dev->nssTag), IO_CONFIG(GPIO_Mode_Out_PP, GPIO_Speed_50MHz));
+#elif defined(STM32F3) || defined(STM32F4)
+ IOConfigGPIOAF(IOGetByTag(dev->nssTag), SPI_IO_AF_CFG, 0);
+#endif
+ }
+}
+
+uint8_t softSpiTransferByte(const softSPIDevice_t *dev, uint8_t byte)
+{
+ for(int ii = 0; ii < 8; ++ii) {
+ if (byte & 0x80) {
+ IOHi(IOGetByTag(dev->mosiTag));
+ } else {
+ IOLo(IOGetByTag(dev->mosiTag));
+ }
+ IOHi(IOGetByTag(dev->sckTag));
+ byte <<= 1;
+ if (IORead(IOGetByTag(dev->misoTag)) == 1) {
+ byte |= 1;
+ }
+ IOLo(IOGetByTag(dev->sckTag));
+ }
+ return byte;
+}
+#endif
diff --git a/src/main/drivers/bus_spi_soft.h b/src/main/drivers/bus_spi_soft.h
new file mode 100644
index 0000000000..88b106e3f4
--- /dev/null
+++ b/src/main/drivers/bus_spi_soft.h
@@ -0,0 +1,36 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include "io_types.h"
+
+typedef enum softSPIDevice {
+ SOFT_SPIDEV_1 = 0,
+ SOFT_SPIDEV_MAX = SOFT_SPIDEV_1,
+} softSPIDevice_e;
+
+typedef struct softSPIDevice_s {
+ ioTag_t sckTag;
+ ioTag_t mosiTag;
+ ioTag_t misoTag;
+ ioTag_t nssTag;
+} softSPIDevice_t;
+
+
+void softSpiInit(const softSPIDevice_t *dev);
+uint8_t softSpiTransferByte(const softSPIDevice_t *dev, uint8_t data);
diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index 06ea6438cd..5e297b3bba 100755
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -109,7 +109,9 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
#else
setup = hardwareMaps[i];
#endif
- TIM_TypeDef* ppmTimer = NULL;
+#ifndef SKIP_RX_PWM_PPM
+ TIM_TypeDef* ppmTimer = NULL;
+#endif
for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT && setup[i] != 0xFFFF; i++) {
uint8_t timerIndex = setup[i] & 0x00FF;
uint8_t type = (setup[i] & 0xFF00) >> 8;
@@ -325,12 +327,13 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
continue;
}
#endif
+#ifndef SKIP_RX_PWM_PPM
if (init->usePPM) {
if (init->pwmProtocolType != PWM_TYPE_CONVENTIONAL && timerHardwarePtr->tim == ppmTimer) {
ppmAvoidPWMTimerClash(timerHardwarePtr, ppmTimer, init->pwmProtocolType);
}
}
-
+#endif
if (init->useFastPwm) {
pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->pwmProtocolType);
pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_OUTPUT_PROTOCOL_ONESHOT;
diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h
index f9430d2844..636f5db146 100644
--- a/src/main/drivers/resource.h
+++ b/src/main/drivers/resource.h
@@ -30,6 +30,8 @@ typedef enum {
OWNER_LED,
OWNER_RX,
OWNER_TX,
+ OWNER_SOFTSPI,
+ OWNER_RX_SPI,
OWNER_TOTAL_COUNT
} resourceOwner_t;
@@ -46,6 +48,7 @@ typedef enum {
RESOURCE_I2C_SCL, RESOURCE_I2C_SDA,
RESOURCE_SPI_SCK, RESOURCE_SPI_MOSI, RESOURCE_SPI_MISO, RESOURCE_SPI_CS,
RESOURCE_ADC_BATTERY, RESOURCE_ADC_RSSI, RESOURCE_ADC_EXTERNAL1, RESOURCE_ADC_CURRENT,
+ RESOURCE_RX_CE,
RESOURCE_TOTAL_COUNT
} resourceType_t;
diff --git a/src/main/drivers/rx_nrf24l01.c b/src/main/drivers/rx_nrf24l01.c
new file mode 100644
index 0000000000..9f13b6362c
--- /dev/null
+++ b/src/main/drivers/rx_nrf24l01.c
@@ -0,0 +1,243 @@
+/*
+ * 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 .
+ */
+
+// This file is copied with modifications from project Deviation,
+// see http://deviationtx.com
+
+#include
+#include
+#include
+
+#include
+
+#ifdef USE_RX_NRF24
+
+#include "build/build_config.h"
+
+#include "system.h"
+#include "gpio.h"
+#include "io.h"
+#include "io_impl.h"
+#include "rcc.h"
+#include "rx_spi.h"
+#include "rx_nrf24l01.h"
+#include "bus_spi.h"
+
+#define NRF24_CE_HI() {IOHi(IOGetByTag(IO_TAG(RX_CE_PIN)));}
+#define NRF24_CE_LO() {IOLo(IOGetByTag(IO_TAG(RX_CE_PIN)));}
+
+// Instruction Mnemonics
+// nRF24L01: Table 16. Command set for the nRF24L01 SPI. Product Specification, p46
+// nRF24L01+: Table 20. Command set for the nRF24L01+ SPI. Product Specification, p51
+#define R_REGISTER 0x00
+#define W_REGISTER 0x20
+#define REGISTER_MASK 0x1F
+#define ACTIVATE 0x50
+#define R_RX_PL_WID 0x60
+#define R_RX_PAYLOAD 0x61
+#define W_TX_PAYLOAD 0xA0
+#define W_ACK_PAYLOAD 0xA8
+#define FLUSH_TX 0xE1
+#define FLUSH_RX 0xE2
+#define REUSE_TX_PL 0xE3
+#define NOP 0xFF
+
+uint8_t NRF24L01_WriteReg(uint8_t reg, uint8_t data)
+{
+ return rxSpiWriteCommand(W_REGISTER | (REGISTER_MASK & reg), data);
+}
+
+uint8_t NRF24L01_WriteRegisterMulti(uint8_t reg, const uint8_t *data, uint8_t length)
+{
+ return rxSpiWriteCommandMulti(W_REGISTER | ( REGISTER_MASK & reg), data, length);
+}
+
+/*
+ * Transfer the payload to the nRF24L01 TX FIFO
+ * Packets in the TX FIFO are transmitted when the
+ * nRF24L01 next enters TX mode
+ */
+uint8_t NRF24L01_WritePayload(const uint8_t *data, uint8_t length)
+{
+ return rxSpiWriteCommandMulti(W_TX_PAYLOAD, data, length);
+}
+
+uint8_t NRF24L01_WriteAckPayload(const uint8_t *data, uint8_t length, uint8_t pipe)
+{
+ return rxSpiWriteCommandMulti(W_ACK_PAYLOAD | (pipe & 0x07), data, length);
+}
+
+uint8_t NRF24L01_ReadReg(uint8_t reg)
+{
+ return rxSpiReadCommand(R_REGISTER | (REGISTER_MASK & reg), NOP);
+}
+
+uint8_t NRF24L01_ReadRegisterMulti(uint8_t reg, uint8_t *data, uint8_t length)
+{
+ return rxSpiReadCommandMulti(R_REGISTER | (REGISTER_MASK & reg), NOP, data, length);
+}
+
+/*
+ * Read a packet from the nRF24L01 RX FIFO.
+ */
+uint8_t NRF24L01_ReadPayload(uint8_t *data, uint8_t length)
+{
+ return rxSpiReadCommandMulti(R_RX_PAYLOAD, NOP, data, length);
+}
+
+/*
+ * Empty the transmit FIFO buffer.
+ */
+void NRF24L01_FlushTx()
+{
+ rxSpiWriteByte(FLUSH_TX);
+}
+
+/*
+ * Empty the receive FIFO buffer.
+ */
+void NRF24L01_FlushRx()
+{
+ rxSpiWriteByte(FLUSH_RX);
+}
+
+uint8_t NRF24L01_Activate(uint8_t code)
+{
+ return rxSpiWriteCommand(ACTIVATE, code);
+}
+
+// standby configuration, used to simplify switching between RX, TX, and Standby modes
+static uint8_t standbyConfig;
+
+void NRF24L01_Initialize(uint8_t baseConfig)
+{
+ standbyConfig = BV(NRF24L01_00_CONFIG_PWR_UP) | baseConfig;
+ NRF24_CE_LO();
+ // nRF24L01+ needs 100 milliseconds settling time from PowerOnReset to PowerDown mode
+ static const uint32_t settlingTimeUs = 100000;
+ const uint32_t currentTimeUs = micros();
+ if (currentTimeUs < settlingTimeUs) {
+ delayMicroseconds(settlingTimeUs - currentTimeUs);
+ }
+ // now in PowerDown mode
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, standbyConfig); // set PWR_UP to enter Standby mode
+ // nRF24L01+ needs 4500 microseconds from PowerDown mode to Standby mode, for crystal oscillator startup
+ delayMicroseconds(4500);
+ // now in Standby mode
+}
+
+/*
+ * Common setup of registers
+ */
+void NRF24L01_SetupBasic(void)
+{
+ NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No auto acknowledgment
+ NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, BV(NRF24L01_02_EN_RXADDR_ERX_P0));
+ NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, NRF24L01_03_SETUP_AW_5BYTES); // 5-byte RX/TX address
+ NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x00); // Disable dynamic payload length on all pipes
+}
+
+/*
+ * Enter standby mode
+ */
+void NRF24L01_SetStandbyMode(void)
+{
+ // set CE low and clear the PRIM_RX bit to enter standby mode
+ NRF24_CE_LO();
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, standbyConfig);
+}
+
+/*
+ * Enter receive mode
+ */
+void NRF24L01_SetRxMode(void)
+{
+ NRF24_CE_LO(); // drop into standby mode
+ // set the PRIM_RX bit
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, standbyConfig | BV(NRF24L01_00_CONFIG_PRIM_RX));
+ NRF24L01_ClearAllInterrupts();
+ // finally set CE high to start enter RX mode
+ NRF24_CE_HI();
+ // nRF24L01+ will now transition from Standby mode to RX mode after 130 microseconds settling time
+}
+
+/*
+ * Enter transmit mode. Anything in the transmit FIFO will be transmitted.
+ */
+void NRF24L01_SetTxMode(void)
+{
+ // Ensure in standby mode, since can only enter TX mode from standby mode
+ NRF24L01_SetStandbyMode();
+ NRF24L01_ClearAllInterrupts();
+ // pulse CE for 10 microseconds to enter TX mode
+ NRF24_CE_HI();
+ delayMicroseconds(10);
+ NRF24_CE_LO();
+ // nRF24L01+ will now transition from Standby mode to TX mode after 130 microseconds settling time.
+ // Transmission will then begin and continue until TX FIFO is empty.
+}
+
+void NRF24L01_ClearAllInterrupts(void)
+{
+ // Writing to the STATUS register clears the specified interrupt bits
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, BV(NRF24L01_07_STATUS_RX_DR) | BV(NRF24L01_07_STATUS_TX_DS) | BV(NRF24L01_07_STATUS_MAX_RT));
+}
+
+void NRF24L01_SetChannel(uint8_t channel)
+{
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, channel);
+}
+
+bool NRF24L01_ReadPayloadIfAvailable(uint8_t *data, uint8_t length)
+{
+ if (NRF24L01_ReadReg(NRF24L01_17_FIFO_STATUS) & BV(NRF24L01_17_FIFO_STATUS_RX_EMPTY)) {
+ return false;
+ }
+ NRF24L01_ReadPayload(data, length);
+ return true;
+}
+
+#ifndef UNIT_TEST
+#define DISABLE_RX() {IOHi(IOGetByTag(IO_TAG(RX_NSS_PIN)));}
+#define ENABLE_RX() {IOLo(IOGetByTag(IO_TAG(RX_NSS_PIN)));}
+/*
+ * Fast read of payload, for use in interrupt service routine
+ */
+bool NRF24L01_ReadPayloadIfAvailableFast(uint8_t *data, uint8_t length)
+{
+ // number of bits transferred = 8 * (3 + length)
+ // for 16 byte payload, that is 8*19 = 152
+ // at 50MHz clock rate that is approximately 3 microseconds
+ bool ret = false;
+ ENABLE_RX();
+ rxSpiTransferByte(R_REGISTER | (REGISTER_MASK & NRF24L01_07_STATUS));
+ const uint8_t status = rxSpiTransferByte(NOP);
+ if ((status & BV(NRF24L01_07_STATUS_RX_DR)) == 0) {
+ ret = true;
+ // clear RX_DR flag
+ rxSpiTransferByte(W_REGISTER | (REGISTER_MASK & NRF24L01_07_STATUS));
+ rxSpiTransferByte(BV(NRF24L01_07_STATUS_RX_DR));
+ rxSpiTransferByte(R_RX_PAYLOAD);
+ for (uint8_t i = 0; i < length; i++) {
+ data[i] = rxSpiTransferByte(NOP);
+ }
+ }
+ DISABLE_RX();
+ return ret;
+}
+#endif // UNIT_TEST
+#endif // USE_RX_NRF24
diff --git a/src/main/drivers/rx_nrf24l01.h b/src/main/drivers/rx_nrf24l01.h
new file mode 100644
index 0000000000..08d2c8afa7
--- /dev/null
+++ b/src/main/drivers/rx_nrf24l01.h
@@ -0,0 +1,201 @@
+/*
+ * 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 .
+ */
+
+// This file is copied with modifications from project Deviation,
+// see http://deviationtx.com, file iface_nrf24l01.h
+
+#pragma once
+
+#include
+#include
+
+#include "rx_spi.h"
+
+#define NRF24L01_MAX_PAYLOAD_SIZE 32
+
+#define BV(x) (1<<(x)) // bit value
+
+// Register map of nRF24L01
+enum {
+ NRF24L01_00_CONFIG = 0x00,
+ NRF24L01_01_EN_AA = 0x01, // Auto Acknowledge
+ NRF24L01_02_EN_RXADDR = 0x02,
+ NRF24L01_03_SETUP_AW = 0x03, // Address Width
+ NRF24L01_04_SETUP_RETR = 0x04, // automatic RETRansmission
+ NRF24L01_05_RF_CH = 0x05, // RF CHannel
+ NRF24L01_06_RF_SETUP = 0x06,
+ NRF24L01_07_STATUS = 0x07,
+ NRF24L01_08_OBSERVE_TX = 0x08,
+ NRF24L01_09_RPD = 0x09, //Received Power Detector in the nRF23L01+, called CD (Carrier Detect) in the nRF24L01
+ NRF24L01_0A_RX_ADDR_P0 = 0x0A,
+ NRF24L01_0B_RX_ADDR_P1 = 0x0B,
+ NRF24L01_0C_RX_ADDR_P2 = 0x0C,
+ NRF24L01_0D_RX_ADDR_P3 = 0x0D,
+ NRF24L01_0E_RX_ADDR_P4 = 0x0E,
+ NRF24L01_0F_RX_ADDR_P5 = 0x0F,
+ NRF24L01_10_TX_ADDR = 0x10,
+ NRF24L01_11_RX_PW_P0 = 0x11, // Payload Width
+ NRF24L01_12_RX_PW_P1 = 0x12,
+ NRF24L01_13_RX_PW_P2 = 0x13,
+ NRF24L01_14_RX_PW_P3 = 0x14,
+ NRF24L01_15_RX_PW_P4 = 0x15,
+ NRF24L01_16_RX_PW_P5 = 0x16,
+ NRF24L01_17_FIFO_STATUS = 0x17,
+ NRF24L01_1C_DYNPD = 0x1C, // DYNamic PayloaD
+ NRF24L01_1D_FEATURE = 0x1D
+};
+
+// Bit position mnemonics
+enum {
+ NRF24L01_00_CONFIG_MASK_RX_DR = 6,
+ NRF24L01_00_CONFIG_MASK_TX_DS = 5,
+ NRF24L01_00_CONFIG_MASK_MAX_RT = 4,
+ NRF24L01_00_CONFIG_EN_CRC = 3,
+ NRF24L01_00_CONFIG_CRCO = 2,
+ NRF24L01_00_CONFIG_PWR_UP = 1,
+ NRF24L01_00_CONFIG_PRIM_RX = 0,
+
+ NRF24L01_01_EN_AA_ENAA_P5 = 5,
+ NRF24L01_01_EN_AA_ENAA_P4 = 4,
+ NRF24L01_01_EN_AA_ENAA_P3 = 3,
+ NRF24L01_01_EN_AA_ENAA_P2 = 2,
+ NRF24L01_01_EN_AA_ENAA_P1 = 1,
+ NRF24L01_01_EN_AA_ENAA_P0 = 0,
+
+ NRF24L01_02_EN_RXADDR_ERX_P5 = 5,
+ NRF24L01_02_EN_RXADDR_ERX_P4 = 4,
+ NRF24L01_02_EN_RXADDR_ERX_P3 = 3,
+ NRF24L01_02_EN_RXADDR_ERX_P2 = 2,
+ NRF24L01_02_EN_RXADDR_ERX_P1 = 1,
+ NRF24L01_02_EN_RXADDR_ERX_P0 = 0,
+
+ NRF24L01_06_RF_SETUP_RF_DR_LOW = 5,
+ NRF24L01_06_RF_SETUP_RF_DR_HIGH = 3,
+ NRF24L01_06_RF_SETUP_RF_PWR_HIGH = 2,
+ NRF24L01_06_RF_SETUP_RF_PWR_LOW = 1,
+
+ NRF24L01_07_STATUS_RX_DR = 6,
+ NRF24L01_07_STATUS_TX_DS = 5,
+ NRF24L01_07_STATUS_MAX_RT = 4,
+
+ NRF24L01_17_FIFO_STATUS_TX_FULL = 5,
+ NRF24L01_17_FIFO_STATUS_TX_EMPTY = 4,
+ NRF24L01_17_FIFO_STATUS_RX_FULL = 1,
+ NRF24L01_17_FIFO_STATUS_RX_EMPTY = 0,
+
+ NRF24L01_1C_DYNPD_DPL_P5 = 5,
+ NRF24L01_1C_DYNPD_DPL_P4 = 4,
+ NRF24L01_1C_DYNPD_DPL_P3 = 3,
+ NRF24L01_1C_DYNPD_DPL_P2 = 2,
+ NRF24L01_1C_DYNPD_DPL_P1 = 1,
+ NRF24L01_1C_DYNPD_DPL_P0 = 0,
+
+ NRF24L01_1D_FEATURE_EN_DPL = 2,
+ NRF24L01_1D_FEATURE_EN_ACK_PAY = 1,
+ NRF24L01_1D_FEATURE_EN_DYN_ACK = 0,
+};
+
+// Pre-shifted and combined bits
+enum {
+ NRF24L01_01_EN_AA_ALL_PIPES = 0x3F,
+
+ NRF24L01_02_EN_RXADDR_ERX_ALL_PIPES = 0x3F,
+
+ NRF24L01_03_SETUP_AW_3BYTES = 0x01,
+ NRF24L01_03_SETUP_AW_4BYTES = 0x02,
+ NRF24L01_03_SETUP_AW_5BYTES = 0x03,
+
+ NRF24L01_04_SETUP_RETR_ARD_250us = 0x00,
+ NRF24L01_04_SETUP_RETR_ARD_500us = 0x10,
+ NRF24L01_04_SETUP_RETR_ARD_750us = 0x20,
+ NRF24L01_04_SETUP_RETR_ARD_1000us = 0x30,
+ NRF24L01_04_SETUP_RETR_ARD_1250us = 0x40,
+ NRF24L01_04_SETUP_RETR_ARD_1500us = 0x50,
+ NRF24L01_04_SETUP_RETR_ARD_1750us = 0x60,
+ NRF24L01_04_SETUP_RETR_ARD_2000us = 0x70,
+ NRF24L01_04_SETUP_RETR_ARD_2250us = 0x80,
+ NRF24L01_04_SETUP_RETR_ARD_2500us = 0x90,
+ NRF24L01_04_SETUP_RETR_ARD_2750us = 0xa0,
+ NRF24L01_04_SETUP_RETR_ARD_3000us = 0xb0,
+ NRF24L01_04_SETUP_RETR_ARD_3250us = 0xc0,
+ NRF24L01_04_SETUP_RETR_ARD_3500us = 0xd0,
+ NRF24L01_04_SETUP_RETR_ARD_3750us = 0xe0,
+ NRF24L01_04_SETUP_RETR_ARD_4000us = 0xf0,
+
+ NRF24L01_04_SETUP_RETR_ARC_0 = 0x00,
+ NRF24L01_04_SETUP_RETR_ARC_1 = 0x01,
+ NRF24L01_04_SETUP_RETR_ARC_2 = 0x02,
+ NRF24L01_04_SETUP_RETR_ARC_3 = 0x03,
+ NRF24L01_04_SETUP_RETR_ARC_4 = 0x04,
+ NRF24L01_04_SETUP_RETR_ARC_5 = 0x05,
+ NRF24L01_04_SETUP_RETR_ARC_6 = 0x06,
+ NRF24L01_04_SETUP_RETR_ARC_7 = 0x07,
+ NRF24L01_04_SETUP_RETR_ARC_8 = 0x08,
+ NRF24L01_04_SETUP_RETR_ARC_9 = 0x09,
+ NRF24L01_04_SETUP_RETR_ARC_10 = 0x0a,
+ NRF24L01_04_SETUP_RETR_ARC_11 = 0x0b,
+ NRF24L01_04_SETUP_RETR_ARC_12 = 0x0c,
+ NRF24L01_04_SETUP_RETR_ARC_13 = 0x0d,
+ NRF24L01_04_SETUP_RETR_ARC_14 = 0x0e,
+ NRF24L01_04_SETUP_RETR_ARC_15 = 0x0f,
+
+
+ NRF24L01_06_RF_SETUP_RF_DR_2Mbps = 0x08,
+ NRF24L01_06_RF_SETUP_RF_DR_1Mbps = 0x00,
+ NRF24L01_06_RF_SETUP_RF_DR_250Kbps = 0x20,
+ NRF24L01_06_RF_SETUP_RF_PWR_n18dbm = 0x01,
+ NRF24L01_06_RF_SETUP_RF_PWR_n12dbm = 0x02,
+ NRF24L01_06_RF_SETUP_RF_PWR_n6dbm = 0x04,
+ NRF24L01_06_RF_SETUP_RF_PWR_0dbm = 0x06,
+
+ NRF24L01_1C_DYNPD_ALL_PIPES = 0x3F,
+};
+
+// Pipes
+enum {
+ NRF24L01_PIPE0 = 0,
+ NRF24L01_PIPE1 = 1,
+ NRF24L01_PIPE2 = 2,
+ NRF24L01_PIPE3 = 3,
+ NRF24L01_PIPE4 = 4,
+ NRF24L01_PIPE5 = 5
+};
+
+void NRF24L01_Initialize(uint8_t baseConfig);
+uint8_t NRF24L01_WriteReg(uint8_t reg, uint8_t data);
+uint8_t NRF24L01_WriteRegisterMulti(uint8_t reg, const uint8_t *data, uint8_t length);
+uint8_t NRF24L01_WritePayload(const uint8_t *data, uint8_t length);
+uint8_t NRF24L01_WriteAckPayload(const uint8_t *data, uint8_t length, uint8_t pipe);
+uint8_t NRF24L01_ReadReg(uint8_t reg);
+uint8_t NRF24L01_ReadRegisterMulti(uint8_t reg, uint8_t *data, uint8_t length);
+uint8_t NRF24L01_ReadPayload(uint8_t *data, uint8_t length);
+
+
+// Utility functions
+
+void NRF24L01_FlushTx(void);
+void NRF24L01_FlushRx(void);
+uint8_t NRF24L01_Activate(uint8_t code);
+
+void NRF24L01_SetupBasic(void);
+void NRF24L01_SetStandbyMode(void);
+void NRF24L01_SetRxMode(void);
+void NRF24L01_SetTxMode(void);
+void NRF24L01_ClearAllInterrupts(void);
+void NRF24L01_SetChannel(uint8_t channel);
+bool NRF24L01_ReadPayloadIfAvailable(uint8_t *data, uint8_t length);
+
diff --git a/src/main/drivers/rx_spi.c b/src/main/drivers/rx_spi.c
new file mode 100644
index 0000000000..e7e8f70d4f
--- /dev/null
+++ b/src/main/drivers/rx_spi.c
@@ -0,0 +1,166 @@
+/*
+ * 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 .
+ */
+
+// This file is copied with modifications from project Deviation,
+// see http://deviationtx.com
+
+#include
+#include
+#include
+
+#include
+
+#ifdef USE_RX_SPI
+
+#include "build/build_config.h"
+
+#include "system.h"
+#include "gpio.h"
+#include "io.h"
+#include "io_impl.h"
+#include "rcc.h"
+#include "rx_spi.h"
+
+#include "bus_spi.h"
+#include "bus_spi_soft.h"
+
+#define DISABLE_RX() {IOHi(IOGetByTag(IO_TAG(RX_NSS_PIN)));}
+#define ENABLE_RX() {IOLo(IOGetByTag(IO_TAG(RX_NSS_PIN)));}
+#ifdef RX_CE_PIN
+#define RX_CE_HI() {IOHi(IOGetByTag(IO_TAG(RX_CE_PIN)));}
+#define RX_CE_LO() {IOLo(IOGetByTag(IO_TAG(RX_CE_PIN)));}
+#endif
+
+#ifdef USE_RX_SOFTSPI
+static const softSPIDevice_t softSPIDevice = {
+ .sckTag = IO_TAG(RX_SCK_PIN),
+ .mosiTag = IO_TAG(RX_MOSI_PIN),
+ .misoTag = IO_TAG(RX_MISO_PIN),
+ // Note: Nordic Semiconductor uses 'CSN', STM uses 'NSS'
+ .nssTag = IO_TAG(RX_NSS_PIN),
+};
+static bool useSoftSPI = false;
+#endif // USE_RX_SOFTSPI
+
+void rxSpiDeviceInit(rx_spi_type_e spiType)
+{
+ static bool hardwareInitialised = false;
+
+ if (hardwareInitialised) {
+ return;
+ }
+
+#ifdef USE_RX_SOFTSPI
+ if (spiType == RX_SPI_SOFTSPI) {
+ useSoftSPI = true;
+ softSpiInit(&softSPIDevice);
+ }
+ const SPIDevice rxSPIDevice = SOFT_SPIDEV_1;
+#else
+ UNUSED(spiType);
+ const SPIDevice rxSPIDevice = spiDeviceByInstance(RX_SPI_INSTANCE);
+ IOInit(IOGetByTag(IO_TAG(RX_NSS_PIN)), OWNER_SPI, RESOURCE_SPI_CS, rxSPIDevice + 1);
+#endif // USE_RX_SOFTSPI
+
+#if defined(STM32F10X)
+ RCC_AHBPeriphClockCmd(RX_NSS_GPIO_CLK_PERIPHERAL, ENABLE);
+ RCC_AHBPeriphClockCmd(RX_CE_GPIO_CLK_PERIPHERAL, ENABLE);
+#endif
+
+#ifdef RX_CE_PIN
+ // CE as OUTPUT
+ IOInit(IOGetByTag(IO_TAG(RX_CE_PIN)), OWNER_RX_SPI, RESOURCE_RX_CE, rxSPIDevice + 1);
+#if defined(STM32F10X)
+ IOConfigGPIO(IOGetByTag(IO_TAG(RX_CE_PIN)), SPI_IO_CS_CFG);
+#elif defined(STM32F3) || defined(STM32F4)
+ IOConfigGPIOAF(IOGetByTag(IO_TAG(RX_CE_PIN)), SPI_IO_CS_CFG, 0);
+#endif
+ RX_CE_LO();
+#endif // RX_CE_PIN
+ DISABLE_RX();
+
+#ifdef RX_SPI_INSTANCE
+ spiSetDivisor(RX_SPI_INSTANCE, SPI_CLOCK_STANDARD);
+#endif
+ hardwareInitialised = true;
+}
+
+uint8_t rxSpiTransferByte(uint8_t data)
+{
+#ifdef USE_RX_SOFTSPI
+ if (useSoftSPI) {
+ return softSpiTransferByte(&softSPIDevice, data);
+ } else
+#endif
+ {
+#ifdef RX_SPI_INSTANCE
+ return spiTransferByte(RX_SPI_INSTANCE, data);
+#else
+ return 0;
+#endif
+ }
+}
+
+uint8_t rxSpiWriteByte(uint8_t data)
+{
+ ENABLE_RX();
+ const uint8_t ret = rxSpiTransferByte(data);
+ DISABLE_RX();
+ return ret;
+}
+
+uint8_t rxSpiWriteCommand(uint8_t command, uint8_t data)
+{
+ ENABLE_RX();
+ const uint8_t ret = rxSpiTransferByte(command);
+ rxSpiTransferByte(data);
+ DISABLE_RX();
+ return ret;
+}
+
+uint8_t rxSpiWriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length)
+{
+ ENABLE_RX();
+ const uint8_t ret = rxSpiTransferByte(command);
+ for (uint8_t i = 0; i < length; i++) {
+ rxSpiTransferByte(data[i]);
+ }
+ DISABLE_RX();
+ return ret;
+}
+
+uint8_t rxSpiReadCommand(uint8_t command, uint8_t data)
+{
+ ENABLE_RX();
+ rxSpiTransferByte(command);
+ const uint8_t ret = rxSpiTransferByte(data);
+ DISABLE_RX();
+ return ret;
+}
+
+uint8_t rxSpiReadCommandMulti(uint8_t command, uint8_t commandData, uint8_t *retData, uint8_t length)
+{
+ ENABLE_RX();
+ const uint8_t ret = rxSpiTransferByte(command);
+ for (uint8_t i = 0; i < length; i++) {
+ retData[i] = rxSpiTransferByte(commandData);
+ }
+ DISABLE_RX();
+ return ret;
+}
+#endif
+
diff --git a/src/main/drivers/rx_spi.h b/src/main/drivers/rx_spi.h
new file mode 100644
index 0000000000..f083ca5058
--- /dev/null
+++ b/src/main/drivers/rx_spi.h
@@ -0,0 +1,35 @@
+/*
+ * 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 .
+ */
+#pragma once
+
+#include
+
+typedef enum {
+ RX_SPI_SOFTSPI,
+ RX_SPI_HARDSPI,
+} rx_spi_type_e;
+
+#define RX_SPI_MAX_PAYLOAD_SIZE 32
+
+void rxSpiDeviceInit(rx_spi_type_e spiType);
+uint8_t rxSpiTransferByte(uint8_t data);
+uint8_t rxSpiWriteByte(uint8_t data);
+uint8_t rxSpiWriteCommand(uint8_t command, uint8_t data);
+uint8_t rxSpiWriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length);
+uint8_t rxSpiReadCommand(uint8_t command, uint8_t commandData);
+uint8_t rxSpiReadCommandMulti(uint8_t command, uint8_t commandData, uint8_t *retData, uint8_t length);
+
diff --git a/src/main/drivers/rx_xn297.c b/src/main/drivers/rx_xn297.c
new file mode 100644
index 0000000000..eba751cad5
--- /dev/null
+++ b/src/main/drivers/rx_xn297.c
@@ -0,0 +1,90 @@
+/*
+ * 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 .
+ */
+
+// This file borrows heavily from project Deviation,
+// see http://deviationtx.com
+
+#include
+#include
+
+#include "rx_spi.h"
+#include "rx_nrf24l01.h"
+#include "common/maths.h"
+
+
+static const uint8_t xn297_data_scramble[30] = {
+ 0xbc, 0xe5, 0x66, 0x0d, 0xae, 0x8c, 0x88, 0x12,
+ 0x69, 0xee, 0x1f, 0xc7, 0x62, 0x97, 0xd5, 0x0b,
+ 0x79, 0xca, 0xcc, 0x1b, 0x5d, 0x19, 0x10, 0x24,
+ 0xd3, 0xdc, 0x3f, 0x8e, 0xc5, 0x2f
+};
+
+static const uint16_t xn297_crc_xorout[26] = {
+ 0x9BA7, 0x8BBB, 0x85E1, 0x3E8C, 0x451E, 0x18E6, 0x6B24, 0xE7AB,
+ 0x3828, 0x814B, 0xD461, 0xF494, 0x2503, 0x691D, 0xFE8B, 0x9BA7,
+ 0x8B17, 0x2920, 0x8B5F, 0x61B1, 0xD391, 0x7401, 0x2138, 0x129F,
+ 0xB3A0, 0x2988
+};
+
+static uint8_t bitReverse(uint8_t bIn)
+{
+ uint8_t bOut = 0;
+ for (int ii = 0; ii < 8; ++ii) {
+ bOut = (bOut << 1) | (bIn & 1);
+ bIn >>= 1;
+ }
+ return bOut;
+}
+
+
+#define RX_TX_ADDR_LEN 5
+
+uint16_t XN297_UnscramblePayload(uint8_t *data, int len, const uint8_t *rxAddr)
+{
+ uint16_t crc = 0xb5d2;
+ if (rxAddr) {
+ for (int ii = 0; ii < RX_TX_ADDR_LEN; ++ii) {
+ crc = crc16_ccitt(crc, rxAddr[RX_TX_ADDR_LEN - 1 - ii]);
+ }
+ }
+ for (int ii = 0; ii < len; ++ii) {
+ crc = crc16_ccitt(crc, data[ii]);
+ data[ii] = bitReverse(data[ii] ^ xn297_data_scramble[ii]);
+ }
+ crc ^= xn297_crc_xorout[len];
+ return crc;
+}
+
+uint8_t XN297_WritePayload(uint8_t *data, int len, const uint8_t *rxAddr)
+{
+ uint8_t packet[NRF24L01_MAX_PAYLOAD_SIZE];
+ uint16_t crc = 0xb5d2;
+ for (int ii = 0; ii < RX_TX_ADDR_LEN; ++ii) {
+ packet[ii] = rxAddr[RX_TX_ADDR_LEN - 1 - ii];
+ crc = crc16_ccitt(crc, packet[ii]);
+ }
+ for (int ii = 0; ii < len; ++ii) {
+ const uint8_t bOut = bitReverse(data[ii]);
+ packet[ii + RX_TX_ADDR_LEN] = bOut ^ xn297_data_scramble[ii];
+ crc = crc16_ccitt(crc, packet[ii + RX_TX_ADDR_LEN]);
+ }
+ crc ^= xn297_crc_xorout[len];
+ packet[RX_TX_ADDR_LEN + len] = crc >> 8;
+ packet[RX_TX_ADDR_LEN + len + 1] = crc & 0xff;
+ return NRF24L01_WritePayload(packet, RX_TX_ADDR_LEN + len + 2);
+}
+
diff --git a/src/main/drivers/rx_xn297.h b/src/main/drivers/rx_xn297.h
new file mode 100644
index 0000000000..167b2f380b
--- /dev/null
+++ b/src/main/drivers/rx_xn297.h
@@ -0,0 +1,25 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include
+#include
+
+uint16_t XN297_UnscramblePayload(uint8_t* data, int len, const uint8_t *rxAddr);
+uint8_t XN297_WritePayload(uint8_t *data, int len, const uint8_t *rxAddr);
+
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 166a02827a..fc5b812d89 100755
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -221,7 +221,8 @@ static const char * const featureNames[] = {
"SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE",
"SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD",
- "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", NULL
+ "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
+ " ", "VTX", "RX_SPI", "SOFTSPI", NULL
};
// sync this with rxFailsafeChannelMode_e
@@ -430,6 +431,20 @@ static const char * const lookupTableSerialRX[] = {
};
#endif
+#ifdef USE_RX_SPI
+// sync with rx_spi_protocol_e
+static const char * const lookupTableRxSpi[] = {
+ "V202_250K",
+ "V202_1M",
+ "SYMA_X",
+ "SYMA_X5C",
+ "CX10",
+ "CX10A",
+ "H8_3D",
+ "INAV"
+};
+#endif
+
static const char * const lookupTableGyroLpf[] = {
"OFF",
"188HZ",
@@ -547,6 +562,9 @@ typedef enum {
TABLE_PID_CONTROLLER,
#ifdef SERIAL_RX
TABLE_SERIAL_RX,
+#endif
+#ifdef USE_RX_SPI
+ TABLE_RX_SPI,
#endif
TABLE_GYRO_LPF,
TABLE_ACC_HARDWARE,
@@ -586,6 +604,9 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) },
#ifdef SERIAL_RX
{ lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) },
+#endif
+#ifdef USE_RX_SPI
+ { lookupTableRxSpi, sizeof(lookupTableRxSpi) / sizeof(char *) },
#endif
{ lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) },
{ lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) },
diff --git a/src/main/msp/msp_server_fc.c b/src/main/msp/msp_server_fc.c
index 919f30247f..4ba993ff20 100755
--- a/src/main/msp/msp_server_fc.c
+++ b/src/main/msp/msp_server_fc.c
@@ -1080,9 +1080,9 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize8(masterConfig.rxConfig.rcInterpolation);
serialize8(masterConfig.rxConfig.rcInterpolationInterval);
serialize16(masterConfig.rxConfig.airModeActivateThreshold);
- serialize8(masterConfig.rxConfig.nrf24rx_protocol);
- serialize32(masterConfig.rxConfig.nrf24rx_id);
- serialize8(masterConfig.rxConfig.nrf24rx_channel_count);
+ serialize8(masterConfig.rxConfig.rx_spi_protocol);
+ serialize32(masterConfig.rxConfig.rx_spi_id);
+ serialize8(masterConfig.rxConfig.rx_spi_rf_channel_count);
break;
case MSP_FAILSAFE_CONFIG:
@@ -1758,9 +1758,9 @@ static bool processInCommand(void)
masterConfig.rxConfig.airModeActivateThreshold = read16();
}
if (currentPort->dataSize > 16) {
- masterConfig.rxConfig.nrf24rx_protocol = read8();
- masterConfig.rxConfig.nrf24rx_id = read32();
- masterConfig.rxConfig.nrf24rx_channel_count = read8();
+ masterConfig.rxConfig.rx_spi_protocol = read8();
+ masterConfig.rxConfig.rx_spi_id = read32();
+ masterConfig.rxConfig.rx_spi_rf_channel_count = read8();
}
break;
case MSP_SET_FAILSAFE_CONFIG:
diff --git a/src/main/rx/nrf24_cx10.c b/src/main/rx/nrf24_cx10.c
new file mode 100644
index 0000000000..2434d1259c
--- /dev/null
+++ b/src/main/rx/nrf24_cx10.c
@@ -0,0 +1,301 @@
+/*
+ * 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 .
+ */
+
+// This file borrows heavily from project Deviation,
+// see http://deviationtx.com
+
+#include
+#include
+#include
+
+#include
+#include "build/build_config.h"
+
+
+#ifdef USE_RX_CX10
+
+#include "drivers/rx_nrf24l01.h"
+#include "drivers/rx_xn297.h"
+#include "drivers/system.h"
+
+#include "rx/rx_spi.h"
+#include "rx/nrf24_cx10.h"
+
+/*
+ * Deviation transmitter
+ * Bind phase lasts 6 seconds for CX10, for CX10A it lasts until an acknowledgment is received.
+ * Other transmitters may vary but should have similar characteristics.
+ * For CX10A protocol: after receiving a bind packet, the receiver must send back a data packet with byte[9] = 1 as acknowledgment
+ */
+
+/*
+ * CX10 Protocol
+ * No auto acknowledgment
+ * Payload size is 19 and static for CX10A variant, 15 and static for CX10 variant.
+ * Data rate is 1Mbps
+ * Bind Phase
+ * uses address {0xcc, 0xcc, 0xcc, 0xcc, 0xcc}, converted by XN297
+ * uses channel 0x02
+ * Data phase
+ * uses same address as bind phase
+ * hops between 4 channels that are set from the txId sent in the bind packet
+ */
+
+#define RC_CHANNEL_COUNT 9
+
+enum {
+ RATE_LOW = 0,
+ RATE_MID = 1,
+ RATE_HIGH= 2,
+};
+
+#define FLAG_FLIP 0x10 // goes to rudder channel
+// flags1
+#define FLAG_MODE_MASK 0x03
+#define FLAG_HEADLESS 0x04
+// flags2
+#define FLAG_VIDEO 0x02
+#define FLAG_PICTURE 0x04
+
+static rx_spi_protocol_e cx10Protocol;
+
+typedef enum {
+ STATE_BIND = 0,
+ STATE_ACK,
+ STATE_DATA
+} protocol_state_t;
+
+STATIC_UNIT_TESTED protocol_state_t protocolState;
+
+#define CX10_PROTOCOL_PAYLOAD_SIZE 15
+#define CX10A_PROTOCOL_PAYLOAD_SIZE 19
+static uint8_t payloadSize;
+#define ACK_TO_SEND_COUNT 8
+
+#define CRC_LEN 2
+#define RX_TX_ADDR_LEN 5
+STATIC_UNIT_TESTED uint8_t txAddr[RX_TX_ADDR_LEN] = {0x55, 0x0F, 0x71, 0x0C, 0x00}; // converted XN297 address, 0xC710F55 (28 bit)
+STATIC_UNIT_TESTED uint8_t rxAddr[RX_TX_ADDR_LEN] = {0x49, 0x26, 0x87, 0x7d, 0x2f}; // converted XN297 address
+#define TX_ID_LEN 4
+STATIC_UNIT_TESTED uint8_t txId[TX_ID_LEN];
+
+#define CX10_RF_BIND_CHANNEL 0x02
+#define RF_CHANNEL_COUNT 4
+STATIC_UNIT_TESTED uint8_t cx10RfChannelIndex = 0;
+STATIC_UNIT_TESTED uint8_t cx10RfChannels[RF_CHANNEL_COUNT]; // channels are set using txId from bind packet
+
+#define CX10_PROTOCOL_HOP_TIMEOUT 1500 // 1.5ms
+#define CX10A_PROTOCOL_HOP_TIMEOUT 6500 // 6.5ms
+static uint32_t hopTimeout;
+static uint32_t timeOfLastHop;
+
+/*
+ * Returns true if it is a bind packet.
+ */
+STATIC_UNIT_TESTED bool cx10CheckBindPacket(const uint8_t *packet)
+{
+ const bool bindPacket = (packet[0] == 0xaa); // 10101010
+ if (bindPacket) {
+ txId[0] = packet[1];
+ txId[1] = packet[2];
+ txId[2] = packet[3];
+ txId[3] = packet[4];
+ return true;
+ }
+ return false;
+}
+
+STATIC_UNIT_TESTED uint16_t cx10ConvertToPwmUnsigned(const uint8_t *pVal)
+{
+ uint16_t ret = (*(pVal + 1)) & 0x7f; // mask out top bit which is used for a flag for the rudder
+ ret = (ret << 8) | *pVal;
+ return ret;
+}
+
+void cx10Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
+{
+ const uint8_t offset = (cx10Protocol == NRF24RX_CX10) ? 0 : 4;
+ rcData[RC_SPI_ROLL] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - cx10ConvertToPwmUnsigned(&payload[5 + offset]); // aileron
+ rcData[RC_SPI_PITCH] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - cx10ConvertToPwmUnsigned(&payload[7 + offset]); // elevator
+ rcData[RC_SPI_THROTTLE] = cx10ConvertToPwmUnsigned(&payload[9 + offset]); // throttle
+ rcData[RC_SPI_YAW] = cx10ConvertToPwmUnsigned(&payload[11 + offset]); // rudder
+ const uint8_t flags1 = payload[13 + offset];
+ const uint8_t rate = flags1 & FLAG_MODE_MASK; // takes values 0, 1, 2
+ if (rate == RATE_LOW) {
+ rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIN;
+ } else if (rate == RATE_MID) {
+ rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIDDLE;
+ } else {
+ rcData[RC_CHANNEL_RATE] = PWM_RANGE_MAX;
+ }
+ // flip flag is in YAW byte
+ rcData[RC_CHANNEL_FLIP] = payload[12 + offset] & FLAG_FLIP ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ const uint8_t flags2 = payload[14 + offset];
+ rcData[RC_CHANNEL_PICTURE] = flags2 & FLAG_PICTURE ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[RC_CHANNEL_VIDEO] = flags2 & FLAG_VIDEO ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[RC_CHANNEL_HEADLESS] = flags1 & FLAG_HEADLESS ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+}
+
+static void cx10HopToNextChannel(void)
+{
+ ++cx10RfChannelIndex;
+ if (cx10RfChannelIndex >= RF_CHANNEL_COUNT) {
+ cx10RfChannelIndex = 0;
+ }
+ NRF24L01_SetChannel(cx10RfChannels[cx10RfChannelIndex]);
+}
+
+// The hopping channels are determined by the txId
+STATIC_UNIT_TESTED void cx10SetHoppingChannels(const uint8_t *txId)
+{
+ cx10RfChannelIndex = 0;
+ cx10RfChannels[0] = 0x03 + (txId[0] & 0x0F);
+ cx10RfChannels[1] = 0x16 + (txId[0] >> 4);
+ cx10RfChannels[2] = 0x2D + (txId[1] & 0x0F);
+ cx10RfChannels[3] = 0x40 + (txId[1] >> 4);
+}
+
+static bool cx10CrcOK(uint16_t crc, const uint8_t *payload)
+{
+ if (payload[payloadSize] != (crc >> 8)) {
+ return false;
+ }
+ if (payload[payloadSize + 1] != (crc & 0xff)) {
+ return false;
+ }
+ return true;
+}
+
+static bool cx10ReadPayloadIfAvailable(uint8_t *payload)
+{
+ if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize + CRC_LEN)) {
+ const uint16_t crc = XN297_UnscramblePayload(payload, payloadSize, rxAddr);
+ if (cx10CrcOK(crc, payload)) {
+ return true;
+ }
+ }
+ return false;
+}
+
+/*
+ * This is called periodically by the scheduler.
+ * Returns RX_SPI_RECEIVED_DATA if a data packet was received.
+ */
+rx_spi_received_e cx10Nrf24DataReceived(uint8_t *payload)
+{
+ static uint8_t ackCount;
+ rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
+ int totalDelayUs;
+ uint32_t timeNowUs;
+
+ switch (protocolState) {
+ case STATE_BIND:
+ if (cx10ReadPayloadIfAvailable(payload)) {
+ const bool bindPacket = cx10CheckBindPacket(payload);
+ if (bindPacket) {
+ // set the hopping channels as determined by the txId received in the bind packet
+ cx10SetHoppingChannels(txId);
+ ret = RX_SPI_RECEIVED_BIND;
+ protocolState = STATE_ACK;
+ ackCount = 0;
+ }
+ }
+ break;
+ case STATE_ACK:
+ // transmit an ACK packet
+ ++ackCount;
+ totalDelayUs = 0;
+ // send out an ACK on the bind channel, required by deviationTx
+ payload[9] = 0x01;
+ NRF24L01_SetChannel(CX10_RF_BIND_CHANNEL);
+ NRF24L01_FlushTx();
+ XN297_WritePayload(payload, payloadSize, rxAddr);
+ NRF24L01_SetTxMode();// enter transmit mode to send the packet
+ // wait for the ACK packet to send before changing channel
+ static const int fifoDelayUs = 100;
+ while (!(NRF24L01_ReadReg(NRF24L01_17_FIFO_STATUS) & BV(NRF24L01_17_FIFO_STATUS_TX_EMPTY))) {
+ delayMicroseconds(fifoDelayUs);
+ totalDelayUs += fifoDelayUs;
+ }
+ // send out an ACK on each of the hopping channels, required by CX10 transmitter
+ for (int ii = 0; ii < RF_CHANNEL_COUNT; ++ii) {
+ NRF24L01_SetChannel(cx10RfChannels[ii]);
+ XN297_WritePayload(payload, payloadSize, rxAddr);
+ NRF24L01_SetTxMode();// enter transmit mode to send the packet
+ // wait for the ACK packet to send before changing channel
+ while (!(NRF24L01_ReadReg(NRF24L01_17_FIFO_STATUS) & BV(NRF24L01_17_FIFO_STATUS_TX_EMPTY))) {
+ delayMicroseconds(fifoDelayUs);
+ totalDelayUs += fifoDelayUs;
+ }
+ }
+ static const int delayBetweenPacketsUs = 1000;
+ if (totalDelayUs < delayBetweenPacketsUs) {
+ delayMicroseconds(delayBetweenPacketsUs - totalDelayUs);
+ }
+ NRF24L01_SetRxMode();//reenter receive mode after sending ACKs
+ if (ackCount > ACK_TO_SEND_COUNT) {
+ NRF24L01_SetChannel(cx10RfChannels[0]);
+ // and go into data state to wait for first data packet
+ protocolState = STATE_DATA;
+ }
+ break;
+ case STATE_DATA:
+ timeNowUs = micros();
+ // read the payload, processing of payload is deferred
+ if (cx10ReadPayloadIfAvailable(payload)) {
+ cx10HopToNextChannel();
+ timeOfLastHop = timeNowUs;
+ ret = RX_SPI_RECEIVED_DATA;
+ }
+ if (timeNowUs > timeOfLastHop + hopTimeout) {
+ cx10HopToNextChannel();
+ timeOfLastHop = timeNowUs;
+ }
+ }
+ return ret;
+}
+
+static void cx10Nrf24Setup(rx_spi_protocol_e protocol)
+{
+ cx10Protocol = protocol;
+ protocolState = STATE_BIND;
+ payloadSize = (protocol == NRF24RX_CX10) ? CX10_PROTOCOL_PAYLOAD_SIZE : CX10A_PROTOCOL_PAYLOAD_SIZE;
+ hopTimeout = (protocol == NRF24RX_CX10) ? CX10_PROTOCOL_HOP_TIMEOUT : CX10A_PROTOCOL_HOP_TIMEOUT;
+
+ NRF24L01_Initialize(0); // sets PWR_UP, no CRC
+ NRF24L01_SetupBasic();
+
+ NRF24L01_SetChannel(CX10_RF_BIND_CHANNEL);
+
+ NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
+ // RX_ADDR for pipes P2 to P5 are left at default values
+ NRF24L01_FlushRx();
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, txAddr, RX_TX_ADDR_LEN);
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxAddr, RX_TX_ADDR_LEN);
+
+ NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, payloadSize + CRC_LEN); // payload + 2 bytes CRC
+
+ NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
+}
+
+void cx10Nrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT;
+ cx10Nrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol);
+}
+#endif
+
diff --git a/src/main/rx/nrf24_cx10.h b/src/main/rx/nrf24_cx10.h
new file mode 100644
index 0000000000..8af1547333
--- /dev/null
+++ b/src/main/rx/nrf24_cx10.h
@@ -0,0 +1,28 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include
+#include
+
+struct rxConfig_s;
+struct rxRuntimeConfig_s;
+void cx10Nrf24Init(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
+void cx10Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
+rx_spi_received_e cx10Nrf24DataReceived(uint8_t *payload);
+
diff --git a/src/main/rx/nrf24_h8_3d.c b/src/main/rx/nrf24_h8_3d.c
new file mode 100644
index 0000000000..9ffa1dcf47
--- /dev/null
+++ b/src/main/rx/nrf24_h8_3d.c
@@ -0,0 +1,283 @@
+/*
+ * 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 .
+ */
+
+// This file borrows heavily from project Deviation,
+// see http://deviationtx.com
+
+#include
+#include
+#include
+
+#include
+
+#ifdef USE_RX_H8_3D
+
+#include "build/build_config.h"
+
+
+#include "drivers/rx_nrf24l01.h"
+#include "drivers/rx_xn297.h"
+#include "drivers/system.h"
+
+#include "rx/rx_spi.h"
+#include "rx/nrf24_h8_3d.h"
+
+
+/*
+ * Deviation transmitter sends 345 bind packets, then starts sending data packets.
+ * Packets are send at rate of at least one every 4 milliseconds, ie at least 250Hz.
+ * This means binding phase lasts 1.4 seconds, the transmitter then enters the data phase.
+ * Other transmitters may vary but should have similar characteristics.
+ */
+
+
+/*
+ * H8_3D Protocol
+ * No auto acknowledgment
+ * Payload size is 20, static
+ * Data rate is 1Mbps
+ * Bind Phase
+ * uses address {0xab,0xac,0xad,0xae,0xaf}, converted by XN297 to {0x41, 0xbd, 0x42, 0xd4, 0xc2}
+ * hops between 4 channels
+ * Data Phase
+ * uses same address as bind phase
+ * hops between 4 channels generated from txId received in bind packets
+ *
+ */
+#define RC_CHANNEL_COUNT 14
+
+#define FLAG_FLIP 0x01
+#define FLAG_RATE_MID 0x02
+#define FLAG_RATE_HIGH 0x04
+#define FLAG_HEADLESS 0x10 // RTH + headless on H8, headless on JJRC H20
+#define FLAG_RTH 0x20 // 360° flip mode on H8 3D, RTH on JJRC H20
+#define FLAG_PICTURE 0x40 // on payload[18]
+#define FLAG_VIDEO 0x80 // on payload[18]
+#define FLAG_CAMERA_UP 0x04 // on payload[18]
+#define FLAG_CAMERA_DOWN 0x08 // on payload[18]
+
+typedef enum {
+ STATE_BIND = 0,
+ STATE_DATA
+} protocol_state_t;
+
+STATIC_UNIT_TESTED protocol_state_t protocolState;
+
+#define H8_3D_PROTOCOL_PAYLOAD_SIZE 20
+STATIC_UNIT_TESTED uint8_t payloadSize;
+
+#define CRC_LEN 2
+#define RX_TX_ADDR_LEN 5
+STATIC_UNIT_TESTED uint8_t rxTxAddrXN297[RX_TX_ADDR_LEN] = {0x41, 0xbd, 0x42, 0xd4, 0xc2}; // converted XN297 address
+#define TX_ID_LEN 4
+STATIC_UNIT_TESTED uint8_t txId[TX_ID_LEN];
+uint32_t *rxSpiIdPtr;
+
+// radio channels for frequency hopping
+#define H8_3D_RF_CHANNEL_COUNT 4
+STATIC_UNIT_TESTED uint8_t h8_3dRfChannelCount = H8_3D_RF_CHANNEL_COUNT;
+STATIC_UNIT_TESTED uint8_t h8_3dRfChannelIndex;
+STATIC_UNIT_TESTED uint8_t h8_3dRfChannels[H8_3D_RF_CHANNEL_COUNT];
+// hop between these channels in the bind phase
+#define H8_3D_RF_BIND_CHANNEL_START 0x06
+#define H8_3D_RF_BIND_CHANNEL_END 0x26
+
+#define DATA_HOP_TIMEOUT 5000 // 5ms
+#define BIND_HOP_TIMEOUT 1000 // 1ms, to find the bind channel as quickly as possible
+static uint32_t hopTimeout = BIND_HOP_TIMEOUT;
+static uint32_t timeOfLastHop;
+
+STATIC_UNIT_TESTED bool h8_3dCheckBindPacket(const uint8_t *payload)
+{
+ bool bindPacket = false;
+ if ((payload[5] == 0x00) && (payload[6] == 0x00) && (payload[7] == 0x01)) {
+ const uint32_t checkSumTxId = (payload[1] + payload[2] + payload[3] + payload[4]) & 0xff;
+ if (checkSumTxId == payload[8]) {
+ bindPacket = true;
+ txId[0] = payload[1];
+ txId[1] = payload[2];
+ txId[2] = payload[3];
+ txId[3] = payload[4];
+ if (rxSpiIdPtr != NULL && *rxSpiIdPtr == 0) {
+ // copy the txId so it can be saved
+ memcpy(rxSpiIdPtr, txId, sizeof(uint32_t));
+ }
+ }
+ }
+ return bindPacket;
+}
+
+STATIC_UNIT_TESTED uint16_t h8_3dConvertToPwm(uint8_t val, int16_t _min, int16_t _max)
+{
+#define PWM_RANGE (PWM_RANGE_MAX - PWM_RANGE_MIN)
+
+ int32_t ret = val;
+ const int32_t range = _max - _min;
+ ret = PWM_RANGE_MIN + ((ret - _min) * PWM_RANGE)/range;
+ return (uint16_t)ret;
+}
+
+void h8_3dNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
+{
+ rcData[RC_SPI_ROLL] = h8_3dConvertToPwm(payload[12], 0xbb, 0x43); // aileron
+ rcData[RC_SPI_PITCH] = h8_3dConvertToPwm(payload[11], 0x43, 0xbb); // elevator
+ rcData[RC_SPI_THROTTLE] = h8_3dConvertToPwm(payload[9], 0, 0xff); // throttle
+ const int8_t yawByte = payload[10]; // rudder
+ rcData[RC_SPI_YAW] = yawByte >= 0 ? h8_3dConvertToPwm(yawByte, -0x3c, 0x3c) : h8_3dConvertToPwm(yawByte, 0xbc, 0x44);
+
+ const uint8_t flags = payload[17];
+ const uint8_t flags2 = payload[18];
+ if (flags & FLAG_RATE_HIGH) {
+ rcData[RC_CHANNEL_RATE] = PWM_RANGE_MAX;
+ } else if (flags & FLAG_RATE_MID) {
+ rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIDDLE;
+ } else {
+ rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIN;
+ }
+
+ rcData[RC_CHANNEL_FLIP] = flags & FLAG_FLIP ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[RC_CHANNEL_PICTURE] = flags2 & FLAG_PICTURE ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[RC_CHANNEL_VIDEO] = flags2 & FLAG_VIDEO ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[RC_CHANNEL_HEADLESS] = flags & FLAG_HEADLESS ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[RC_CHANNEL_RTH] = flags & FLAG_RTH ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+
+ if (flags2 & FLAG_CAMERA_UP) {
+ rcData[RC_SPI_AUX7] = PWM_RANGE_MAX;
+ } else if (flags2 & FLAG_CAMERA_DOWN) {
+ rcData[RC_SPI_AUX7] = PWM_RANGE_MIN;
+ } else {
+ rcData[RC_SPI_AUX7] = PWM_RANGE_MIDDLE;
+ }
+ rcData[RC_SPI_AUX8] = h8_3dConvertToPwm(payload[14], 0x10, 0x30);
+ rcData[RC_SPI_AUX9] = h8_3dConvertToPwm(payload[15], 0x30, 0x10);
+ rcData[RC_SPI_AUX10] = h8_3dConvertToPwm(payload[16], 0x10, 0x30);
+}
+
+static void h8_3dHopToNextChannel(void)
+{
+ ++h8_3dRfChannelIndex;
+ if (protocolState == STATE_BIND) {
+ if (h8_3dRfChannelIndex > H8_3D_RF_BIND_CHANNEL_END) {
+ h8_3dRfChannelIndex = H8_3D_RF_BIND_CHANNEL_START;
+ }
+ NRF24L01_SetChannel(h8_3dRfChannelIndex);
+ } else {
+ if (h8_3dRfChannelIndex >= h8_3dRfChannelCount) {
+ h8_3dRfChannelIndex = 0;
+ }
+ NRF24L01_SetChannel(h8_3dRfChannels[h8_3dRfChannelIndex]);
+ }
+}
+
+// The hopping channels are determined by the txId
+static void h8_3dSetHoppingChannels(const uint8_t *txId)
+{
+ for (int ii = 0; ii < H8_3D_RF_CHANNEL_COUNT; ++ii) {
+ h8_3dRfChannels[ii] = 0x06 + (0x0f * ii) + ((txId[ii] >> 4) + (txId[ii] & 0x0f)) % 0x0f;
+ }
+}
+
+static void h8_3dSetBound(const uint8_t *txId)
+{
+ protocolState = STATE_DATA;
+ h8_3dSetHoppingChannels(txId);
+ hopTimeout = DATA_HOP_TIMEOUT;
+ timeOfLastHop = micros();
+ h8_3dRfChannelIndex = 0;
+ NRF24L01_SetChannel(h8_3dRfChannels[0]);
+}
+
+static bool h8_3dCrcOK(uint16_t crc, const uint8_t *payload)
+{
+ if (payload[payloadSize] != (crc >> 8)) {
+ return false;
+ }
+ if (payload[payloadSize + 1] != (crc & 0xff)) {
+ return false;
+ }
+ return true;
+}
+
+/*
+ * This is called periodically by the scheduler.
+ * Returns NRF24L01_RECEIVED_DATA if a data packet was received.
+ */
+rx_spi_received_e h8_3dNrf24DataReceived(uint8_t *payload)
+{
+ rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
+ bool payloadReceived = false;
+ if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize + CRC_LEN)) {
+ const uint16_t crc = XN297_UnscramblePayload(payload, payloadSize, rxTxAddrXN297);
+ if (h8_3dCrcOK(crc, payload)) {
+ payloadReceived = true;
+ }
+ }
+ switch (protocolState) {
+ case STATE_BIND:
+ if (payloadReceived) {
+ const bool bindPacket = h8_3dCheckBindPacket(payload);
+ if (bindPacket) {
+ ret = RX_SPI_RECEIVED_BIND;
+ h8_3dSetBound(txId);
+ }
+ }
+ break;
+ case STATE_DATA:
+ if (payloadReceived) {
+ ret = RX_SPI_RECEIVED_DATA;
+ }
+ break;
+ }
+ const uint32_t timeNowUs = micros();
+ if ((ret == RX_SPI_RECEIVED_DATA) || (timeNowUs > timeOfLastHop + hopTimeout)) {
+ h8_3dHopToNextChannel();
+ timeOfLastHop = timeNowUs;
+ }
+ return ret;
+}
+
+static void h8_3dNrf24Setup(rx_spi_protocol_e protocol, const uint32_t *rxSpiId)
+{
+ UNUSED(protocol);
+ protocolState = STATE_BIND;
+
+ NRF24L01_Initialize(0); // sets PWR_UP, no CRC - hardware CRC not used for XN297
+ NRF24L01_SetupBasic();
+
+ NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
+ // RX_ADDR for pipes P1-P5 are left at default values
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddrXN297, RX_TX_ADDR_LEN);
+ rxSpiIdPtr = (uint32_t*)rxSpiId;
+ if (rxSpiId == NULL || *rxSpiId == 0) {
+ h8_3dRfChannelIndex = H8_3D_RF_BIND_CHANNEL_START;
+ NRF24L01_SetChannel(H8_3D_RF_BIND_CHANNEL_START);
+ } else {
+ h8_3dSetBound((uint8_t*)rxSpiId);
+ }
+
+ payloadSize = H8_3D_PROTOCOL_PAYLOAD_SIZE;
+ NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, payloadSize + CRC_LEN); // payload + 2 bytes CRC
+
+ NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
+}
+
+void h8_3dNrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT;
+ h8_3dNrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol, &rxConfig->rx_spi_id);
+}
+#endif
diff --git a/src/main/rx/nrf24_h8_3d.h b/src/main/rx/nrf24_h8_3d.h
new file mode 100644
index 0000000000..306259f9aa
--- /dev/null
+++ b/src/main/rx/nrf24_h8_3d.h
@@ -0,0 +1,27 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include
+#include
+
+struct rxConfig_s;
+struct rxRuntimeConfig_s;
+void h8_3dNrf24Init(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
+void h8_3dNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
+rx_spi_received_e h8_3dNrf24DataReceived(uint8_t *payload);
diff --git a/src/main/rx/nrf24_inav.c b/src/main/rx/nrf24_inav.c
new file mode 100644
index 0000000000..35b526f6ac
--- /dev/null
+++ b/src/main/rx/nrf24_inav.c
@@ -0,0 +1,424 @@
+/*
+ * 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
+
+#include "platform.h"
+
+#ifdef USE_RX_INAV
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "drivers/rx_nrf24l01.h"
+#include "drivers/system.h"
+
+#include "rx/rx_spi.h"
+#include "rx/nrf24_inav.h"
+
+#include "telemetry/ltm.h"
+
+// debug build flags
+//#define DEBUG_NRF24_INAV
+//#define NO_RF_CHANNEL_HOPPING
+//#define USE_BIND_ADDRESS_FOR_DATA_STATE
+
+
+#define USE_AUTO_ACKKNOWLEDGEMENT
+
+/*
+ * iNav Protocol
+ * Data rate is 250Kbps - lower data rate for better reliability and range
+ *
+ * Uses auto acknowledgment and dynamic payload size
+ * ACK payload is used for handshaking in bind phase and telemetry in data phase
+ *
+ * Bind payload size is 16 bytes
+ * Data payload size is 8, 16 or 18 bytes dependent on variant of protocol, (small payload is read more quickly (marginal benefit))
+ *
+ * Bind Phase
+ * uses address {0x4b,0x5c,0x6d,0x7e,0x8f}
+ * uses channel 0x4c (76)
+ *
+ * Data Phase
+ * 1) Uses the address received in bind packet
+ *
+ * 2) Hops between RF channels generated from the address received in bind packet.
+ * The number of RF hopping channels is set during bind handshaking:
+ * the transmitter requests a number of bind channels in payload[7]
+ * the receiver sets ackPayload[7] with the number of hopping channels actually allocated - the transmitter must
+ * use this value.
+ * All receiver variants must support the 16 byte payload. Support for the 8 and 18 byte payload is optional.
+ *
+ * 3) Uses the payload size negotiated in the bind phase, payload size may be 8, 16 or 18 bytes
+ * a) For 8 byte payload there are 6 channels: AETR with resolution of 1 (10-bits are used for the channel data), and AUX1
+ * and AUX2 with resolution of 4 (8-bits are used for the channel data)
+ * b) For 16 byte payload there are 16 channels: eight 10-bit analog channels, two 8-bit analog channels, and six digital channels as follows:
+ * Channels 0 to 3, are the AETR channels, values 1000 to 2000 with resolution of 1 (10-bit channels)
+ * Channel AUX1 by deviation convention is used for rate, values 1000, 1500, 2000
+ * Channels AUX2 to AUX6 are binary channels, values 1000 or 2000,
+ * by deviation convention these channels are used for: flip, picture, video, headless, and return to home
+ * Channels AUX7 to AUX10 are analog channels, values 1000 to 2000 with resolution of 1 (10-bit channels)
+ * Channels AUX11 and AUX12 are analog channels, values 1000 to 2000 with resolution of 4 (8-bit channels)
+ * c) For 18 byte payload there are 18 channels, the first 16 channelsar are as for 16 byte payload, and then there are two
+ * additional channels: AUX13 and AUX14 both with resolution of 4 (8-bit channels)
+ */
+
+#define RC_CHANNEL_COUNT 16 // standard variant of the protocol has 16 RC channels
+#define RC_CHANNEL_COUNT_MAX MAX_SUPPORTED_RC_CHANNEL_COUNT // up to 18 RC channels are supported
+
+enum {
+ RATE_LOW = 0,
+ RATE_MID = 1,
+ RATE_HIGH = 2,
+};
+
+enum {
+ FLAG_FLIP = 0x01,
+ FLAG_PICTURE = 0x02,
+ FLAG_VIDEO = 0x04,
+ FLAG_RTH = 0x08,
+ FLAG_HEADLESS = 0x10,
+};
+
+typedef enum {
+ STATE_BIND = 0,
+ STATE_DATA
+} protocol_state_t;
+
+STATIC_UNIT_TESTED protocol_state_t protocolState;
+
+STATIC_UNIT_TESTED uint8_t ackPayload[NRF24L01_MAX_PAYLOAD_SIZE];
+#define BIND_PAYLOAD0 0xae // 10101110
+#define BIND_PAYLOAD1 0xc9 // 11001001
+#define BIND_ACK_PAYLOAD0 0x83 // 10000111
+#define BIND_ACK_PAYLOAD1 0xa5 // 10100101
+
+#define INAV_PROTOCOL_PAYLOAD_SIZE_MIN 8
+#define INAV_PROTOCOL_PAYLOAD_SIZE_DEFAULT 16
+#define INAV_PROTOCOL_PAYLOAD_SIZE_MAX 18
+STATIC_UNIT_TESTED const uint8_t payloadSize = INAV_PROTOCOL_PAYLOAD_SIZE_DEFAULT;
+uint8_t receivedPowerSnapshot;
+
+#define RX_TX_ADDR_LEN 5
+// set rxTxAddr to the bind address
+STATIC_UNIT_TESTED uint8_t rxTxAddr[RX_TX_ADDR_LEN] = {0x4b,0x5c,0x6d,0x7e,0x8f};
+uint32_t *rxSpiIdPtr;
+#define RX_TX_ADDR_4 0xD2 // rxTxAddr[4] always set to this value
+
+// radio channels for frequency hopping
+#define INAV_RF_CHANNEL_COUNT_MAX 8
+#define INAV_RF_CHANNEL_HOPPING_COUNT_DEFAULT 4
+STATIC_UNIT_TESTED const uint8_t inavRfChannelHoppingCount = INAV_RF_CHANNEL_HOPPING_COUNT_DEFAULT;
+STATIC_UNIT_TESTED uint8_t inavRfChannelCount;
+STATIC_UNIT_TESTED uint8_t inavRfChannelIndex;
+STATIC_UNIT_TESTED uint8_t inavRfChannels[INAV_RF_CHANNEL_COUNT_MAX];
+#define INAV_RF_BIND_CHANNEL 0x4c
+
+static uint32_t timeOfLastHop;
+static const uint32_t hopTimeout = 5000; // 5ms
+
+STATIC_UNIT_TESTED bool inavCheckBindPacket(const uint8_t *payload)
+{
+ bool bindPacket = false;
+ if (payload[0] == BIND_PAYLOAD0 && payload[1] == BIND_PAYLOAD1) {
+ bindPacket = true;
+ if (protocolState ==STATE_BIND) {
+ rxTxAddr[0] = payload[2];
+ rxTxAddr[1] = payload[3];
+ rxTxAddr[2] = payload[4];
+ rxTxAddr[3] = payload[5];
+ rxTxAddr[4] = payload[6];
+ /*inavRfChannelHoppingCount = payload[7]; // !!TODO not yet implemented on transmitter
+ if (inavRfChannelHoppingCount > INAV_RF_CHANNEL_COUNT_MAX) {
+ inavRfChannelHoppingCount = INAV_RF_CHANNEL_COUNT_MAX;
+ }*/
+ if (rxSpiIdPtr != NULL && *rxSpiIdPtr == 0) {
+ // copy the rxTxAddr so it can be saved
+ memcpy(rxSpiIdPtr, rxTxAddr, sizeof(uint32_t));
+ }
+ }
+ }
+ return bindPacket;
+}
+
+void inavNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
+{
+ memset(rcData, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT * sizeof(uint16_t));
+ // payload[0] and payload[1] are zero in DATA state
+ // the AETR channels have 10 bit resolution
+ uint8_t lowBits = payload[6]; // least significant bits for AETR
+ rcData[RC_SPI_ROLL] = PWM_RANGE_MIN + ((payload[2] << 2) | (lowBits & 0x03)); // Aileron
+ lowBits >>= 2;
+ rcData[RC_SPI_PITCH] = PWM_RANGE_MIN + ((payload[3] << 2) | (lowBits & 0x03)); // Elevator
+ lowBits >>= 2;
+ rcData[RC_SPI_THROTTLE] = PWM_RANGE_MIN + ((payload[4] << 2) | (lowBits & 0x03)); // Throttle
+ lowBits >>= 2;
+ rcData[RC_SPI_YAW] = PWM_RANGE_MIN + ((payload[5] << 2) | (lowBits & 0x03)); // Rudder
+
+ if (payloadSize == INAV_PROTOCOL_PAYLOAD_SIZE_MIN) {
+ // small payload variant of protocol, supports 6 channels
+ rcData[RC_SPI_AUX1] = PWM_RANGE_MIN + (payload[7] << 2);
+ rcData[RC_SPI_AUX2] = PWM_RANGE_MIN + (payload[1] << 2);
+ } else {
+ // channel AUX1 is used for rate, as per the deviation convention
+ const uint8_t rate = payload[7];
+ // AUX1
+ if (rate == RATE_HIGH) {
+ rcData[RC_CHANNEL_RATE] = PWM_RANGE_MAX;
+ } else if (rate == RATE_MID) {
+ rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIDDLE;
+ } else {
+ rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIN;
+ }
+
+ // channels AUX2 to AUX7 use the deviation convention
+ const uint8_t flags = payload[8];
+ rcData[RC_CHANNEL_FLIP]= (flags & FLAG_FLIP) ? PWM_RANGE_MAX : PWM_RANGE_MIN; // AUX2
+ rcData[RC_CHANNEL_PICTURE]= (flags & FLAG_PICTURE) ? PWM_RANGE_MAX : PWM_RANGE_MIN; // AUX3
+ rcData[RC_CHANNEL_VIDEO]= (flags & FLAG_VIDEO) ? PWM_RANGE_MAX : PWM_RANGE_MIN; // AUX4
+ rcData[RC_CHANNEL_HEADLESS]= (flags & FLAG_HEADLESS) ? PWM_RANGE_MAX : PWM_RANGE_MIN; //AUX5
+ rcData[RC_CHANNEL_RTH]= (flags & FLAG_RTH) ? PWM_RANGE_MAX : PWM_RANGE_MIN; // AUX6
+
+ // channels AUX7 to AUX10 have 10 bit resolution
+ lowBits = payload[13]; // least significant bits for AUX7 to AUX10
+ rcData[RC_SPI_AUX7] = PWM_RANGE_MIN + ((payload[9] << 2) | (lowBits & 0x03));
+ lowBits >>= 2;
+ rcData[RC_SPI_AUX8] = PWM_RANGE_MIN + ((payload[10] << 2) | (lowBits & 0x03));
+ lowBits >>= 2;
+ rcData[RC_SPI_AUX9] = PWM_RANGE_MIN + ((payload[11] << 2) | (lowBits & 0x03));
+ lowBits >>= 2;
+ rcData[RC_SPI_AUX10] = PWM_RANGE_MIN + ((payload[12] << 2) | (lowBits & 0x03));
+ lowBits >>= 2;
+
+ // channels AUX11 and AUX12 have 8 bit resolution
+ rcData[RC_SPI_AUX11] = PWM_RANGE_MIN + (payload[14] << 2);
+ rcData[RC_SPI_AUX12] = PWM_RANGE_MIN + (payload[15] << 2);
+ }
+ if (payloadSize == INAV_PROTOCOL_PAYLOAD_SIZE_MAX) {
+ // large payload variant of protocol
+ // channels AUX13 to AUX16 have 8 bit resolution
+ rcData[RC_SPI_AUX13] = PWM_RANGE_MIN + (payload[16] << 2);
+ rcData[RC_SPI_AUX14] = PWM_RANGE_MIN + (payload[17] << 2);
+ }
+}
+
+static void inavHopToNextChannel(void)
+{
+ ++inavRfChannelIndex;
+ if (inavRfChannelIndex >= inavRfChannelCount) {
+ inavRfChannelIndex = 0;
+ }
+ NRF24L01_SetChannel(inavRfChannels[inavRfChannelIndex]);
+#ifdef DEBUG_NRF24_INAV
+ debug[0] = inavRfChannels[inavRfChannelIndex];
+#endif
+}
+
+// The hopping channels are determined by the low bits of rxTxAddr
+STATIC_UNIT_TESTED void inavSetHoppingChannels(void)
+{
+#ifdef NO_RF_CHANNEL_HOPPING
+ // just stay on bind channel, useful for debugging
+ inavRfChannelCount = 1;
+ inavRfChannels[0] = INAV_RF_BIND_CHANNEL;
+#else
+ inavRfChannelCount = inavRfChannelHoppingCount;
+ const uint8_t addr = rxTxAddr[0];
+ uint8_t ch = 0x10 + (addr & 0x07);
+ for (int ii = 0; ii < INAV_RF_CHANNEL_COUNT_MAX; ++ii) {
+ inavRfChannels[ii] = ch;
+ ch += 0x0c;
+ }
+#endif
+}
+
+static void inavSetBound(void)
+{
+ protocolState = STATE_DATA;
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddr, RX_TX_ADDR_LEN);
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rxTxAddr, RX_TX_ADDR_LEN);
+
+ timeOfLastHop = micros();
+ inavRfChannelIndex = 0;
+ inavSetHoppingChannels();
+ NRF24L01_SetChannel(inavRfChannels[0]);
+#ifdef DEBUG_NRF24_INAV
+ debug[0] = inavRfChannels[inavRfChannelIndex];
+#endif
+}
+
+static void writeAckPayload(const uint8_t *data, uint8_t length)
+{
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, BV(NRF24L01_07_STATUS_MAX_RT));
+ NRF24L01_WriteAckPayload(data, length, NRF24L01_PIPE0);
+}
+
+static void writeTelemetryAckPayload(void)
+{
+#ifdef TELEMETRY_NRF24_LTM
+ // set up telemetry data, send back telemetry data in the ACK packet
+ static uint8_t sequenceNumber = 0;
+ static ltm_frame_e ltmFrameType = LTM_FRAME_START;
+
+ ackPayload[0] = sequenceNumber++;
+ const int ackPayloadSize = getLtmFrame(&ackPayload[1], ltmFrameType) + 1;
+
+ ++ltmFrameType;
+ if (ltmFrameType > LTM_FRAME_COUNT) {
+ ltmFrameType = LTM_FRAME_START;
+ }
+ writeAckPayload(ackPayload, ackPayloadSize);
+#ifdef DEBUG_NRF24_INAV
+ debug[1] = ackPayload[0]; // sequenceNumber
+ debug[2] = ackPayload[1]; // frame type, 'A', 'S' etc
+ debug[3] = ackPayload[2]; // pitch for AFrame
+#endif
+#endif
+}
+
+static void writeBindAckPayload(uint8_t *payload)
+{
+#ifdef USE_AUTO_ACKKNOWLEDGEMENT
+ // send back the payload with the first two bytes set to zero as the ack
+ payload[0] = BIND_ACK_PAYLOAD0;
+ payload[1] = BIND_ACK_PAYLOAD1;
+ // respond to request for rfChannelCount;
+ payload[7] = inavRfChannelHoppingCount;
+ // respond to request for payloadSize
+ switch (payloadSize) {
+ case INAV_PROTOCOL_PAYLOAD_SIZE_MIN:
+ case INAV_PROTOCOL_PAYLOAD_SIZE_DEFAULT:
+ case INAV_PROTOCOL_PAYLOAD_SIZE_MAX:
+ payload[8] = payloadSize;
+ break;
+ default:
+ payload[8] = INAV_PROTOCOL_PAYLOAD_SIZE_DEFAULT;
+ break;
+ }
+ writeAckPayload(payload, payloadSize);
+#else
+ UNUSED(payload);
+#endif
+}
+
+/*
+ * This is called periodically by the scheduler.
+ * Returns RX_SPI_RECEIVED_DATA if a data packet was received.
+ */
+rx_spi_received_e inavNrf24DataReceived(uint8_t *payload)
+{
+ rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
+ uint32_t timeNowUs;
+ switch (protocolState) {
+ case STATE_BIND:
+ if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) {
+ const bool bindPacket = inavCheckBindPacket(payload);
+ if (bindPacket) {
+ ret = RX_SPI_RECEIVED_BIND;
+ writeBindAckPayload(payload);
+ // got a bind packet, so set the hopping channels and the rxTxAddr and start listening for data
+ inavSetBound();
+ }
+ }
+ break;
+ case STATE_DATA:
+ timeNowUs = micros();
+ // read the payload, processing of payload is deferred
+ if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) {
+ receivedPowerSnapshot = NRF24L01_ReadReg(NRF24L01_09_RPD); // set to 1 if received power > -64dBm
+ const bool bindPacket = inavCheckBindPacket(payload);
+ if (bindPacket) {
+ // transmitter may still continue to transmit bind packets after we have switched to data mode
+ ret = RX_SPI_RECEIVED_BIND;
+ writeBindAckPayload(payload);
+ } else {
+ ret = RX_SPI_RECEIVED_DATA;
+ writeTelemetryAckPayload();
+ }
+ }
+ if ((ret == RX_SPI_RECEIVED_DATA) || (timeNowUs > timeOfLastHop + hopTimeout)) {
+ inavHopToNextChannel();
+ timeOfLastHop = timeNowUs;
+ }
+ break;
+ }
+ return ret;
+}
+
+static void inavNrf24Setup(rx_spi_protocol_e protocol, const uint32_t *rxSpiId, int rfChannelHoppingCount)
+{
+ UNUSED(protocol);
+ UNUSED(rfChannelHoppingCount);
+
+ // sets PWR_UP, EN_CRC, CRCO - 2 byte CRC, only get IRQ pin interrupt on RX_DR
+ NRF24L01_Initialize(BV(NRF24L01_00_CONFIG_EN_CRC) | BV(NRF24L01_00_CONFIG_CRCO) | BV(NRF24L01_00_CONFIG_MASK_MAX_RT) | BV(NRF24L01_00_CONFIG_MASK_TX_DS));
+
+#ifdef USE_AUTO_ACKKNOWLEDGEMENT
+ NRF24L01_WriteReg(NRF24L01_01_EN_AA, BV(NRF24L01_01_EN_AA_ENAA_P0)); // auto acknowledgment on P0
+ NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, BV(NRF24L01_02_EN_RXADDR_ERX_P0));
+ NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, NRF24L01_03_SETUP_AW_5BYTES); // 5-byte RX/TX address
+ NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0);
+ NRF24L01_Activate(0x73); // activate R_RX_PL_WID, W_ACK_PAYLOAD, and W_TX_PAYLOAD_NOACK registers
+ NRF24L01_WriteReg(NRF24L01_1D_FEATURE, BV(NRF24L01_1D_FEATURE_EN_ACK_PAY) | BV(NRF24L01_1D_FEATURE_EN_DPL));
+ NRF24L01_WriteReg(NRF24L01_1C_DYNPD, BV(NRF24L01_1C_DYNPD_DPL_P0)); // enable dynamic payload length on P0
+ //NRF24L01_Activate(0x73); // deactivate R_RX_PL_WID, W_ACK_PAYLOAD, and W_TX_PAYLOAD_NOACK registers
+
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rxTxAddr, RX_TX_ADDR_LEN);
+#else
+ NRF24L01_SetupBasic();
+#endif
+
+ NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_250Kbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
+ // RX_ADDR for pipes P1-P5 are left at default values
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddr, RX_TX_ADDR_LEN);
+ NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, payloadSize);
+
+#ifdef USE_BIND_ADDRESS_FOR_DATA_STATE
+ inavSetBound();
+ UNUSED(rxSpiId);
+#else
+ rxSpiId = NULL; // !!TODO remove this once configurator supports setting rx_id
+ if (rxSpiId == NULL || *rxSpiId == 0) {
+ rxSpiIdPtr = NULL;
+ protocolState = STATE_BIND;
+ inavRfChannelCount = 1;
+ inavRfChannelIndex = 0;
+ NRF24L01_SetChannel(INAV_RF_BIND_CHANNEL);
+ } else {
+ rxSpiIdPtr = (uint32_t*)rxSpiId;
+ // use the rxTxAddr provided and go straight into DATA_STATE
+ memcpy(rxTxAddr, rxSpiId, sizeof(uint32_t));
+ rxTxAddr[4] = RX_TX_ADDR_4;
+ inavSetBound();
+ }
+#endif
+
+ NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
+ // put a null packet in the transmit buffer to be sent as ACK on first receive
+ writeAckPayload(ackPayload, payloadSize);
+}
+
+void inavNrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT_MAX;
+ inavNrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol, &rxConfig->rx_spi_id, rxConfig->rx_spi_rf_channel_count);
+}
+#endif
+
diff --git a/src/main/rx/nrf24_inav.h b/src/main/rx/nrf24_inav.h
new file mode 100644
index 0000000000..f499f12a3a
--- /dev/null
+++ b/src/main/rx/nrf24_inav.h
@@ -0,0 +1,28 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include
+#include
+
+struct rxConfig_s;
+struct rxRuntimeConfig_s;
+void inavNrf24Init(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
+void inavNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
+rx_spi_received_e inavNrf24DataReceived(uint8_t *payload);
+
diff --git a/src/main/rx/nrf24_syma.c b/src/main/rx/nrf24_syma.c
new file mode 100644
index 0000000000..b08cfaa275
--- /dev/null
+++ b/src/main/rx/nrf24_syma.c
@@ -0,0 +1,301 @@
+/*
+ * 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 .
+ */
+
+// This file borrows heavily from project Deviation,
+// see http://deviationtx.com
+
+#include
+#include
+#include
+
+#include
+
+#ifdef USE_RX_SYMA
+
+#include "build/build_config.h"
+
+#include "drivers/rx_nrf24l01.h"
+#include "drivers/system.h"
+
+#include "rx/rx_spi.h"
+#include "rx/nrf24_syma.h"
+
+/*
+ * Deviation transmitter sends 345 bind packets, then starts sending data packets.
+ * Packets are send at rate of at least one every 4 milliseconds, ie at least 250Hz.
+ * This means binding phase lasts 1.4 seconds, the transmitter then enters the data phase.
+ * Other transmitters may vary but should have similar characteristics.
+ */
+
+
+/*
+ * SymaX Protocol
+ * No auto acknowledgment
+ * Data rate is 250Kbps
+ * Payload size is 10, static
+ * Bind Phase
+ * uses address {0xab,0xac,0xad,0xae,0xaf}
+ * hops between 4 channels {0x4b, 0x30, 0x40, 0x20}
+ * Data Phase
+ * uses address received in bind packets
+ * hops between 4 channels generated from address received in bind packets
+ *
+ * SymaX5C Protocol
+ * No auto acknowledgment
+ * Payload size is 16, static
+ * Data rate is 1Mbps
+ * Bind Phase
+ * uses address {0x6d,0x6a,0x73,0x73,0x73}
+ * hops between 16 channels {0x27, 0x1b, 0x39, 0x28, 0x24, 0x22, 0x2e, 0x36, 0x19, 0x21, 0x29, 0x14, 0x1e, 0x12, 0x2d, 0x18};
+ * Data phase
+ * uses same address as bind phase
+ * hops between 15 channels {0x1d, 0x2f, 0x26, 0x3d, 0x15, 0x2b, 0x25, 0x24, 0x27, 0x2c, 0x1c, 0x3e, 0x39, 0x2d, 0x22};
+ * (common channels between both phases are: 0x27, 0x39, 0x24, 0x22, 0x2d)
+ */
+
+#define RC_CHANNEL_COUNT 9
+
+enum {
+ RATE_LOW = 0,
+ RATE_MID = 1,
+ RATE_HIGH= 2,
+};
+
+#define FLAG_PICTURE 0x40
+#define FLAG_VIDEO 0x80
+#define FLAG_FLIP 0x40
+#define FLAG_HEADLESS 0x80
+
+#define FLAG_FLIP_X5C 0x01
+#define FLAG_PICTURE_X5C 0x08
+#define FLAG_VIDEO_X5C 0x10
+#define FLAG_RATE_X5C 0x04
+
+STATIC_UNIT_TESTED rx_spi_protocol_e symaProtocol;
+
+typedef enum {
+ STATE_BIND = 0,
+ STATE_DATA
+} protocol_state_t;
+
+STATIC_UNIT_TESTED protocol_state_t protocolState;
+
+// X11, X12, X5C-1 have 10-byte payload, X5C has 16-byte payload
+#define SYMA_X_PROTOCOL_PAYLOAD_SIZE 10
+#define SYMA_X5C_PROTOCOL_PAYLOAD_SIZE 16
+STATIC_UNIT_TESTED uint8_t payloadSize;
+
+#define RX_TX_ADDR_LEN 5
+// set rxTxAddr to SymaX bind values
+STATIC_UNIT_TESTED uint8_t rxTxAddr[RX_TX_ADDR_LEN] = {0xab, 0xac, 0xad, 0xae, 0xaf};
+STATIC_UNIT_TESTED const uint8_t rxTxAddrX5C[RX_TX_ADDR_LEN] = {0x6d, 0x6a, 0x73, 0x73, 0x73}; // X5C uses same address for bind and data
+
+// radio channels for frequency hopping
+#define SYMA_X_RF_BIND_CHANNEL 8
+#define SYMA_X_RF_CHANNEL_COUNT 4
+#define SYMA_X5C_RF_BIND_CHANNEL_COUNT 16
+#define SYMA_X5C_RF_CHANNEL_COUNT 15
+
+STATIC_UNIT_TESTED uint8_t symaRfChannelCount = SYMA_X_RF_CHANNEL_COUNT;
+STATIC_UNIT_TESTED uint8_t symaRfChannelIndex = 0;
+// set rfChannels to SymaX bind channels, reserve enough space for SymaX5C channels
+STATIC_UNIT_TESTED uint8_t symaRfChannels[SYMA_X5C_RF_BIND_CHANNEL_COUNT] = {0x4b, 0x30, 0x40, 0x20};
+STATIC_UNIT_TESTED const uint8_t symaRfChannelsX5C[SYMA_X5C_RF_CHANNEL_COUNT] = {0x1d, 0x2f, 0x26, 0x3d, 0x15, 0x2b, 0x25, 0x24, 0x27, 0x2c, 0x1c, 0x3e, 0x39, 0x2d, 0x22};
+
+static uint32_t packetCount = 0;
+static uint32_t timeOfLastHop;
+static uint32_t hopTimeout = 10000; // 10ms
+
+STATIC_UNIT_TESTED bool symaCheckBindPacket(const uint8_t *packet)
+{
+ bool bindPacket = false;
+ if (symaProtocol == NRF24RX_SYMA_X) {
+ if ((packet[5] == 0xaa) && (packet[6] == 0xaa) && (packet[7] == 0xaa)) {
+ bindPacket = true;
+ rxTxAddr[4] = packet[0];
+ rxTxAddr[3] = packet[1];
+ rxTxAddr[2] = packet[2];
+ rxTxAddr[1] = packet[3];
+ rxTxAddr[0] = packet[4];
+ }
+ } else {
+ if ((packet[0] == 0) && (packet[1] == 0) && (packet[14] == 0xc0) && (packet[15] == 0x17)) {
+ bindPacket = true;
+ }
+ }
+ return bindPacket;
+}
+
+STATIC_UNIT_TESTED uint16_t symaConvertToPwmUnsigned(uint8_t val)
+{
+ uint32_t ret = val;
+ ret = ret * (PWM_RANGE_MAX - PWM_RANGE_MIN) / UINT8_MAX + PWM_RANGE_MIN;
+ return (uint16_t)ret;
+}
+
+STATIC_UNIT_TESTED uint16_t symaConvertToPwmSigned(uint8_t val)
+{
+ int32_t ret = val & 0x7f;
+ ret = (ret * (PWM_RANGE_MAX - PWM_RANGE_MIN)) / (2 * INT8_MAX);
+ if (val & 0x80) {// sign bit set
+ ret = -ret;
+ }
+ return (uint16_t)(PWM_RANGE_MIDDLE + ret);
+}
+
+void symaNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet)
+{
+ rcData[RC_SPI_THROTTLE] = symaConvertToPwmUnsigned(packet[0]); // throttle
+ rcData[RC_SPI_ROLL] = symaConvertToPwmSigned(packet[3]); // aileron
+ if (symaProtocol == NRF24RX_SYMA_X) {
+ rcData[RC_SPI_PITCH] = symaConvertToPwmSigned(packet[1]); // elevator
+ rcData[RC_SPI_YAW] = symaConvertToPwmSigned(packet[2]); // rudder
+ const uint8_t rate = (packet[5] & 0xc0) >> 6;
+ if (rate == RATE_LOW) {
+ rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIN;
+ } else if (rate == RATE_MID) {
+ rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIDDLE;
+ } else {
+ rcData[RC_CHANNEL_RATE] = PWM_RANGE_MAX;
+ }
+ rcData[RC_CHANNEL_FLIP] = packet[6] & FLAG_FLIP ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[RC_CHANNEL_PICTURE] = packet[4] & FLAG_PICTURE ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[RC_CHANNEL_VIDEO] = packet[4] & FLAG_VIDEO ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[RC_CHANNEL_HEADLESS] = packet[14] & FLAG_HEADLESS ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ } else {
+ rcData[RC_SPI_PITCH] = symaConvertToPwmSigned(packet[2]); // elevator
+ rcData[RC_SPI_YAW] = symaConvertToPwmSigned(packet[1]); // rudder
+ const uint8_t flags = packet[14];
+ rcData[RC_CHANNEL_RATE] = flags & FLAG_RATE_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[RC_CHANNEL_FLIP] = flags & FLAG_FLIP_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[RC_CHANNEL_PICTURE] = flags & FLAG_PICTURE_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[RC_CHANNEL_VIDEO] = flags & FLAG_VIDEO_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ }
+}
+
+static void symaHopToNextChannel(void)
+{
+ // hop channel every second packet
+ ++packetCount;
+ if ((packetCount & 0x01) == 0) {
+ ++symaRfChannelIndex;
+ if (symaRfChannelIndex >= symaRfChannelCount) {
+ symaRfChannelIndex = 0;
+ }
+ }
+ NRF24L01_SetChannel(symaRfChannels[symaRfChannelIndex]);
+}
+
+// The SymaX hopping channels are determined by the low bits of rxTxAddress
+static void setSymaXHoppingChannels(uint32_t addr)
+{
+ addr = addr & 0x1f;
+ if (addr == 0x06) {
+ addr = 0x07;
+ }
+ const uint32_t inc = (addr << 24) | (addr << 16) | (addr << 8) | addr;
+ uint32_t * const prfChannels = (uint32_t *)symaRfChannels;
+ if (addr == 0x16) {
+ *prfChannels = 0x28481131;
+ } else if (addr == 0x1e) {
+ *prfChannels = 0x38184121;
+ } else if (addr < 0x10) {
+ *prfChannels = 0x3A2A1A0A + inc;
+ } else if (addr < 0x18) {
+ *prfChannels = 0x1231FA1A + inc;
+ } else {
+ *prfChannels = 0x19FA2202 + inc;
+ }
+}
+
+/*
+ * This is called periodically by the scheduler.
+ * Returns RX_SPI_RECEIVED_DATA if a data packet was received.
+ */
+rx_spi_received_e symaNrf24DataReceived(uint8_t *payload)
+{
+ rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
+
+ switch (protocolState) {
+ case STATE_BIND:
+ if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) {
+ const bool bindPacket = symaCheckBindPacket(payload);
+ if (bindPacket) {
+ ret = RX_SPI_RECEIVED_BIND;
+ protocolState = STATE_DATA;
+ // using protocol NRF24L01_SYMA_X, since NRF24L01_SYMA_X5C went straight into data mode
+ // set the hopping channels as determined by the rxTxAddr received in the bind packet
+ setSymaXHoppingChannels(rxTxAddr[0]);
+ // set the NRF24 to use the rxTxAddr received in the bind packet
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddr, RX_TX_ADDR_LEN);
+ packetCount = 0;
+ symaRfChannelIndex = 0;
+ NRF24L01_SetChannel(symaRfChannels[0]);
+ }
+ }
+ break;
+ case STATE_DATA:
+ // read the payload, processing of payload is deferred
+ if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) {
+ symaHopToNextChannel();
+ timeOfLastHop = micros();
+ ret = RX_SPI_RECEIVED_DATA;
+ }
+ if (micros() > timeOfLastHop + hopTimeout) {
+ symaHopToNextChannel();
+ timeOfLastHop = micros();
+ }
+ break;
+ }
+ return ret;
+}
+
+static void symaNrf24Setup(rx_spi_protocol_e protocol)
+{
+ symaProtocol = protocol;
+ NRF24L01_Initialize(BV(NRF24L01_00_CONFIG_EN_CRC) | BV( NRF24L01_00_CONFIG_CRCO)); // sets PWR_UP, EN_CRC, CRCO - 2 byte CRC
+ NRF24L01_SetupBasic();
+
+ if (symaProtocol == NRF24RX_SYMA_X) {
+ payloadSize = SYMA_X_PROTOCOL_PAYLOAD_SIZE;
+ NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_250Kbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
+ protocolState = STATE_BIND;
+ // RX_ADDR for pipes P1-P5 are left at default values
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddr, RX_TX_ADDR_LEN);
+ } else {
+ payloadSize = SYMA_X5C_PROTOCOL_PAYLOAD_SIZE;
+ NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
+ // RX_ADDR for pipes P1-P5 are left at default values
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddrX5C, RX_TX_ADDR_LEN);
+ // just go straight into data mode, since the SYMA_X5C protocol does not actually require binding
+ protocolState = STATE_DATA;
+ symaRfChannelCount = SYMA_X5C_RF_CHANNEL_COUNT;
+ memcpy(symaRfChannels, symaRfChannelsX5C, SYMA_X5C_RF_CHANNEL_COUNT);
+ }
+ NRF24L01_SetChannel(symaRfChannels[0]);
+ NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, payloadSize);
+
+ NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
+}
+
+void symaNrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT;
+ symaNrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol);
+}
+#endif
+
diff --git a/src/main/rx/nrf24_syma.h b/src/main/rx/nrf24_syma.h
new file mode 100644
index 0000000000..688bbb99ee
--- /dev/null
+++ b/src/main/rx/nrf24_syma.h
@@ -0,0 +1,28 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include
+#include
+
+struct rxConfig_s;
+struct rxRuntimeConfig_s;
+void symaNrf24Init(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
+void symaNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
+rx_spi_received_e symaNrf24DataReceived(uint8_t *payload);
+
diff --git a/src/main/rx/nrf24_v202.c b/src/main/rx/nrf24_v202.c
new file mode 100644
index 0000000000..be724469f3
--- /dev/null
+++ b/src/main/rx/nrf24_v202.c
@@ -0,0 +1,258 @@
+/*
+ * 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 .
+ */
+
+// this file is copied with modifications from bradwii for jd385
+// see https://github.com/hackocopter/bradwii-jd385
+
+#include
+#include
+#include
+
+#include
+#include "build/build_config.h"
+
+
+#ifdef USE_RX_V202
+
+#include "drivers/rx_nrf24l01.h"
+#include "drivers/system.h"
+
+#include "rx/rx_spi.h"
+#include "rx/nrf24_v202.h"
+
+/*
+ * V202 Protocol
+ * No auto acknowledgment
+ * Payload size is 16 and static
+ * Data rate is 1Mbps, there is a 256Kbps data rate used by the Deviation transmitter implementation
+ * Bind Phase
+ * uses address {0x66, 0x88, 0x68, 0x68, 0x68}
+ * uses channels from the frequency hopping table
+ * Data phase
+ * uses same address as bind phase
+ * hops between 16 channels that are set using the txId sent in the bind packet and the frequency hopping table
+ */
+
+#define V2X2_PAYLOAD_SIZE 16
+#define V2X2_NFREQCHANNELS 16
+#define TXIDSIZE 3
+#define V2X2_RC_CHANNEL_COUNT 11
+
+enum {
+ // packet[14] flags
+ V2X2_FLAG_CAMERA = 0x01, // also automatic Missile Launcher and Hoist in one direction
+ V2X2_FLAG_VIDEO = 0x02, // also Sprayer, Bubbler, Missile Launcher(1), and Hoist in the other dir.
+ V2X2_FLAG_FLIP = 0x04,
+ V2X2_FLAG_UNK9 = 0x08,
+ V2X2_FLAG_LED = 0x10,
+ V2X2_FLAG_UNK10 = 0x20,
+ V2X2_FLAG_BIND = 0xC0,
+ // packet[10] flags
+ V2X2_FLAG_HEADLESS = 0x02,
+ V2X2_FLAG_MAG_CAL_X = 0x08,
+ V2X2_FLAG_MAG_CAL_Y = 0x20
+};
+
+enum {
+ PHASE_NOT_BOUND = 0,
+ PHASE_BOUND
+};
+
+// This is frequency hopping table for V202 protocol
+// The table is the first 4 rows of 32 frequency hopping
+// patterns, all other rows are derived from the first 4.
+// For some reason the protocol avoids channels, dividing
+// by 16 and replaces them by subtracting 3 from the channel
+// number in this case.
+// The pattern is defined by 5 least significant bits of
+// sum of 3 bytes comprising TX id
+static const uint8_t v2x2_freq_hopping[][V2X2_NFREQCHANNELS] = {
+ { 0x27, 0x1B, 0x39, 0x28, 0x24, 0x22, 0x2E, 0x36,
+ 0x19, 0x21, 0x29, 0x14, 0x1E, 0x12, 0x2D, 0x18 }, // 00
+ { 0x2E, 0x33, 0x25, 0x38, 0x19, 0x12, 0x18, 0x16,
+ 0x2A, 0x1C, 0x1F, 0x37, 0x2F, 0x23, 0x34, 0x10 }, // 01
+ { 0x11, 0x1A, 0x35, 0x24, 0x28, 0x18, 0x25, 0x2A,
+ 0x32, 0x2C, 0x14, 0x27, 0x36, 0x34, 0x1C, 0x17 }, // 02
+ { 0x22, 0x27, 0x17, 0x39, 0x34, 0x28, 0x2B, 0x1D,
+ 0x18, 0x2A, 0x21, 0x38, 0x10, 0x26, 0x20, 0x1F } // 03
+};
+
+STATIC_UNIT_TESTED uint8_t rf_channels[V2X2_NFREQCHANNELS];
+STATIC_UNIT_TESTED uint8_t rf_ch_num;
+STATIC_UNIT_TESTED uint8_t bind_phase;
+static uint32_t packet_timer;
+STATIC_UNIT_TESTED uint8_t txid[TXIDSIZE];
+static uint32_t rx_timeout;
+extern uint16_t rxSpiRcData[];
+
+static const unsigned char v2x2_channelindex[] = {RC_SPI_THROTTLE,RC_SPI_YAW,RC_SPI_PITCH,RC_SPI_ROLL,
+ RC_SPI_AUX1,RC_SPI_AUX2,RC_SPI_AUX3,RC_SPI_AUX4,RC_SPI_AUX5,RC_SPI_AUX6,RC_SPI_AUX7};
+
+static void prepare_to_bind(void)
+{
+ packet_timer = micros();
+ for (int i = 0; i < V2X2_NFREQCHANNELS; ++i) {
+ rf_channels[i] = v2x2_freq_hopping[0][i];
+ }
+ rx_timeout = 1000L;
+}
+
+static void switch_channel(void)
+{
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_channels[rf_ch_num]);
+ if (++rf_ch_num >= V2X2_NFREQCHANNELS) rf_ch_num = 0;
+}
+
+static void v2x2_set_tx_id(uint8_t *id)
+{
+ uint8_t sum;
+ txid[0] = id[0];
+ txid[1] = id[1];
+ txid[2] = id[2];
+ sum = id[0] + id[1] + id[2];
+
+ // Base row is defined by lowest 2 bits
+ const uint8_t *fh_row = v2x2_freq_hopping[sum & 0x03];
+ // Higher 3 bits define increment to corresponding row
+ uint8_t increment = (sum & 0x1e) >> 2;
+ for (int i = 0; i < V2X2_NFREQCHANNELS; ++i) {
+ uint8_t val = fh_row[i] + increment;
+ // Strange avoidance of channels divisible by 16
+ rf_channels[i] = (val & 0x0f) ? val : val - 3;
+ }
+}
+
+static void decode_bind_packet(uint8_t *packet)
+{
+ if ((packet[14] & V2X2_FLAG_BIND) == V2X2_FLAG_BIND) {
+ // Fill out rf_channels with bound protocol parameters
+ v2x2_set_tx_id(&packet[7]);
+ bind_phase = PHASE_BOUND;
+ rx_timeout = 1000L; // find the channel as fast as possible
+ }
+}
+
+// Returns whether the data was successfully decoded
+static rx_spi_received_e decode_packet(uint8_t *packet)
+{
+ if(bind_phase != PHASE_BOUND) {
+ decode_bind_packet(packet);
+ return RX_SPI_RECEIVED_BIND;
+ }
+ // Decode packet
+ if ((packet[14] & V2X2_FLAG_BIND) == V2X2_FLAG_BIND) {
+ return RX_SPI_RECEIVED_BIND;
+ }
+ if (packet[7] != txid[0] ||
+ packet[8] != txid[1] ||
+ packet[9] != txid[2]) {
+ return RX_SPI_RECEIVED_NONE;
+ }
+ // Restore regular interval
+ rx_timeout = 10000L; // 4ms interval, duplicate packets, (8ms unique) + 25%
+ // TREA order in packet to MultiWii order is handled by
+ // correct assignment to channelindex
+ // Throttle 0..255 to 1000..2000
+ rxSpiRcData[v2x2_channelindex[0]] = ((uint16_t)packet[0]) * 1000 / 255 + 1000;
+ for (int i = 1; i < 4; ++i) {
+ uint8_t a = packet[i];
+ rxSpiRcData[v2x2_channelindex[i]] = ((uint16_t)(a < 0x80 ? 0x7f - a : a)) * 1000 / 255 + 1000;
+ }
+ const uint8_t flags[] = {V2X2_FLAG_LED, V2X2_FLAG_FLIP, V2X2_FLAG_CAMERA, V2X2_FLAG_VIDEO}; // two more unknown bits
+ for (int i = 4; i < 8; ++i) {
+ rxSpiRcData[v2x2_channelindex[i]] = (packet[14] & flags[i-4]) ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ }
+ const uint8_t flags10[] = {V2X2_FLAG_HEADLESS, V2X2_FLAG_MAG_CAL_X, V2X2_FLAG_MAG_CAL_Y};
+ for (int i = 8; i < 11; ++i) {
+ rxSpiRcData[v2x2_channelindex[i]] = (packet[10] & flags10[i-8]) ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ }
+ packet_timer = micros();
+ return RX_SPI_RECEIVED_DATA;
+}
+
+void v202Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet)
+{
+ UNUSED(rcData);
+ UNUSED(packet);
+ // Ideally the decoding of the packet should be moved into here, to reduce the overhead of v202DataReceived function.
+}
+
+static rx_spi_received_e readrx(uint8_t *packet)
+{
+ if (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & BV(NRF24L01_07_STATUS_RX_DR))) {
+ uint32_t t = micros() - packet_timer;
+ if (t > rx_timeout) {
+ switch_channel();
+ packet_timer = micros();
+ }
+ return RX_SPI_RECEIVED_NONE;
+ }
+ packet_timer = micros();
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, BV(NRF24L01_07_STATUS_RX_DR)); // clear the RX_DR flag
+ NRF24L01_ReadPayload(packet, V2X2_PAYLOAD_SIZE);
+ NRF24L01_FlushRx();
+
+ switch_channel();
+ return decode_packet(packet);
+}
+
+/*
+ * This is called periodically by the scheduler.
+ * Returns RX_SPI_RECEIVED_DATA if a data packet was received.
+ */
+rx_spi_received_e v202Nrf24DataReceived(uint8_t *packet)
+{
+ return readrx(packet);
+}
+
+static void v202Nrf24Setup(rx_spi_protocol_e protocol)
+{
+ NRF24L01_Initialize(BV(NRF24L01_00_CONFIG_EN_CRC) | BV(NRF24L01_00_CONFIG_CRCO)); // 2-bytes CRC
+
+ NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknowledgment
+ NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, BV(NRF24L01_02_EN_RXADDR_ERX_P0)); // Enable data pipe 0
+ NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, NRF24L01_03_SETUP_AW_5BYTES); // 5-byte RX/TX address
+ NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0xFF); // 4ms retransmit t/o, 15 tries
+ if (protocol == NRF24RX_V202_250K) {
+ NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_250Kbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
+ } else {
+ NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
+ }
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, BV(NRF24L01_07_STATUS_RX_DR) | BV(NRF24L01_07_STATUS_TX_DS) | BV(NRF24L01_07_STATUS_MAX_RT)); // Clear data ready, data sent, and retransmit
+ NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, V2X2_PAYLOAD_SIZE); // bytes of data payload for pipe 0
+ NRF24L01_WriteReg(NRF24L01_17_FIFO_STATUS, 0x00); // Just in case, no real bits to write here
+#define RX_TX_ADDR_LEN 5
+ const uint8_t rx_tx_addr[RX_TX_ADDR_LEN] = {0x66, 0x88, 0x68, 0x68, 0x68};
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, RX_TX_ADDR_LEN);
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, RX_TX_ADDR_LEN);
+
+ NRF24L01_FlushTx();
+ NRF24L01_FlushRx();
+
+ rf_ch_num = 0;
+ bind_phase = PHASE_NOT_BOUND;
+ prepare_to_bind();
+ switch_channel();
+ NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
+}
+
+void v202Nrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ rxRuntimeConfig->channelCount = V2X2_RC_CHANNEL_COUNT;
+ v202Nrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol);
+}
+#endif
diff --git a/src/main/rx/nrf24_v202.h b/src/main/rx/nrf24_v202.h
new file mode 100644
index 0000000000..f3600ad7d0
--- /dev/null
+++ b/src/main/rx/nrf24_v202.h
@@ -0,0 +1,27 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include
+#include
+
+struct rxConfig_s;
+struct rxRuntimeConfig_s;
+void v202Nrf24Init(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
+void v202Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
+rx_spi_received_e v202Nrf24DataReceived(uint8_t *payload);
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 598a2f0257..2b26282af6 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -36,6 +36,7 @@
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
+#include "drivers/rx_spi.h"
#include "drivers/system.h"
#include "fc/rc_controls.h"
@@ -54,6 +55,7 @@
#include "rx/xbus.h"
#include "rx/ibus.h"
#include "rx/jetiexbus.h"
+#include "rx/rx_spi.h"
//#define DEBUG_RX_SIGNAL_LOSS
@@ -195,6 +197,18 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct
}
#endif
+#ifdef USE_RX_SPI
+ if (feature(FEATURE_RX_SPI)) {
+ rxRefreshRate = 10000;
+ const rx_spi_type_e spiType = feature(FEATURE_SOFTSPI) ? RX_SPI_SOFTSPI : RX_SPI_HARDSPI;
+ const bool enabled = rxSpiInit(spiType, rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
+ if (!enabled) {
+ featureClear(FEATURE_RX_SPI);
+ rcReadRawFunc = nullReadRawRC;
+ }
+ }
+#endif
+
#ifndef SKIP_RX_PWM_PPM
if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) {
rxPwmInit(rxConfig, &rxRuntimeConfig);
@@ -338,6 +352,17 @@ bool rxUpdate(uint32_t currentTime)
}
#endif
+#ifdef USE_RX_SPI
+ if (feature(FEATURE_RX_SPI)) {
+ rxDataReceived = rxSpiDataReceived();
+ if (rxDataReceived) {
+ rxSignalReceived = true;
+ rxIsInFailsafeMode = false;
+ needRxSignalBefore = currentTime + DELAY_10_HZ;
+ }
+ }
+#endif
+
#ifndef SKIP_RX_MSP
if (feature(FEATURE_RX_MSP)) {
const uint8_t frameStatus = rxMspFrameStatus();
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 3a1d6c9d62..8d5ea135af 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -113,9 +113,9 @@ typedef struct rxConfig_s {
uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order
uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first.
uint8_t sbus_inversion; // default sbus (Futaba, FrSKY) is inverted. Support for uninverted OpenLRS (and modified FrSKY) receivers.
- uint8_t nrf24rx_protocol; // type of nrf24 protocol (0 = v202 250kbps). Must be enabled by FEATURE_RX_NRF24 first.
- uint32_t nrf24rx_id;
- uint8_t nrf24rx_channel_count;
+ uint8_t rx_spi_protocol; // type of nrf24 protocol (0 = v202 250kbps). Must be enabled by FEATURE_RX_NRF24 first.
+ uint32_t rx_spi_id;
+ uint8_t rx_spi_rf_channel_count;
uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers
uint8_t spektrum_sat_bind_autoreset; // whenever we will reset (exit) binding mode after hard reboot
uint8_t rssi_channel;
diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c
new file mode 100644
index 0000000000..44b6c13fe1
--- /dev/null
+++ b/src/main/rx/rx_spi.c
@@ -0,0 +1,139 @@
+/*
+ * 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
+
+#ifdef USE_RX_SPI
+
+#include "build/build_config.h"
+
+#include "drivers/rx_nrf24l01.h"
+#include "rx/rx.h"
+#include "rx/rx_spi.h"
+#include "rx/nrf24_cx10.h"
+#include "rx/nrf24_syma.h"
+#include "rx/nrf24_v202.h"
+#include "rx/nrf24_h8_3d.h"
+#include "rx/nrf24_inav.h"
+
+
+uint16_t rxSpiRcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+STATIC_UNIT_TESTED uint8_t rxSpiPayload[RX_SPI_MAX_PAYLOAD_SIZE];
+STATIC_UNIT_TESTED uint8_t rxSpiNewPacketAvailable; // set true when a new packet is received
+
+typedef void (*protocolInitPtr)(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+typedef rx_spi_received_e (*protocolDataReceivedPtr)(uint8_t *payload);
+typedef void (*protocolSetRcDataFromPayloadPtr)(uint16_t *rcData, const uint8_t *payload);
+
+static protocolInitPtr protocolInit;
+static protocolDataReceivedPtr protocolDataReceived;
+static protocolSetRcDataFromPayloadPtr protocolSetRcDataFromPayload;
+
+STATIC_UNIT_TESTED uint16_t rxSpiReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel)
+{
+ BUILD_BUG_ON(NRF24L01_MAX_PAYLOAD_SIZE > RX_SPI_MAX_PAYLOAD_SIZE);
+ if (channel >= rxRuntimeConfig->channelCount) {
+ return 0;
+ }
+ if (rxSpiNewPacketAvailable) {
+ protocolSetRcDataFromPayload(rxSpiRcData, rxSpiPayload);
+ rxSpiNewPacketAvailable = false;
+ }
+ return rxSpiRcData[channel];
+}
+
+STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
+{
+ switch (protocol) {
+ default:
+#ifdef USE_RX_V202
+ case NRF24RX_V202_250K:
+ case NRF24RX_V202_1M:
+ protocolInit = v202Nrf24Init;
+ protocolDataReceived = v202Nrf24DataReceived;
+ protocolSetRcDataFromPayload = v202Nrf24SetRcDataFromPayload;
+ break;
+#endif
+#ifdef USE_RX_SYMA
+ case NRF24RX_SYMA_X:
+ case NRF24RX_SYMA_X5C:
+ protocolInit = symaNrf24Init;
+ protocolDataReceived = symaNrf24DataReceived;
+ protocolSetRcDataFromPayload = symaNrf24SetRcDataFromPayload;
+ break;
+#endif
+#ifdef USE_RX_CX10
+ case NRF24RX_CX10:
+ case NRF24RX_CX10A:
+ protocolInit = cx10Nrf24Init;
+ protocolDataReceived = cx10Nrf24DataReceived;
+ protocolSetRcDataFromPayload = cx10Nrf24SetRcDataFromPayload;
+ break;
+#endif
+#ifdef USE_RX_H8_3D
+ case NRF24RX_H8_3D:
+ protocolInit = h8_3dNrf24Init;
+ protocolDataReceived = h8_3dNrf24DataReceived;
+ protocolSetRcDataFromPayload = h8_3dNrf24SetRcDataFromPayload;
+ break;
+#endif
+#ifdef USE_RX_INAV
+ case NRF24RX_INAV:
+ protocolInit = inavNrf24Init;
+ protocolDataReceived = inavNrf24DataReceived;
+ protocolSetRcDataFromPayload = inavNrf24SetRcDataFromPayload;
+ break;
+#endif
+ }
+ return true;
+}
+
+/*
+ * Returns true if the RX has received new data.
+ * Called from updateRx in rx.c, updateRx called from taskUpdateRxCheck.
+ * If taskUpdateRxCheck returns true, then taskUpdateRxMain will shortly be called.
+ */
+bool rxSpiDataReceived(void)
+{
+ if (protocolDataReceived(rxSpiPayload) == RX_SPI_RECEIVED_DATA) {
+ rxSpiNewPacketAvailable = true;
+ return true;
+ }
+ return false;
+}
+
+/*
+ * Set and initialize the RX protocol
+ */
+bool rxSpiInit(rx_spi_type_e spiType, const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
+{
+ bool ret = false;
+ rxSpiDeviceInit(spiType);
+ if (rxSpiSetProtocol(rxConfig->rx_spi_protocol)) {
+ protocolInit(rxConfig, rxRuntimeConfig);
+ ret = true;
+ }
+ rxSpiNewPacketAvailable = false;
+ if (callback) {
+ *callback = rxSpiReadRawRC;
+ }
+ return ret;
+}
+#endif
diff --git a/src/main/rx/rx_spi.h b/src/main/rx/rx_spi.h
new file mode 100644
index 0000000000..48b7f0a54f
--- /dev/null
+++ b/src/main/rx/rx_spi.h
@@ -0,0 +1,76 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include
+#include
+
+#include "rx/rx.h"
+
+typedef enum {
+ NRF24RX_V202_250K = 0,
+ NRF24RX_V202_1M,
+ NRF24RX_SYMA_X,
+ NRF24RX_SYMA_X5C,
+ NRF24RX_CX10,
+ NRF24RX_CX10A,
+ NRF24RX_H8_3D,
+ NRF24RX_INAV,
+ NRF24RX_PROTOCOL_COUNT
+} rx_spi_protocol_e;
+
+typedef enum {
+ RX_SPI_RECEIVED_NONE = 0,
+ RX_SPI_RECEIVED_BIND,
+ RX_SPI_RECEIVED_DATA
+} rx_spi_received_e;
+
+// RC channels in AETR order
+typedef enum {
+ RC_SPI_ROLL = 0,
+ RC_SPI_PITCH,
+ RC_SPI_THROTTLE,
+ RC_SPI_YAW,
+ RC_SPI_AUX1,
+ RC_SPI_AUX2,
+ RC_SPI_AUX3,
+ RC_SPI_AUX4,
+ RC_SPI_AUX5,
+ RC_SPI_AUX6,
+ RC_SPI_AUX7,
+ RC_SPI_AUX8,
+ RC_SPI_AUX9,
+ RC_SPI_AUX10,
+ RC_SPI_AUX11,
+ RC_SPI_AUX12,
+ RC_SPI_AUX13,
+ RC_SPI_AUX14
+} rc_spi_aetr_e;
+
+// RC channels as used by deviation
+#define RC_CHANNEL_RATE RC_SPI_AUX1
+#define RC_CHANNEL_FLIP RC_SPI_AUX2
+#define RC_CHANNEL_PICTURE RC_SPI_AUX3
+#define RC_CHANNEL_VIDEO RC_SPI_AUX4
+#define RC_CHANNEL_HEADLESS RC_SPI_AUX5
+#define RC_CHANNEL_RTH RC_SPI_AUX6 // return to home
+
+bool rxSpiDataReceived(void);
+struct rxConfig_s;
+struct rxRuntimeConfig_s;
+bool rxSpiInit(rx_spi_type_e spiType, const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig, rcReadRawDataPtr *callback);
diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h
index beb4be9657..7c2bb3738c 100644
--- a/src/main/target/CJMCU/target.h
+++ b/src/main/target/CJMCU/target.h
@@ -24,14 +24,16 @@
#define LED1 PC13
#define LED2 PC15
-#define ACC
-#define USE_ACC_MPU6050
+#undef BEEPER
#define GYRO
#define USE_GYRO_MPU6050
-//#define MAG
-//#define USE_MAG_HMC5883
+#define ACC
+#define USE_ACC_MPU6050
+
+#define MAG
+#define USE_MAG_HMC5883
#define BRUSHED_MOTORS
@@ -41,30 +43,83 @@
#define SERIAL_PORT_COUNT 2
#define USE_I2C
-#define I2C_DEVICE (I2CDEV_1)
+#define I2C_DEVICE (I2CDEV_1)
// #define SOFT_I2C // enable to test software i2c
// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
// #define SOFT_I2C_PB67
+#define USE_SPI
+#define USE_SPI_DEVICE_1
-#if (FLASH_SIZE > 64)
-#define BLACKBOX
-#define USE_SERVOS
-#define SPEKTRUM_BIND
-// USART2, PA3
-#define BIND_PIN PA3
-#else
-#undef USE_CLI
+#define USE_RX_NRF24
+#ifdef USE_RX_NRF24
+
+#define USE_RX_SPI
+#define RX_SPI_INSTANCE SPI1
+
+// Nordic Semiconductor uses 'CSN', STM uses 'NSS'
+#define RX_CE_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA
+#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA
+#define RX_IRQ_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA
+#define RX_CE_PIN PA4
+#define RX_NSS_PIN PA11
+#define RX_SCK_PIN PA5
+#define RX_MISO_PIN PA6
+#define RX_MOSI_PIN PA7
+#define RX_IRQ_PIN PA8
+// CJMCU has NSS on PA11, rather than the standard PA4
+#define SPI1_NSS_PIN RX_NSS_PIN
+#define SPI1_SCK_PIN RX_SCK_PIN
+#define SPI1_MISO_PIN RX_MISO_PIN
+#define SPI1_MOSI_PIN RX_MOSI_PIN
+
+#define USE_RX_NRF24
+#define USE_RX_CX10
+#define USE_RX_H8_3D
+#define USE_RX_INAV
+#define USE_RX_SYMA
+#define USE_RX_V202
+//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_SYMA_X5
+//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_SYMA_X5C
+#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_INAV
+//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_H8_3D
+//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_CX10A
+//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_V202_1M
+
+#define DEFAULT_RX_FEATURE FEATURE_RX_SPI
+//#define TELEMETRY
+//#define TELEMETRY_LTM
+//#define TELEMETRY_NRF24_LTM
+#define SKIP_RX_PWM_PPM
#undef SERIAL_RX
-#define SKIP_TASK_STATISTICS
-#define SKIP_CLI_COMMAND_HELP
+//#undef SKIP_TASK_STATISTICS
+
+#else
+
+#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
+#undef SKIP_RX_MSP
+#define SPEKTRUM_BIND
+#define BIND_PIN PA3 // UART2, PA3
+
+#endif //USE_RX_NRF24
+
+#define BRUSHED_MOTORS
+#define DEFAULT_FEATURES FEATURE_MOTOR_STOP
+#define SKIP_SERIAL_PASSTHROUGH
#define SKIP_PID_FLOAT
-#endif
+#undef USE_CLI
// Since the CJMCU PCB has holes for 4 motors in each corner we can save same flash space by disabling support for other mixers.
#define USE_QUAD_MIXER_ONLY
-#define SKIP_SERIAL_PASSTHROUGH
+#undef USE_SERVOS
+
+#if (FLASH_SIZE <= 64)
+#undef BLACKBOX
+#endif
+
+// Number of available PWM outputs
+//#define MAX_PWM_OUTPUT_PORTS 4
// IO - assuming all IOs on 48pin package TODO
#define TARGET_IO_PORTA 0xffff
diff --git a/src/main/target/CJMCU/target.mk b/src/main/target/CJMCU/target.mk
index 72f2c64b03..8da31647cf 100644
--- a/src/main/target/CJMCU/target.mk
+++ b/src/main/target/CJMCU/target.mk
@@ -5,7 +5,8 @@ TARGET_SRC = \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu6050.c \
drivers/compass_hmc5883l.c \
- flight/gtune.c \
blackbox/blackbox.c \
- blackbox/blackbox_io.c
+ blackbox/blackbox_io.c \
+ telemetry/telemetry.c \
+ telemetry/ltm.c