diff --git a/Makefile b/Makefile
index 078dc834c2..167e187d45 100644
--- a/Makefile
+++ b/Makefile
@@ -418,6 +418,72 @@ COMMON_SRC = \
sensors/initialisation.c \
$(CMSIS_SRC) \
$(DEVICE_STDPERIPH_SRC)
+=======
+INCLUDE_DIRS := $(INCLUDE_DIRS) \
+ $(TARGET_DIR)
+
+VPATH := $(VPATH):$(TARGET_DIR)
+
+COMMON_SRC = build_config.c \
+ debug.c \
+ version.c \
+ $(TARGET_SRC) \
+ config/config.c \
+ config/runtime_config.c \
+ common/maths.c \
+ common/printf.c \
+ common/typeconversion.c \
+ common/encoding.c \
+ common/filter.c \
+ scheduler/scheduler.c \
+ scheduler/scheduler_tasks.c \
+ main.c \
+ mw.c \
+ flight/failsafe.c \
+ flight/pid.c \
+ flight/imu.c \
+ flight/hil.c \
+ flight/mixer.c \
+ drivers/bus_i2c_soft.c \
+ drivers/serial.c \
+ drivers/sound_beeper.c \
+ drivers/system.c \
+ drivers/gps_i2cnav.c \
+ drivers/gyro_sync.c \
+ drivers/buf_writer.c \
+ drivers/rx_nrf24l01.c \
+ io/beeper.c \
+ io/rc_controls.c \
+ io/rc_curves.c \
+ io/serial.c \
+ io/serial_4way.c \
+ io/serial_4way_avrootloader.c \
+ io/serial_4way_stk500v2.c \
+ io/serial_cli.c \
+ io/serial_msp.c \
+ io/statusindicator.c \
+ rx/rx.c \
+ rx/pwm.c \
+ rx/msp.c \
+ rx/sbus.c \
+ rx/sumd.c \
+ rx/sumh.c \
+ rx/spektrum.c \
+ rx/xbus.c \
+ rx/ibus.c \
+ rx/nrf24.c \
+ rx/nrf24_cx10.c \
+ rx/nrf24_syma.c \
+ rx/nrf24_v202.c \
+ sensors/acceleration.c \
+ sensors/battery.c \
+ sensors/boardalignment.c \
+ sensors/compass.c \
+ sensors/gyro.c \
+ sensors/initialisation.c \
+ $(CMSIS_SRC) \
+ $(DEVICE_STDPERIPH_SRC)
+>>>>>>> NRF24 support for iNav.
HIGHEND_SRC = \
blackbox/blackbox.c \
@@ -455,6 +521,7 @@ VCP_SRC = \
drivers/serial_usb_vcp.c
else
VCP_SRC = \
+<<<<<<< HEAD
vcp/hw_config.c \
vcp/stm32_it.c \
vcp/usb_desc.c \
@@ -496,6 +563,7 @@ STM32F4xx_COMMON_SRC = \
startup_stm32f40xx.s \
target/system_stm32f4xx.c \
drivers/accgyro_mpu.c \
+<<<<<<< HEAD
drivers/adc_stm32f4xx.c \
drivers/adc_stm32f4xx.c \
drivers/bus_i2c_stm32f10x.c \
@@ -518,6 +586,102 @@ endif
ifneq ($(filter ONBOARDFLASH,$(FEATURES)),)
TARGET_SRC += \
+=======
+ drivers/accgyro_mpu3050.c \
+ drivers/accgyro_mpu6050.c \
+ drivers/accgyro_mpu6500.c \
+ drivers/accgyro_spi_mpu6500.c \
+ drivers/barometer_bmp085.c \
+ drivers/barometer_bmp280.c \
+ drivers/barometer_ms5611.c \
+ drivers/compass_ak8975.c \
+ drivers/compass_hmc5883l.c \
+ drivers/compass_mag3110.c \
+ drivers/flash_m25p16.c \
+ drivers/light_ws2811strip.c \
+ drivers/light_ws2811strip_stm32f10x.c \
+ drivers/sonar_hcsr04.c \
+ drivers/sonar_srf10.c \
+ io/flashfs.c \
+ hardware_revision.c \
+ $(HIGHEND_SRC) \
+ $(COMMON_SRC)
+=======
+ vcp/hw_config.c \
+ vcp/stm32_it.c \
+ vcp/usb_desc.c \
+ vcp/usb_endp.c \
+ vcp/usb_istr.c \
+ vcp/usb_prop.c \
+ vcp/usb_pwr.c \
+ drivers/serial_usb_vcp.c
+
+NAZE_SRC = startup_stm32f10x_md_gcc.S \
+ drivers/accgyro_adxl345.c \
+ drivers/accgyro_bma280.c \
+ drivers/accgyro_l3g4200d.c \
+ drivers/accgyro_mma845x.c \
+ drivers/accgyro_mpu.c \
+ drivers/accgyro_mpu3050.c \
+ drivers/accgyro_mpu6050.c \
+ drivers/accgyro_mpu6500.c \
+ drivers/accgyro_spi_mpu6500.c \
+ drivers/adc.c \
+ drivers/adc_stm32f10x.c \
+ drivers/barometer_bmp085.c \
+ drivers/barometer_ms5611.c \
+ drivers/barometer_bmp280.c \
+ drivers/bus_spi.c \
+ drivers/bus_spi_soft.c \
+ drivers/bus_i2c_stm32f10x.c \
+ drivers/compass_hmc5883l.c \
+ drivers/compass_mag3110.c \
+ drivers/compass_ak8975.c \
+ drivers/display_ug2864hsweg01.h \
+ drivers/flash_m25p16.c \
+ drivers/gpio_stm32f10x.c \
+ drivers/inverter.c \
+ drivers/light_led_stm32f10x.c \
+ drivers/light_ws2811strip.c \
+ drivers/light_ws2811strip_stm32f10x.c \
+ drivers/sonar_hcsr04.c \
+ drivers/sonar_srf10.c \
+ drivers/pwm_mapping.c \
+ drivers/pwm_output.c \
+ drivers/pwm_rx.c \
+ drivers/serial_softserial.c \
+ drivers/serial_uart.c \
+ drivers/serial_uart_stm32f10x.c \
+ drivers/sound_beeper_stm32f10x.c \
+ drivers/system_stm32f10x.c \
+ drivers/timer.c \
+ drivers/timer_stm32f10x.c \
+ io/flashfs.c \
+ hardware_revision.c \
+ $(HIGHEND_SRC) \
+ $(COMMON_SRC)
+>>>>>>> NRF24 support for iNav.
+
+ALIENFLIGHTF1_SRC = $(NAZE_SRC)
+
+EUSTM32F103RC_SRC = \
+ $(STM32F10x_COMMON_SRC) \
+ drivers/accgyro_adxl345.c \
+ drivers/accgyro_bma280.c \
+ drivers/accgyro_l3g4200d.c \
+ drivers/accgyro_mma845x.c \
+ drivers/accgyro_mpu.c \
+ drivers/accgyro_mpu3050.c \
+ drivers/accgyro_mpu6050.c \
+ drivers/accgyro_spi_mpu6000.c \
+ drivers/accgyro_spi_mpu6500.c \
+ drivers/barometer_bmp085.c \
+ drivers/barometer_bmp280.c \
+ drivers/barometer_ms5611.c \
+ drivers/compass_ak8975.c \
+ drivers/compass_hmc5883l.c \
+ drivers/compass_mag3110.c \
+>>>>>>> NRF24 support for iNav.
drivers/flash_m25p16.c \
io/flashfs.c
endif
diff --git a/src/main/config/config.c b/src/main/config/config.c
index a5c7a637d4..543e2fbdaa 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -55,6 +55,7 @@
#include "io/gps.h"
#include "rx/rx.h"
+#include "rx/nrf24.h"
#include "blackbox/blackbox_io.h"
@@ -81,6 +82,13 @@
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
+#ifndef DEFAULT_RX_FEATURE
+#define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM
+#endif
+#ifndef NRF24_DEFAULT_PROTOCOL
+#define NRF24_DEFAULT_PROTOCOL 0
+#endif
+
#if !defined(FLASH_SIZE)
#error "Flash size not defined for target. (specify in KB)"
#endif
@@ -465,6 +473,7 @@ static void resetConf(void)
resetTelemetryConfig(&masterConfig.telemetryConfig);
masterConfig.rxConfig.serialrx_provider = 0;
+ masterConfig.rxConfig.nrf24rx_protocol = NRF24_DEFAULT_PROTOCOL;
masterConfig.rxConfig.spektrum_sat_bind = 0;
masterConfig.rxConfig.midrc = 1500;
masterConfig.rxConfig.mincheck = 1100;
@@ -794,24 +803,25 @@ void activateConfig(void)
void validateAndFixConfig(void)
{
- if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP))) {
- featureSet(FEATURE_RX_PARALLEL_PWM); // Consider changing the default to PPM
- }
+ if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_NRF24))) {
+ featureSet(DEFAULT_RX_FEATURE);
+ }
- if (featureConfigured(FEATURE_RX_PPM)) {
- featureClear(FEATURE_RX_PARALLEL_PWM);
- }
+ if (featureConfigured(FEATURE_RX_PPM)) {
+ featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_NRF24);
+ }
- if (featureConfigured(FEATURE_RX_MSP)) {
- featureClear(FEATURE_RX_SERIAL);
- featureClear(FEATURE_RX_PARALLEL_PWM);
- featureClear(FEATURE_RX_PPM);
- }
+ if (featureConfigured(FEATURE_RX_MSP)) {
+ featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_NRF24);
+ }
- if (featureConfigured(FEATURE_RX_SERIAL)) {
- featureClear(FEATURE_RX_PARALLEL_PWM);
- featureClear(FEATURE_RX_PPM);
- }
+ if (featureConfigured(FEATURE_RX_SERIAL)) {
+ featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_NRF24);
+ }
+
+ if (featureConfigured(FEATURE_RX_NRF24)) {
+ featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_MSP);
+ }
#if defined(NAV)
// Ensure sane values of navConfig settings
@@ -819,6 +829,7 @@ void validateAndFixConfig(void)
#endif
if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
+ featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_NRF24);
#if defined(STM32F10X)
// rssi adc needs the same ports
featureClear(FEATURE_RSSI_ADC);
diff --git a/src/main/config/config.h b/src/main/config/config.h
index e601b027bc..e9f8bd5597 100644
--- a/src/main/config/config.h
+++ b/src/main/config/config.h
@@ -43,7 +43,8 @@ typedef enum {
FEATURE_DISPLAY = 1 << 17,
FEATURE_ONESHOT125 = 1 << 18,
FEATURE_BLACKBOX = 1 << 19,
- FEATURE_CHANNEL_FORWARDING = 1 << 20
+ FEATURE_CHANNEL_FORWARDING = 1 << 20,
+ FEATURE_RX_NRF24 = 1 << 21,
} features_e;
void handleOneshotFeatureChangeOnRestart(void);
diff --git a/src/main/drivers/bus_spi_soft.c b/src/main/drivers/bus_spi_soft.c
new file mode 100644
index 0000000000..9273e8c685
--- /dev/null
+++ b/src/main/drivers/bus_spi_soft.c
@@ -0,0 +1,97 @@
+/*
+ * 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 "build_config.h"
+
+#ifdef USE_SOFTSPI
+
+#include "gpio.h"
+#include "bus_spi_soft.h"
+
+
+void softSpiInit(const softSPIDevice_t *dev)
+{
+
+#ifdef STM32F303xC
+ GPIO_InitTypeDef GPIO_InitStructure;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+ GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+ // SCK as output
+ GPIO_InitStructure.GPIO_Pin = dev->sck_pin;
+ GPIO_InitStructure(dev->sck_gpio, &GPIO_InitStructure);
+ // MOSI as output
+ GPIO_InitStructure.GPIO_Pin = dev->mosi_pin;
+ GPIO_Init(dev->mosi_gpio, &GPIO_InitStructure);
+ // MISO as input
+ GPIO_InitStructure.GPIO_Pin = dev->miso_pin;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+ GPIO_Init(dev->miso_gpio, &GPIO_InitStructure);
+#ifdef SOFTSPI_NSS_PIN
+ // NSS as gpio not slave select
+ GPIO_InitStructure.GPIO_Pin = dev->nss_pin;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+ GPIO_InitStructure(dev->csn_gpio, &GPIO_InitStructure);
+#endif
+#endif
+#ifdef STM32F10X
+ gpio_config_t gpio;
+ gpio.speed = Speed_50MHz;
+ gpio.mode = Mode_AF_PP;
+ // SCK as output
+ gpio.pin = dev->sck_pin;
+ gpioInit(dev->sck_gpio, &gpio);
+ // MOSI as output
+ gpio.pin = dev->mosi_pin;
+ gpioInit(dev->mosi_gpio, &gpio);
+ // MISO as input
+ gpio.pin = dev->miso_pin;
+ gpio.mode = Mode_IN_FLOATING;
+ gpioInit(dev->miso_gpio, &gpio);
+#ifdef SOFTSPI_NSS_PIN
+ // NSS as output
+ gpio.pin = dev->nss_pin;
+ gpio.mode = Mode_Out_PP;
+ gpioInit(dev->csn_gpio, &gpio);
+#endif
+#endif
+}
+
+uint8_t softSpiTransferByte(const softSPIDevice_t *dev, uint8_t byte)
+{
+ for(int ii = 0; ii < 8; ++ii) {
+ if (byte & 0x80) {
+ GPIO_SetBits(dev->mosi_gpio, dev->mosi_pin);
+ } else {
+ GPIO_ResetBits(dev->mosi_gpio, dev->mosi_pin);
+ }
+ GPIO_SetBits(dev->sck_gpio, dev->sck_pin);
+ byte <<= 1;
+ if (GPIO_ReadInputDataBit(dev->miso_gpio, dev->miso_pin) == 1) {
+ byte |= 1;
+ }
+ GPIO_ResetBits(dev->sck_gpio, dev->sck_pin);
+ }
+ 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..47470b64d5
--- /dev/null
+++ b/src/main/drivers/bus_spi_soft.h
@@ -0,0 +1,53 @@
+/*
+ * 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
+
+typedef struct softSPIDevice_s {
+#ifdef SOFTSPI_NSS_PIN
+ GPIO_TypeDef* nss_gpio;
+ uint16_t nss_pin;
+#endif
+ GPIO_TypeDef* sck_gpio;
+ uint16_t sck_pin;
+ GPIO_TypeDef* mosi_gpio;
+ uint16_t mosi_pin;
+ GPIO_TypeDef* miso_gpio;
+ uint16_t miso_pin;
+} softSPIDevice_t;
+
+// Convenience macros to set pins high or low,
+// where 'dev' is an instance of SoftSPIDevice
+/*#define SOFTSPI_CSN_LO(dev) {GPIO_ResetBits(dev.csn_gpio, dev.nss_pin);}
+#define SOFTSPI_CSN_HI(dev) {GPIO_SetBits(dev.csn_gpio, dev.nss_pin);}
+
+#define SOFTSPI_SCK_LO(dev) {GPIO_ResetBits(dev.sck_gpio, dev.sck_pin);}
+#define SOFTSPI_SCK_HI(dev) {GPIO_SetBits(dev.sck_gpio, dev.sck_pin);}
+
+#define SOFTSPI_MISO_LO(dev) {GPIO_ResetBits(dev.miso_gpio, dev.miso_pin);}
+#define SOFTSPI_MISO_HI(dev) {GPIO_SetBits(dev.miso_gpio, dev.miso_pin);}
+
+#define SOFTSPI_MOSI_LO(dev) {GPIO_ResetBits(dev.mosi_gpio, dev.mosi_pin);}
+#define SOFTSPI_MOSI_HI(dev) {GPIO_SetBits(dev.mosi_gpio, dev.mosi_pin);}
+
+#define SOFTSPI_MISO_R(dev) {GPIO_ReadInputDataBit(dev.miso_gpio, dev.miso_pin);}
+*/
+
+void softSpiInit(const softSPIDevice_t *dev);
+uint8_t softSpiTransferByte(const softSPIDevice_t *dev, uint8_t data);
+//bool softSpiInit(SPI_TypeDef *instance);
+//uint8_t softSpiTransferByte(SPI_TypeDef *instance, uint8_t data);
diff --git a/src/main/drivers/rx_nrf24l01.c b/src/main/drivers/rx_nrf24l01.c
new file mode 100644
index 0000000000..c36cbc7fca
--- /dev/null
+++ b/src/main/drivers/rx_nrf24l01.c
@@ -0,0 +1,333 @@
+/*
+ * 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 "system.h"
+#include "gpio.h"
+#include "rx_nrf24l01.h"
+
+#ifdef UNIT_TEST
+
+#define NRF24_CE_HI() {}
+#define NRF24_CE_LO() {}
+void NRF24L01_SpiInit(void) {}
+
+#else
+
+#include "bus_spi.h"
+#include "bus_spi_soft.h"
+
+#define DISABLE_NRF24() {GPIO_SetBits(NRF24_CSN_GPIO, NRF24_CSN_PIN);}
+#define ENABLE_NRF24() {GPIO_ResetBits(NRF24_CSN_GPIO, NRF24_CSN_PIN);}
+#define NRF24_CE_HI() {GPIO_SetBits(NRF24_CE_GPIO, NRF24_CE_PIN);}
+#define NRF24_CE_LO() {GPIO_ResetBits(NRF24_CE_GPIO, NRF24_CE_PIN);}
+
+#ifdef USE_NRF24_SOFTSPI
+static const softSPIDevice_t softSPI = {
+ .sck_gpio = NRF24_SCK_GPIO,
+ .mosi_gpio = NRF24_MOSI_GPIO,
+ .miso_gpio = NRF24_MISO_GPIO,
+ .sck_pin = NRF24_SCK_PIN,
+ .mosi_pin = NRF24_MOSI_PIN,
+ .miso_pin = NRF24_MISO_PIN
+};
+#endif
+
+void NRF24L01_SpiInit(void)
+{
+ static bool hardwareInitialised = false;
+
+ if (hardwareInitialised) {
+ return;
+ }
+#ifdef USE_NRF24_SOFTSPI
+ softSpiInit(&softSPI);
+#endif
+ // Note: Nordic Semiconductor uses 'CSN', STM uses 'NSS'
+#ifdef STM32F303xC
+ GPIO_InitTypeDef GPIO_InitStructure;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+ GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+#ifndef USE_NRF24_SOFTSPI
+ // CSN as output
+ RCC_AHBPeriphClockCmd(NRF24_CSN_GPIO_CLK_PERIPHERAL, ENABLE);
+ GPIO_InitStructure.GPIO_Pin = NRF24_CSN_PIN;
+ GPIO_Init(NRF24_CSN_GPIO, &GPIO_InitStructure);
+#endif
+ // CE as OUTPUT
+ RCC_AHBPeriphClockCmd(NRF24_CE_GPIO_CLK_PERIPHERAL, ENABLE);
+ GPIO_InitStructure.GPIO_Pin = NRF24_CE_PIN;
+ GPIO_Init(NRF24_CE_GPIO, &GPIO_InitStructure);
+#endif // STM32F303xC
+
+#ifdef STM32F10X
+ gpio_config_t gpio;
+ gpio.speed = Speed_50MHz;
+ gpio.mode = Mode_Out_PP;
+#ifndef USE_NRF24_SOFTSPI
+ // CSN as output
+ RCC_APB2PeriphClockCmd(NRF24_CSN_GPIO_CLK_PERIPHERAL, ENABLE);
+ gpio.pin = NRF24_CSN_PIN;
+ gpioInit(NRF24_CSN_GPIO, &gpio);
+#endif
+ // CE as output
+ RCC_APB2PeriphClockCmd(NRF24_CE_GPIO_CLK_PERIPHERAL, ENABLE);
+ gpio.pin = NRF24_CE_PIN;
+ gpioInit(NRF24_CE_GPIO, &gpio);
+ // TODO: NRF24_IRQ as input
+#endif // STM32F10X
+
+ DISABLE_NRF24();
+ NRF24_CE_LO();
+
+#ifdef NRF24_SPI_INSTANCE
+ spiSetDivisor(NRF24_SPI_INSTANCE, SPI_9MHZ_CLOCK_DIVIDER);
+#endif
+ hardwareInitialised = true;
+}
+
+uint8_t nrf24TransferByte(uint8_t data)
+{
+#ifdef USE_NRF24_SOFTSPI
+ return softSpiTransferByte(&softSPI, data);
+#else
+ return spiTransferByte(NRF24_SPI_INSTANCE, data);
+#endif
+}
+
+// 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)
+{
+ ENABLE_NRF24();
+ nrf24TransferByte(W_REGISTER | (REGISTER_MASK & reg));
+ nrf24TransferByte(data);
+ DISABLE_NRF24();
+ return true;
+}
+
+uint8_t NRF24L01_WriteRegisterMulti(uint8_t reg, const uint8_t *data, uint8_t length)
+{
+ ENABLE_NRF24();
+ const uint8_t ret = nrf24TransferByte(W_REGISTER | ( REGISTER_MASK & reg));
+ for (uint8_t i = 0; i < length; i++) {
+ nrf24TransferByte(data[i]);
+ }
+ DISABLE_NRF24();
+ return ret;
+}
+
+/*
+ * 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)
+{
+ ENABLE_NRF24();
+ const uint8_t ret = nrf24TransferByte(W_TX_PAYLOAD);
+ for (uint8_t i = 0; i < length; i++) {
+ nrf24TransferByte(data[i]);
+ }
+ DISABLE_NRF24();
+ return ret;
+}
+
+uint8_t NRF24L01_ReadReg(uint8_t reg)
+{
+ ENABLE_NRF24();
+ nrf24TransferByte(R_REGISTER | (REGISTER_MASK & reg));
+ const uint8_t ret = nrf24TransferByte(NOP);
+ DISABLE_NRF24();
+ return ret;
+}
+
+uint8_t NRF24L01_ReadRegisterMulti(uint8_t reg, uint8_t *data, uint8_t length)
+{
+ ENABLE_NRF24();
+ const uint8_t ret = nrf24TransferByte(R_REGISTER | (REGISTER_MASK & reg));
+ for (uint8_t i = 0; i < length; i++) {
+ data[i] = nrf24TransferByte(NOP);
+ }
+ DISABLE_NRF24();
+ return ret;
+}
+
+/*
+ * Read a packet from the nRF24L01 RX FIFO.
+ */
+uint8_t NRF24L01_ReadPayload(uint8_t *data, uint8_t length)
+{
+ ENABLE_NRF24();
+ const uint8_t ret = nrf24TransferByte(R_RX_PAYLOAD);
+ for (uint8_t i = 0; i < length; i++) {
+ data[i] = nrf24TransferByte(NOP);
+ }
+ DISABLE_NRF24();
+ return ret;
+}
+
+/*
+ * Empty the transmit FIFO buffer.
+ */
+void NRF24L01_FlushTx()
+{
+ ENABLE_NRF24();
+ nrf24TransferByte(FLUSH_TX);
+ DISABLE_NRF24();
+}
+
+/*
+ * Empty the receive FIFO buffer.
+ */
+void NRF24L01_FlushRx()
+{
+ ENABLE_NRF24();
+ nrf24TransferByte(FLUSH_RX);
+ DISABLE_NRF24();
+}
+
+#endif // UNIT_TEST
+
+// 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
+}
+
+/*
+ * 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
+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_NRF24();
+ nrf24TransferByte(R_REGISTER | (REGISTER_MASK & NRF24L01_17_FIFO_STATUS));
+ const uint8_t fifoStatus = nrf24TransferByte(NOP);
+ if ((fifoStatus & BV(NRF24L01_17_FIFO_STATUS_RX_EMPTY)) == 0) {
+ ret = true;
+ nrf24TransferByte(R_RX_PAYLOAD);
+ for (uint8_t i = 0; i < length; i++) {
+ data[i] = nrf24TransferByte(NOP);
+ }
+ }
+ DISABLE_NRF24();
+ return ret;
+}
+#endif
+#endif
diff --git a/src/main/drivers/rx_nrf24l01.h b/src/main/drivers/rx_nrf24l01.h
new file mode 100644
index 0000000000..88a4422ac8
--- /dev/null
+++ b/src/main/drivers/rx_nrf24l01.h
@@ -0,0 +1,140 @@
+/*
+ * 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
+
+#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_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_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_500uS = 0x10,
+
+ 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,
+};
+
+void NRF24L01_SpiInit(void);
+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 len);
+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 len);
+
+void NRF24L01_FlushTx(void);
+void NRF24L01_FlushRx(void);
+
+
+// Utility functions
+
+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/sound_beeper.h b/src/main/drivers/sound_beeper.h
index 76a7887c77..7f92d81343 100644
--- a/src/main/drivers/sound_beeper.h
+++ b/src/main/drivers/sound_beeper.h
@@ -18,9 +18,9 @@
#pragma once
#ifdef BEEPER
-#define BEEP_TOGGLE digitalToggle(BEEP_GPIO, BEEP_PIN)
-#define BEEP_OFF systemBeep(false)
-#define BEEP_ON systemBeep(true)
+#define BEEP_TOGGLE {digitalToggle(BEEP_GPIO, BEEP_PIN);}
+#define BEEP_OFF {systemBeep(false);}
+#define BEEP_ON {systemBeep(true);}
#else
#define BEEP_TOGGLE {}
#define BEEP_OFF {}
diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index 98b443ae54..bfdcd4e7d4 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -20,6 +20,8 @@
#include "platform.h"
+#include "build_config.h"
+
#include "debug.h"
#include "common/axis.h"
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index ef393b2778..06fe127ba8 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -273,7 +273,7 @@ const mixer_t mixers[] = {
{ 2, true, NULL, true }, // MIXER_CUSTOM_AIRPLANE
{ 3, true, NULL, true }, // MIXER_CUSTOM_TRI
};
-#endif //USE_QUAD_MIXER_ONLY
+#endif // USE_QUAD_MIXER_ONLY
#ifdef USE_SERVOS
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 04c7e22fae..3a9e524e55 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -185,7 +185,7 @@ 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", "ONESHOT125",
- "BLACKBOX", "CHANNEL_FORWARDING", NULL
+ "BLACKBOX", "CHANNEL_FORWARDING", "RX_NRF24", NULL
};
// sync this with rxFailsafeChannelMode_e
@@ -369,6 +369,7 @@ static const char * const lookupTableBlackboxDevice[] = {
"SERIAL", "SPIFLASH"
};
+#ifdef SERIAL_RX
static const char * const lookupTableSerialRX[] = {
"SPEK1024",
"SPEK2048",
@@ -379,6 +380,18 @@ static const char * const lookupTableSerialRX[] = {
"XB-B-RJ01",
"IBUS"
};
+#endif
+
+#ifdef USE_RX_NRF24
+static const char * const lookupTableNRF24RX[] = {
+ "V202_250K",
+ "V202_1M",
+ "SYMA_X",
+ "SYMA_X5C",
+ "CX10",
+ "CX10A"
+};
+#endif
static const char * const lookupTableGyroLpf[] = {
"256HZ",
@@ -423,7 +436,12 @@ typedef enum {
TABLE_CURRENT_SENSOR,
TABLE_GIMBAL_MODE,
TABLE_PID_CONTROLLER,
+#ifdef SERIAL_RX
TABLE_SERIAL_RX,
+#endif
+#ifdef USE_RX_NRF24
+ TABLE_NRF24_RX,
+#endif
TABLE_GYRO_LPF,
TABLE_FAILSAFE_PROCEDURE,
#ifdef NAV
@@ -447,8 +465,13 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTableCurrentSensor, sizeof(lookupTableCurrentSensor) / sizeof(char *) },
{ lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) },
{ lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) },
+#ifdef SERIAL_RX
{ lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) },
- { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) },
+#endif
+#ifdef USE_RX_NRF24
+ { lookupTableNRF24RX, sizeof(lookupTableNRF24RX) / sizeof(char *) },
+#endif
+ { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) },
{ lookupTableFailsafeProcedure, sizeof(lookupTableFailsafeProcedure) / sizeof(char *) },
#ifdef NAV
{ lookupTableNavControlMode, sizeof(lookupTableNavControlMode) / sizeof(char *) },
@@ -622,7 +645,12 @@ const clivalue_t valueTable[] = {
{ "nav_fw_loiter_radius", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_loiter_radius, .config.minmax = { 0, 10000 }, 0 },
#endif
+#ifdef SERIAL_RX
{ "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.serialrx_provider, .config.lookup = { TABLE_SERIAL_RX }, 0 },
+#endif
+#ifdef USE_RX_NRF24
+ { "nrf24rx_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.nrf24rx_protocol, .config.lookup = { TABLE_NRF24_RX }, 0 },
+#endif
{ "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.spektrum_sat_bind, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, 0 },
{ "telemetry_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.telemetryConfig.telemetry_switch, .config.lookup = { TABLE_OFF_ON }, 0 },
@@ -1697,7 +1725,6 @@ static void cliDump(char *cmdline)
cliWrite(' ');
cliPrintf("%s\r\n", ftoa(yaw, buf));
}
-
#ifdef USE_SERVOS
// print custom servo mixer if exists
cliPrintf("smix reset\r\n");
@@ -1719,8 +1746,7 @@ static void cliDump(char *cmdline)
);
}
#endif // USE_SERVOS
-
-#endif
+#endif // USE_QUAD_MIXER_ONLY
cliPrint("\r\n\r\n# feature\r\n");
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 6bdfc07821..328257f911 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -961,8 +961,9 @@ static bool processOutCommand(uint8_t cmdMSP)
break;
case MSP_RX_CONFIG:
- headSerialReply(12);
+ headSerialReply(13);
serialize8(masterConfig.rxConfig.serialrx_provider);
+ serialize8(masterConfig.rxConfig.nrf24rx_protocol);
serialize16(masterConfig.rxConfig.maxcheck);
serialize16(masterConfig.rxConfig.midrc);
serialize16(masterConfig.rxConfig.mincheck);
@@ -1441,6 +1442,7 @@ static bool processInCommand(void)
case MSP_SET_RX_CONFIG:
masterConfig.rxConfig.serialrx_provider = read8();
+ masterConfig.rxConfig.nrf24rx_protocol = read8();
masterConfig.rxConfig.maxcheck = read16();
masterConfig.rxConfig.midrc = read16();
masterConfig.rxConfig.mincheck = read16();
diff --git a/src/main/rx/nrf24.c b/src/main/rx/nrf24.c
new file mode 100644
index 0000000000..d4294be4e2
--- /dev/null
+++ b/src/main/rx/nrf24.c
@@ -0,0 +1,122 @@
+/*
+ * 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 "build_config.h"
+
+#ifdef USE_RX_NRF24
+#include "drivers/rx_nrf24l01.h"
+#include "rx/rx.h"
+#include "rx/nrf24.h"
+#include "rx/nrf24_cx10.h"
+#include "rx/nrf24_syma.h"
+#include "rx/nrf24_v202.h"
+
+
+uint16_t nrf24RcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+STATIC_UNIT_TESTED uint8_t nrf24Payload[NRF24L01_MAX_PAYLOAD_SIZE];
+STATIC_UNIT_TESTED uint8_t nrf24NewPacketAvailable; // set true when a new packet is received
+
+typedef void (*protocolInitPtr)(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+typedef nrf24_received_t (*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 rxNrf24ReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel)
+{
+ if (channel >= rxRuntimeConfig->channelCount) {
+ return 0;
+ }
+ if (nrf24NewPacketAvailable) {
+ protocolSetRcDataFromPayload(nrf24RcData, nrf24Payload);
+ nrf24NewPacketAvailable = false;
+ }
+ return nrf24RcData[channel];
+}
+
+STATIC_UNIT_TESTED bool rxNrf24SetProtocol(nrf24_protocol_t protocol)
+{
+ switch (protocol) {
+#ifdef USE_RX_V202
+ case NRF24RX_V202_250K:
+ case NRF24RX_V202_1M:
+ protocolInit = v202Init;
+ protocolDataReceived = v202DataReceived;
+ protocolSetRcDataFromPayload = v202SetRcDataFromPayload;
+ break;
+#endif
+#ifdef USE_RX_SYMA
+ case NRF24RX_SYMA_X:
+ case NRF24RX_SYMA_X5C:
+ protocolInit = symaInit;
+ protocolDataReceived = symaDataReceived;
+ protocolSetRcDataFromPayload = symaSetRcDataFromPayload;
+ break;
+#endif
+#ifdef USE_RX_CX10
+ case NRF24RX_CX10:
+ case NRF24RX_CX10A:
+ protocolInit = cx10Init;
+ protocolDataReceived = cx10DataReceived;
+ protocolSetRcDataFromPayload = cx10SetRcDataFromPayload;
+ break;
+#endif
+ default:
+ return false;
+ break;
+ }
+ return true;
+}
+
+/*
+ * Returns true if the NRF24L01 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 rxNrf24DataReceived(void)
+{
+ if (protocolDataReceived(nrf24Payload) == NRF24_RECEIVED_DATA) {
+ nrf24NewPacketAvailable = true;
+ return true;
+ }
+ return false;
+}
+
+/*
+ * Set and initialize the NRF24 protocol
+ */
+bool rxNrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
+{
+ bool ret = false;
+ NRF24L01_SpiInit();
+ if (rxNrf24SetProtocol(rxConfig->nrf24rx_protocol)) {
+ protocolInit(rxConfig, rxRuntimeConfig);
+ ret = true;
+ }
+ nrf24NewPacketAvailable = false;
+ if (callback) {
+ *callback = rxNrf24ReadRawRC;
+ }
+ return ret;
+}
+#endif
diff --git a/src/main/rx/nrf24.h b/src/main/rx/nrf24.h
new file mode 100644
index 0000000000..930a955178
--- /dev/null
+++ b/src/main/rx/nrf24.h
@@ -0,0 +1,65 @@
+/*
+ * 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_PROTOCOL_COUNT
+} nrf24_protocol_t;
+
+typedef enum {
+ NRF24_RECEIVED_NONE = 0,
+ NRF24_RECEIVED_BIND,
+ NRF24_RECEIVED_DATA
+} nrf24_received_t;
+
+// RC channels in AETR order
+typedef enum {
+ NRF24_ROLL = 0,
+ NRF24_PITCH,
+ NRF24_THROTTLE,
+ NRF24_YAW,
+ NRF24_AUX1,
+ NRF24_AUX2,
+ NRF24_AUX3,
+ NRF24_AUX4,
+ NRF24_AUX5,
+ NRF24_AUX6,
+ NRF24_AUX7,
+ NRF24_AUX8,
+ NRF24_AUX9,
+ NRF24_AUX10,
+ NRF24_AUX11,
+ NRF24_AUX12,
+ NRF24_AUX13,
+ NRF24_AUX14
+} nrf24_AETR_t;
+
+
+bool rxNrf24DataReceived(void);
+bool rxNrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
diff --git a/src/main/rx/nrf24_cx10.c b/src/main/rx/nrf24_cx10.c
new file mode 100644
index 0000000000..070fc6d3d1
--- /dev/null
+++ b/src/main/rx/nrf24_cx10.c
@@ -0,0 +1,348 @@
+/*
+ * 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_config.h"
+
+#ifdef USE_RX_CX10
+
+#include "drivers/rx_nrf24l01.h"
+#include "drivers/system.h"
+
+#include "rx/nrf24.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 CX10_RC_CHANNEL_COUNT 9
+
+#define CX10_PROTOCOL_PAYLOAD_SIZE 15
+#define CX10A_PROTOCOL_PAYLOAD_SIZE 19
+
+#define CX10_RF_BIND_CHANNEL 0x02
+
+#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
+
+// XN297 emulation layer
+STATIC_UNIT_TESTED uint8_t XN297_WritePayload(uint8_t* data, int len);
+STATIC_UNIT_TESTED void XN297_UnscramblePayload(uint8_t* data, int len);
+
+static nrf24_protocol_t cx10Protocol;
+
+typedef enum {
+ STATE_BIND = 0,
+ STATE_ACK,
+ STATE_DATA
+} protocol_state_t;
+
+STATIC_UNIT_TESTED protocol_state_t protocolState;
+
+
+
+#define ACK_TO_SEND_COUNT 8
+static uint8_t payloadSize;
+
+#define RX_TX_ADDR_LEN 5
+//STATIC_UNIT_TESTED uint8_t rxTxAddr[RX_TX_ADDR_LEN] = {0xcc, 0xcc, 0xcc, 0xcc, 0xcc};
+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];
+
+STATIC_UNIT_TESTED uint8_t rfChannelIndex = 0;
+#define RF_CHANNEL_COUNT 4
+STATIC_UNIT_TESTED uint8_t rfChannels[RF_CHANNEL_COUNT]; // channels are set using txId from bind packet
+
+static uint32_t timeOfLastHop;
+static const uint32_t hopTimeout = 5000; // 5ms
+
+void cx10Nrf24Init(nrf24_protocol_t protocol)
+{
+ cx10Protocol = protocol;
+ protocolState = STATE_BIND;
+ payloadSize = (protocol == NRF24RX_CX10) ? CX10_PROTOCOL_PAYLOAD_SIZE : CX10A_PROTOCOL_PAYLOAD_SIZE;
+
+ NRF24L01_Initialize(0); // sets PWR_UP, no CRC
+
+ NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0);
+ NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, BV(NRF24L01_02_EN_RXADDR_ERX_P0)); // Enable data pipe 0 only
+ NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, NRF24L01_03_SETUP_AW_5BYTES); // 5-byte RX/TX address
+ 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_08_OBSERVE_TX, 0x00);
+ NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, payloadSize + 2); // payload + 2 bytes CRC
+
+ NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x00); // Disable dynamic payload length on all pipes
+
+ NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
+}
+
+/*
+ * Returns true if it is a bind packet.
+ */
+STATIC_UNIT_TESTED bool checkBindPacket(const uint8_t *packet)
+{
+ const bool bindPacket = (packet[0] == 0xaa);
+ 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 convertToPwmUnsigned(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 cx10SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
+{
+ const uint8_t offset = (cx10Protocol == NRF24RX_CX10) ? 0 : 4;
+ rcData[NRF24_ROLL] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - convertToPwmUnsigned(&payload[5 + offset]); // aileron
+ rcData[NRF24_PITCH] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - convertToPwmUnsigned(&payload[7 + offset]); // elevator
+ rcData[NRF24_THROTTLE] = convertToPwmUnsigned(&payload[9 + offset]); // throttle
+ rcData[NRF24_YAW] = convertToPwmUnsigned(&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 == 0) {
+ rcData[NRF24_AUX1] = PWM_RANGE_MIN;
+ } else if (rate == 1) {
+ rcData[NRF24_AUX1] = PWM_RANGE_MIDDLE;
+ } else {
+ rcData[NRF24_AUX1] = PWM_RANGE_MAX;
+ }
+ // flip flag is in YAW byte
+ rcData[NRF24_AUX2] = payload[12 + offset] & FLAG_FLIP ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ const uint8_t flags2 = payload[14 + offset];
+ rcData[NRF24_AUX3] = flags2 & FLAG_PICTURE ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[NRF24_AUX4] = flags2 & FLAG_VIDEO ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[NRF24_AUX5] = flags1 & FLAG_HEADLESS ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+}
+
+static void hopToNextChannel(void)
+{
+ ++rfChannelIndex;
+ if (rfChannelIndex >= RF_CHANNEL_COUNT) {
+ rfChannelIndex = 0;
+ }
+ NRF24L01_SetChannel(rfChannels[rfChannelIndex]);
+}
+
+// The hopping channels are determined by the txId
+STATIC_UNIT_TESTED void setHoppingChannels(const uint8_t* txId)
+{
+ rfChannelIndex = 0;
+ rfChannels[0] = 0x03 + (txId[0] & 0x0F);
+ rfChannels[1] = 0x16 + (txId[0] >> 4);
+ rfChannels[2] = 0x2D + (txId[1] & 0x0F);
+ rfChannels[3] = 0x40 + (txId[1] >> 4);
+}
+
+/*
+ * This is called periodically by the scheduler.
+ * Returns NRF24_RECEIVED_DATA if a data packet was received.
+ */
+nrf24_received_t cx10DataReceived(uint8_t *payload)
+{
+ static uint8_t ackCount;
+ nrf24_received_t ret = NRF24_RECEIVED_NONE;
+ int totalDelayUs;
+ uint32_t timeNowUs;
+
+ switch (protocolState) {
+ case STATE_BIND:
+ if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize + 2)) {
+ XN297_UnscramblePayload(payload, payloadSize + 2);
+ const bool bindPacket = checkBindPacket(payload);
+ if (bindPacket) {
+ // set the hopping channels as determined by the txId received in the bind packet
+ setHoppingChannels(txId);
+ ret = NRF24_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);
+ 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 (uint8_t ii = 0; ii < RF_CHANNEL_COUNT; ++ii) {
+ NRF24L01_SetChannel(rfChannels[ii]);
+ XN297_WritePayload(payload, payloadSize);
+ 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(rfChannels[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 (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize + 2)) {
+ XN297_UnscramblePayload(payload, payloadSize + 2);
+ hopToNextChannel();
+ timeOfLastHop = timeNowUs;
+ ret = NRF24_RECEIVED_DATA;
+ }
+ if (timeNowUs > timeOfLastHop + hopTimeout) {
+ hopToNextChannel();
+ timeOfLastHop = timeNowUs;
+ }
+ }
+ return ret;
+}
+
+void cx10Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ rxRuntimeConfig->channelCount = CX10_RC_CHANNEL_COUNT;
+ cx10Nrf24Init((nrf24_protocol_t)rxConfig->nrf24rx_protocol);
+}
+
+
+// XN297 emulation layer
+
+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 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;
+}
+
+static uint16_t crc16_update(uint16_t crc, unsigned char a)
+{
+ static const uint16_t crcPolynomial = 0x1021;
+ crc ^= a << 8;
+ for (int ii = 0; ii < 8; ++ii) {
+ if (crc & 0x8000) {
+ crc = (crc << 1) ^ crcPolynomial;
+ } else {
+ crc = crc << 1;
+ }
+ }
+ return crc;
+}
+
+STATIC_UNIT_TESTED uint8_t XN297_WritePayload(uint8_t* data, int len)
+{
+ 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_update(crc, packet[ii]);
+ }
+ for (int ii = 0; ii < len; ++ii) {
+ // bit-reverse bytes in packet
+ const uint8_t bOut = bitReverse(data[ii]);
+ packet[ii + RX_TX_ADDR_LEN] = bOut ^ xn297_data_scramble[ii];
+ crc = crc16_update(crc, packet[ii + RX_TX_ADDR_LEN]);
+ }
+ const uint16_t crcXor = (len == CX10_PROTOCOL_PAYLOAD_SIZE) ? 0x9BA7 : 0x61B1;
+ crc ^= crcXor;
+ 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);
+}
+
+STATIC_UNIT_TESTED void XN297_UnscramblePayload(uint8_t* data, int len)
+{
+ for (uint8_t ii = 0; ii < len; ++ii) {
+ data[ii] = bitReverse(data[ii] ^ xn297_data_scramble[ii]);
+ }
+}
+
+// End of XN297 emulation
+
+#endif
+
diff --git a/src/main/rx/nrf24_cx10.h b/src/main/rx/nrf24_cx10.h
new file mode 100644
index 0000000000..55c5614d90
--- /dev/null
+++ b/src/main/rx/nrf24_cx10.h
@@ -0,0 +1,26 @@
+/*
+ * 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
+
+void cx10Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+void cx10SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
+nrf24_received_t cx10DataReceived(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..0ac8bc3216
--- /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
+#include "build_config.h"
+
+#ifdef USE_RX_SYMA
+
+#include "drivers/rx_nrf24l01.h"
+#include "drivers/system.h"
+
+#include "rx/nrf24.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
+ *
+ * SymaX5 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 SYMA_RC_CHANNEL_COUNT 9
+
+#define SYMA_X_PROTOCOL_PAYLOAD_SIZE 10
+#define SYMA_X5C_PROTOCOL_PAYLOAD_SIZE 16
+
+#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
+
+#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 nrf24_protocol_t 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
+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
+static int packetCount = 0;
+STATIC_UNIT_TESTED uint8_t rfChannelIndex = 0;
+STATIC_UNIT_TESTED uint8_t rfChannelCount = SYMA_X_RF_CHANNEL_COUNT;
+// set rfChannels to SymaX bind channels, reserve enough space for SymaX5C channels
+STATIC_UNIT_TESTED uint8_t rfChannels[SYMA_X5C_RF_BIND_CHANNEL_COUNT] = {0x4b, 0x30, 0x40, 0x20};
+STATIC_UNIT_TESTED const uint8_t rfChannelsX5C[SYMA_X5C_RF_CHANNEL_COUNT] = {0x1d, 0x2f, 0x26, 0x3d, 0x15, 0x2b, 0x25, 0x24, 0x27, 0x2c, 0x1c, 0x3e, 0x39, 0x2d, 0x22};
+
+static uint32_t timeOfLastHop;
+static uint32_t hopTimeout = 10000; // 10ms
+
+void symaNrf24Init(nrf24_protocol_t 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_WriteReg(NRF24L01_01_EN_AA, 0); // 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
+ 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;
+ rfChannelCount = SYMA_X5C_RF_CHANNEL_COUNT;
+ memcpy(rfChannels, rfChannelsX5C, SYMA_X5C_RF_CHANNEL_COUNT);
+ }
+ NRF24L01_SetChannel(rfChannels[0]);
+
+ NRF24L01_WriteReg(NRF24L01_08_OBSERVE_TX, 0x00);
+ NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x00); // Disable dynamic payload length on all pipes
+
+ NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, payloadSize);
+
+ NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
+}
+
+STATIC_UNIT_TESTED uint16_t convertToPwmUnsigned(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 convertToPwmSigned(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);
+}
+
+STATIC_UNIT_TESTED bool checkBindPacket(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;
+}
+
+void symaSetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet)
+{
+ rcData[NRF24_THROTTLE] = convertToPwmUnsigned(packet[0]); // throttle
+ rcData[NRF24_ROLL] = convertToPwmSigned(packet[3]); // aileron
+ if (symaProtocol == NRF24RX_SYMA_X) {
+ rcData[NRF24_PITCH] = convertToPwmSigned(packet[1]); // elevator
+ rcData[NRF24_YAW] = convertToPwmSigned(packet[2]); // rudder
+ const uint8_t rate = (packet[5] & 0xc0) >> 6; // takes values 0, 1, 2
+ if (rate == 0) {
+ rcData[NRF24_AUX1] = PWM_RANGE_MIN;
+ } else if (rate == 1) {
+ rcData[NRF24_AUX1] = PWM_RANGE_MIDDLE;
+ } else {
+ rcData[NRF24_AUX1] = PWM_RANGE_MAX;
+ }
+ rcData[NRF24_AUX2] = packet[6] & FLAG_FLIP ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[NRF24_AUX3] = packet[4] & FLAG_PICTURE ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[NRF24_AUX4] = packet[4] & FLAG_VIDEO ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[NRF24_AUX5] = packet[14] & FLAG_HEADLESS ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ } else {
+ rcData[NRF24_PITCH] = convertToPwmSigned(packet[2]); // elevator
+ rcData[NRF24_YAW] = convertToPwmSigned(packet[1]); // rudder
+ const uint8_t flags = packet[14];
+ rcData[NRF24_AUX1] = flags & FLAG_RATE_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[NRF24_AUX2] = flags & FLAG_FLIP_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[NRF24_AUX3] = flags & FLAG_PICTURE_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[NRF24_AUX4] = flags & FLAG_VIDEO_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ }
+}
+
+static void hopToNextChannel(void)
+{
+ // hop channel every second packet
+ ++packetCount;
+ if ((packetCount & 0x01) == 0) {
+ ++rfChannelIndex;
+ if (rfChannelIndex >= rfChannelCount) {
+ rfChannelIndex = 0;
+ }
+ }
+ NRF24L01_SetChannel(rfChannels[rfChannelIndex]);
+}
+
+// The SymaX hopping channels are determined by the low bits of rxTxAddress
+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 *)rfChannels;
+ 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 NRF24_RECEIVED_DATA if a data packet was received.
+ */
+nrf24_received_t symaDataReceived(uint8_t *payload)
+{
+ nrf24_received_t ret = NRF24_RECEIVED_NONE;
+ switch (protocolState) {
+ case STATE_BIND:
+ if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) {
+ const bool bindPacket = checkBindPacket(payload);
+ if (bindPacket) {
+ ret = NRF24_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;
+ rfChannelIndex = 0;
+ NRF24L01_SetChannel(rfChannels[0]);
+ }
+ }
+ break;
+ case STATE_DATA:
+ // read the payload, processing of payload is deferred
+ if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) {
+ hopToNextChannel();
+ timeOfLastHop = micros();
+ ret = NRF24_RECEIVED_DATA;
+ }
+ if (micros() > timeOfLastHop + hopTimeout) {
+ hopToNextChannel();
+ timeOfLastHop = micros();
+ }
+ break;
+ }
+ return ret;
+}
+
+void symaInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ rxRuntimeConfig->channelCount = SYMA_RC_CHANNEL_COUNT;
+ symaNrf24Init((nrf24_protocol_t)rxConfig->nrf24rx_protocol);
+}
+#endif
+
diff --git a/src/main/rx/nrf24_syma.h b/src/main/rx/nrf24_syma.h
new file mode 100644
index 0000000000..126b0dcc0b
--- /dev/null
+++ b/src/main/rx/nrf24_syma.h
@@ -0,0 +1,26 @@
+/*
+ * 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
+
+void symaInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+void symaSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
+nrf24_received_t symaDataReceived(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..78aa023891
--- /dev/null
+++ b/src/main/rx/nrf24_v202.c
@@ -0,0 +1,271 @@
+/*
+ * 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 .
+ */
+
+/*
+ * v202 Protocol
+ * No auto acknowledgment
+ * Payload size is 16, static
+ * Data rate is 250Kbps or 1Mbps depending on variant
+ * Bind Phase
+ * uses address {0x66, 0x88, 0x68, 0x68, 0x68}
+ * hops between 16 channels
+ * Data Phase
+ * uses same address as bind phase
+ * hops between 16 channels generated from address received in bind packets
+ */
+
+// this file is copied with modifications from bradwii for jd385
+// see https://github.com/hackocopter/bradwii-jd385
+
+#include
+#include
+#include
+
+#include
+#include "build_config.h"
+
+#ifdef USE_RX_V202
+
+#include "drivers/rx_nrf24l01.h"
+#include "drivers/system.h"
+
+#include "rx/nrf24.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 nrf24RcData[];
+
+const unsigned char v2x2_channelindex[] = {NRF24_THROTTLE,NRF24_YAW,NRF24_PITCH,NRF24_ROLL,
+ NRF24_AUX1,NRF24_AUX2,NRF24_AUX3,NRF24_AUX4,NRF24_AUX5,NRF24_AUX6,NRF24_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;
+}
+
+void v202Nrf24Init(nrf24_protocol_t 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 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 nrf24_received_t decode_packet(uint8_t *packet)
+{
+ if(bind_phase != PHASE_BOUND) {
+ decode_bind_packet(packet);
+ return NRF24_RECEIVED_BIND;
+ }
+ // Decode packet
+ if ((packet[14] & V2X2_FLAG_BIND) == V2X2_FLAG_BIND) {
+ return NRF24_RECEIVED_BIND;
+ }
+ if (packet[7] != txid[0] ||
+ packet[8] != txid[1] ||
+ packet[9] != txid[2]) {
+ return NRF24_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
+ nrf24RcData[v2x2_channelindex[0]] = ((uint16_t)packet[0]) * 1000 / 255 + 1000;
+ for (int i = 1; i < 4; ++i) {
+ uint8_t a = packet[i];
+ nrf24RcData[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) {
+ nrf24RcData[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) {
+ nrf24RcData[v2x2_channelindex[i]] = (packet[10] & flags10[i-8]) ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ }
+ packet_timer = micros();
+ return NRF24_RECEIVED_DATA;
+}
+
+void v202SetRcDataFromPayload(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.
+}
+
+nrf24_received_t 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 NRF24_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 NRF24_RECEIVED_DATA if a data packet was received.
+ */
+nrf24_received_t v202DataReceived(uint8_t *packet)
+{
+ return readrx(packet);
+}
+
+void v202Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ rxRuntimeConfig->channelCount = V2X2_RC_CHANNEL_COUNT;
+ v202Nrf24Init((nrf24_protocol_t)rxConfig->nrf24rx_protocol);
+}
+
+#endif
diff --git a/src/main/rx/nrf24_v202.h b/src/main/rx/nrf24_v202.h
new file mode 100644
index 0000000000..276b91aaed
--- /dev/null
+++ b/src/main/rx/nrf24_v202.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
+
+void v202Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+nrf24_received_t v202DataReceived(uint8_t *payload);
+void v202SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index c095977819..54b1798faa 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -49,6 +49,7 @@
#include "rx/msp.h"
#include "rx/xbus.h"
#include "rx/ibus.h"
+#include "rx/nrf24.h"
#include "rx/rx.h"
@@ -183,11 +184,23 @@ void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationCondi
}
#endif
+#ifdef USE_RX_NRF24
+ if (feature(FEATURE_RX_NRF24)) {
+ rxRefreshRate = 10000;
+ const bool enabled = rxNrf24Init(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
+ if (!enabled) {
+ featureClear(FEATURE_RX_NRF24);
+ rcReadRawFunc = nullReadRawRC;
+ }
+ }
+#endif
+
+#ifndef SKIP_RX_PWM
if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) {
rxRefreshRate = 20000;
rxPwmInit(&rxRuntimeConfig, &rcReadRawFunc);
}
-
+#endif
rxRuntimeConfig.auxChannelCount = rxRuntimeConfig.channelCount - STICK_CHANNEL_COUNT;
}
@@ -332,6 +345,17 @@ void updateRx(uint32_t currentTime)
}
#endif
+#ifdef USE_RX_NRF24
+ if (feature(FEATURE_RX_NRF24)) {
+ rxDataReceived = rxNrf24DataReceived();
+ if (rxDataReceived) {
+ rxSignalReceived = true;
+ rxIsInFailsafeMode = false;
+ needRxSignalBefore = currentTime + DELAY_10_HZ;
+ }
+ }
+#endif
+
#ifndef SKIP_RX_MSP
if (feature(FEATURE_RX_MSP)) {
rxDataReceived = rxMspFrameComplete();
@@ -344,6 +368,7 @@ void updateRx(uint32_t currentTime)
}
#endif
+#ifndef SKIP_RX_PWM
if (feature(FEATURE_RX_PPM)) {
if (isPPMDataBeingReceived()) {
rxSignalReceivedNotDataDriven = true;
@@ -360,7 +385,7 @@ void updateRx(uint32_t currentTime)
needRxSignalBefore = currentTime + DELAY_10_HZ;
}
}
-
+#endif
}
bool shouldProcessRx(uint32_t currentTime)
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 570022c928..1f15dc930b 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -111,6 +111,7 @@ typedef struct rxChannelRangeConfiguration_s {
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 nrf24rx_protocol; // type of nrf24 protocol (0 = v202 250kbps). Must be enabled by FEATURE_RX_NRF24 first.
uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers
uint8_t rssi_channel;
uint8_t rssi_scale;
diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h
index c7f8d23dc6..a8da772185 100644
--- a/src/main/target/CJMCU/target.h
+++ b/src/main/target/CJMCU/target.h
@@ -20,6 +20,8 @@
#define TARGET_BOARD_IDENTIFIER "CJM1" // CJMCU
#define USE_HARDWARE_REVISION_DETECTION
+#define BRUSHED_MOTORS
+
#define LED0
#define LED0_GPIO GPIOC
#define LED0_PIN Pin_14 // PC14 (LED)
@@ -35,29 +37,49 @@
#define LED2_PIN Pin_15 // PC15 (LED)
#define LED2_PERIPHERAL RCC_APB2Periph_GPIOC
-
-#define ACC
-#define USE_ACC_MPU6050
+#undef BEEPER
#define GYRO
#define USE_GYRO_MPU6050
+#define ACC
+#define USE_ACC_MPU6050
+
#define MAG
#define USE_MAG_HMC5883
-#define BRUSHED_MOTORS
-
#define USE_USART1
#define USE_USART2
-
#define SERIAL_PORT_COUNT 2
#define USE_I2C
#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
+
+#define NRF24_SPI_INSTANCE SPI1
+#define USE_NRF24_SPI1
+
+// Nordic Semiconductor uses 'CSN', STM uses 'NSS'
+#define NRF24_CE_GPIO GPIOA
+#define NRF24_CE_PIN GPIO_Pin_4
+#define NRF24_CE_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA
+#define NRF24_CSN_GPIO GPIOA
+#define NRF24_CSN_PIN GPIO_Pin_11
+#define NRF24_CSN_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA
+#define NRF24_IRQ_GPIO GPIOA
+#define NRF24_IRQ_PIN GPIO_Pin_8
+#define NRF24_IRQ_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA
+
+#define DEFAULT_RX_FEATURE FEATURE_RX_NRF24
+#define USE_RX_NRF24
+#define USE_RX_V202
+#define USE_RX_SYMA
+#define USE_RX_CX10
+#define NRF24_DEFAULT_PROTOCOL NRF24RX_SYMA_X5C
+
+#define DEFAULT_FEATURES FEATURE_MOTOR_STOP
#define SPEKTRUM_BIND
// USART2, PA3
@@ -71,15 +93,15 @@
#undef USE_SERVOS
#if (FLASH_SIZE <= 64)
+//#define SKIP_TASK_STATISTICS
+#define SKIP_RX_PWM
+#define SKIP_CLI_COMMAND_HELP
+#undef SERIAL_RX
#undef BLACKBOX
-#define SKIP_TASK_STATISTICS
#endif
-#undef BEEPER
#undef SKIP_RX_MSP
-//#undef USE_CLI
-
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4))
#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4)