From 551bbe1d1a0dc8c3caccc6109b7169234c8d1a53 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 14 Sep 2016 01:44:43 +0100 Subject: [PATCH] Initial commit of SPI receiver code. --- Makefile | 10 + src/main/common/maths.c | 14 ++ src/main/common/maths.h | 2 + src/main/config/config.h | 2 +- src/main/drivers/bus_spi.h | 1 + src/main/drivers/bus_spi_soft.c | 88 +++++++ src/main/drivers/bus_spi_soft.h | 36 +++ src/main/drivers/pwm_mapping.c | 7 +- src/main/drivers/resource.h | 3 + src/main/drivers/rx_nrf24l01.c | 243 ++++++++++++++++++ src/main/drivers/rx_nrf24l01.h | 201 +++++++++++++++ src/main/drivers/rx_spi.c | 166 +++++++++++++ src/main/drivers/rx_spi.h | 35 +++ src/main/drivers/rx_xn297.c | 90 +++++++ src/main/drivers/rx_xn297.h | 25 ++ src/main/io/serial_cli.c | 23 +- src/main/msp/msp_server_fc.c | 12 +- src/main/rx/nrf24_cx10.c | 301 +++++++++++++++++++++++ src/main/rx/nrf24_cx10.h | 28 +++ src/main/rx/nrf24_h8_3d.c | 283 +++++++++++++++++++++ src/main/rx/nrf24_h8_3d.h | 27 ++ src/main/rx/nrf24_inav.c | 424 ++++++++++++++++++++++++++++++++ src/main/rx/nrf24_inav.h | 28 +++ src/main/rx/nrf24_syma.c | 301 +++++++++++++++++++++++ src/main/rx/nrf24_syma.h | 28 +++ src/main/rx/nrf24_v202.c | 258 +++++++++++++++++++ src/main/rx/nrf24_v202.h | 27 ++ src/main/rx/rx.c | 25 ++ src/main/rx/rx.h | 6 +- src/main/rx/rx_spi.c | 139 +++++++++++ src/main/rx/rx_spi.h | 76 ++++++ src/main/target/CJMCU/target.h | 89 +++++-- src/main/target/CJMCU/target.mk | 5 +- 33 files changed, 2971 insertions(+), 32 deletions(-) create mode 100644 src/main/drivers/bus_spi_soft.c create mode 100644 src/main/drivers/bus_spi_soft.h create mode 100644 src/main/drivers/rx_nrf24l01.c create mode 100644 src/main/drivers/rx_nrf24l01.h create mode 100644 src/main/drivers/rx_spi.c create mode 100644 src/main/drivers/rx_spi.h create mode 100644 src/main/drivers/rx_xn297.c create mode 100644 src/main/drivers/rx_xn297.h create mode 100644 src/main/rx/nrf24_cx10.c create mode 100644 src/main/rx/nrf24_cx10.h create mode 100644 src/main/rx/nrf24_h8_3d.c create mode 100644 src/main/rx/nrf24_h8_3d.h create mode 100644 src/main/rx/nrf24_inav.c create mode 100644 src/main/rx/nrf24_inav.h create mode 100644 src/main/rx/nrf24_syma.c create mode 100644 src/main/rx/nrf24_syma.h create mode 100644 src/main/rx/nrf24_v202.c create mode 100644 src/main/rx/nrf24_v202.h create mode 100644 src/main/rx/rx_spi.c create mode 100644 src/main/rx/rx_spi.h diff --git a/Makefile b/Makefile index 3852953ff..c930eb33c 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 ee1b65ce6..1dd55519a 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 b47a19ae7..a672fba3a 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 d0e85f361..56f66645a 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 58c76c9d7..3d00aee52 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 000000000..6207f6591 --- /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 000000000..88b106e3f --- /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 06ea6438c..5e297b3bb 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 f9430d284..636f5db14 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 000000000..9f13b6362 --- /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 000000000..08d2c8afa --- /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 000000000..e7e8f70d4 --- /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 000000000..f083ca505 --- /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 000000000..eba751cad --- /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 000000000..167b2f380 --- /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 166a02827..fc5b812d8 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 919f30247..4ba993ff2 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 000000000..2434d1259 --- /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 000000000..8af154733 --- /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 000000000..9ffa1dcf4 --- /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 000000000..306259f9a --- /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 000000000..35b526f6a --- /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 000000000..f499f12a3 --- /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 000000000..b08cfaa27 --- /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 000000000..688bbb99e --- /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 000000000..be724469f --- /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 000000000..f3600ad7d --- /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 598a2f025..2b26282af 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 3a1d6c9d6..8d5ea135a 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 000000000..44b6c13fe --- /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 000000000..48b7f0a54 --- /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 beb4be965..7c2bb3738 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 72f2c64b0..8da31647c 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