From 86a5a3026716c092a9c1e13442ddb950f5330f48 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 2 Mar 2019 15:34:34 +1300 Subject: [PATCH 1/3] Made CC2500 (FrSky, SFHSS) SPI RX runtime configurable. --- src/main/cli/cli.c | 8 +++ src/main/cli/settings.c | 16 +++-- src/main/drivers/resource.c | 4 ++ src/main/drivers/resource.h | 4 ++ src/main/drivers/rx/rx_a7105.c | 2 +- src/main/drivers/rx/rx_cyrf6936.c | 4 +- src/main/pg/pg_ids.h | 2 +- src/main/pg/rx_spi.c | 2 + src/main/pg/rx_spi.h | 1 + src/main/pg/rx_spi_cc2500.c | 49 +++++++++++++ src/main/pg/rx_spi_cc2500.h | 46 ++++++++++++ src/main/rx/cc2500_common.c | 78 +++++++++++++-------- src/main/rx/cc2500_common.h | 2 - src/main/rx/cc2500_frsky_common.h | 19 ----- src/main/rx/cc2500_frsky_d.c | 17 ++--- src/main/rx/cc2500_frsky_shared.c | 57 ++++++--------- src/main/rx/cc2500_frsky_x.c | 20 +++--- src/main/rx/cc2500_sfhss.c | 28 ++++---- src/main/rx/rx_spi_common.c | 57 +++++++++------ src/main/target/CJMCU/target.h | 2 +- src/main/target/CRAZYBEEF3FR/target.h | 4 +- src/main/target/EACHIF3/target.h | 2 +- src/main/target/MATEKF411RX/target.h | 8 +-- src/main/target/MIDELICF3/target.h | 6 +- src/main/target/OMNIBUSF4/target.h | 2 +- src/main/target/STM32F411/target.h | 20 +++--- src/main/target/common_defaults_post.h | 26 +++++++ src/main/target/common_post.h | 8 +++ unified_targets/configs/CRAZYBEEF4FR.config | 19 ++--- 29 files changed, 329 insertions(+), 184 deletions(-) create mode 100644 src/main/pg/rx_spi_cc2500.c create mode 100644 src/main/pg/rx_spi_cc2500.h diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index c1b444c7fb..aa50ff788d 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -4411,8 +4411,16 @@ const cliResourceValue_t resourceTable[] = { #endif #ifdef USE_RX_SPI DEFS( OWNER_RX_SPI_CS, PG_RX_SPI_CONFIG, rxSpiConfig_t, csnTag ), + DEFS( OWNER_RX_SPI_EXTI, PG_RX_SPI_CONFIG, rxSpiConfig_t, extiIoTag ), DEFS( OWNER_RX_SPI_BIND, PG_RX_SPI_CONFIG, rxSpiConfig_t, bindIoTag ), DEFS( OWNER_RX_SPI_LED, PG_RX_SPI_CONFIG, rxSpiConfig_t, ledIoTag ), +#if defined(USE_RX_SPI_CC2500) && defined(USE_RX_CC2500_SPI_PA_LNA) + DEFS( OWNER_RX_SPI_CC2500_TX_EN, PG_RX_SPI_CC2500_CONFIG, rxCc2500SpiConfig_t, txEnIoTag ), + DEFS( OWNER_RX_SPI_CC2500_LNA_EN, PG_RX_SPI_CC2500_CONFIG, rxCc2500SpiConfig_t, lnaEnIoTag ), +#if defined(USE_RX_CC2500_SPI_DIVERSITY) + DEFS( OWNER_RX_SPI_CC2500_ANT_SEL, PG_RX_SPI_CC2500_CONFIG, rxCc2500SpiConfig_t, antSelIoTag ), +#endif +#endif #endif #ifdef USE_GYRO_EXTI DEFW( OWNER_GYRO_EXTI, PG_GYRO_DEVICE_CONFIG, gyroDeviceConfig_t, extiTag, MAX_GYRODEV_COUNT ), diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index e225bad9b2..2b699459b9 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -79,8 +79,9 @@ #include "pg/pinio.h" #include "pg/piniobox.h" #include "pg/rx.h" -#include "pg/rx_spi.h" #include "pg/rx_pwm.h" +#include "pg/rx_spi.h" +#include "pg/rx_spi_cc2500.h" #include "pg/sdcard.h" #include "pg/vcd.h" #include "pg/usb.h" @@ -1300,12 +1301,13 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_RX_FRSKY_SPI - { "frsky_spi_autobind", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, autoBind) }, - { "frsky_spi_tx_id", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 2, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, bindTxId) }, - { "frsky_spi_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -127, 127 }, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, bindOffset) }, - { "frsky_spi_bind_hop_data", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 50, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, bindHopData) }, - { "frsky_x_rx_num", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, rxNum) }, - { "frsky_spi_a1_source", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RX_FRSKY_SPI_A1_SOURCE }, PG_RX_FRSKY_SPI_CONFIG, offsetof(rxFrSkySpiConfig_t, a1Source) }, + { "frsky_spi_autobind", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, autoBind) }, + { "frsky_spi_tx_id", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 2, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, bindTxId) }, + { "frsky_spi_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -127, 127 }, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, bindOffset) }, + { "frsky_spi_bind_hop_data", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 50, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, bindHopData) }, + { "frsky_x_rx_num", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, rxNum) }, + { "frsky_spi_a1_source", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RX_FRSKY_SPI_A1_SOURCE }, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, a1Source) }, + { "cc2500_spi_chip_detect", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CC2500_SPI_CONFIG, offsetof(rxCc2500SpiConfig_t, chipDetectEnabled) }, #endif { "led_inversion", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, ((1 << STATUS_LED_NUMBER) - 1) }, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, inversion) }, #ifdef USE_DASHBOARD diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index 30da4136da..45bf5ab926 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -80,4 +80,8 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "RX_SPI_BIND", "RX_SPI_LED", "PREINIT", + "RX_SPI_EXTI", + "RX_SPI_CC2500_TX_EN", + "RX_SPI_CC2500_LNA_EN", + "RX_SPI_CC2500_ANT_SEL", }; diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 378f0fc015..71ba9475a9 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -78,6 +78,10 @@ typedef enum { OWNER_RX_SPI_BIND, OWNER_RX_SPI_LED, OWNER_PREINIT, + OWNER_RX_SPI_EXTI, + OWNER_RX_SPI_CC2500_TX_EN, + OWNER_RX_SPI_CC2500_LNA_EN, + OWNER_RX_SPI_CC2500_ANT_SEL, OWNER_TOTAL_COUNT } resourceOwner_e; diff --git a/src/main/drivers/rx/rx_a7105.c b/src/main/drivers/rx/rx_a7105.c index 01c5d51de1..48bcb143e5 100644 --- a/src/main/drivers/rx/rx_a7105.c +++ b/src/main/drivers/rx/rx_a7105.c @@ -57,7 +57,7 @@ void a7105extiHandler(extiCallbackRec_t* cb) void A7105Init(uint32_t id) { spiDeviceByInstance(RX_SPI_INSTANCE); - rxIntIO = IOGetByTag(IO_TAG(RX_IRQ_PIN)); /* config receiver IRQ pin */ + rxIntIO = IOGetByTag(IO_TAG(RX_EXTI_PIN)); /* config receiver IRQ pin */ IOInit(rxIntIO, OWNER_RX_SPI_CS, 0); EXTIHandlerInit(&a7105extiCallbackRec, a7105extiHandler); EXTIConfig(rxIntIO, &a7105extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IPD, EXTI_TRIGGER_RISING); diff --git a/src/main/drivers/rx/rx_cyrf6936.c b/src/main/drivers/rx/rx_cyrf6936.c index 4b2527ad3d..5b188d7033 100644 --- a/src/main/drivers/rx/rx_cyrf6936.c +++ b/src/main/drivers/rx/rx_cyrf6936.c @@ -72,7 +72,7 @@ bool cyrf6936RxFinished(uint32_t *timeStamp) bool cyrf6936Init(void) { spiDeviceByInstance(RX_SPI_INSTANCE); - rxIntIO = IOGetByTag(IO_TAG(RX_IRQ_PIN)); + rxIntIO = IOGetByTag(IO_TAG(RX_EXTI_PIN)); IOInit(rxIntIO, OWNER_RX_SPI_CS, 0); EXTIHandlerInit(&cyrf6936extiCallbackRec, cyrf6936ExtiHandler); EXTIConfig(rxIntIO, &cyrf6936extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IPD, EXTI_TRIGGER_FALLING); @@ -177,4 +177,4 @@ void cyrf6936RecvLen(uint8_t *data, const uint8_t length) cyrf6936ReadBlock(CYRF6936_RX_BUFFER, data, length); } -#endif /* USE_RX_SPEKTRUM */ \ No newline at end of file +#endif /* USE_RX_SPEKTRUM */ diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h index cbbf2625a6..3b67e65b4f 100644 --- a/src/main/pg/pg_ids.h +++ b/src/main/pg/pg_ids.h @@ -117,7 +117,7 @@ #define PG_SPI_PIN_CONFIG 520 #define PG_ESCSERIAL_CONFIG 521 #define PG_CAMERA_CONTROL_CONFIG 522 -#define PG_RX_FRSKY_SPI_CONFIG 523 +#define PG_RX_CC2500_SPI_CONFIG 523 #define PG_MAX7456_CONFIG 524 #define PG_FLYSKY_CONFIG 525 #define PG_TIME_CONFIG 526 diff --git a/src/main/pg/rx_spi.c b/src/main/pg/rx_spi.c index dbdfc432f0..f9b6760fc4 100644 --- a/src/main/pg/rx_spi.c +++ b/src/main/pg/rx_spi.c @@ -41,6 +41,8 @@ void pgResetFn_rxSpiConfig(rxSpiConfig_t *rxSpiConfig) rxSpiConfig->csnTag = IO_TAG(RX_NSS_PIN); rxSpiConfig->spibus = SPI_DEV_TO_CFG(spiDeviceByInstance(RX_SPI_INSTANCE)); + rxSpiConfig->extiIoTag = IO_TAG(RX_SPI_EXTI_PIN); + rxSpiConfig->bindIoTag = IO_TAG(BINDPLUG_PIN); rxSpiConfig->ledIoTag = IO_TAG(RX_SPI_LED_PIN); #ifdef RX_SPI_LED_INVERTED diff --git a/src/main/pg/rx_spi.h b/src/main/pg/rx_spi.h index adbd80a8af..15fa79bc62 100644 --- a/src/main/pg/rx_spi.h +++ b/src/main/pg/rx_spi.h @@ -39,6 +39,7 @@ typedef struct rxSpiConfig_s { ioTag_t ledIoTag; uint8_t ledInversion; + ioTag_t extiIoTag; } rxSpiConfig_t; PG_DECLARE(rxSpiConfig_t, rxSpiConfig); diff --git a/src/main/pg/rx_spi_cc2500.c b/src/main/pg/rx_spi_cc2500.c new file mode 100644 index 0000000000..885046c867 --- /dev/null +++ b/src/main/pg/rx_spi_cc2500.c @@ -0,0 +1,49 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include "platform.h" + +#if defined(USE_RX_CC2500) + +#include "drivers/io.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "rx_spi_cc2500.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(rxCc2500SpiConfig_t, rxCc2500SpiConfig, PG_RX_CC2500_SPI_CONFIG, 1); + +PG_RESET_TEMPLATE(rxCc2500SpiConfig_t, rxCc2500SpiConfig, + .autoBind = false, + .bindTxId = {0, 0}, + .bindOffset = 0, + .bindHopData = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + .rxNum = 0, + .a1Source = FRSKY_SPI_A1_SOURCE_VBAT, + .chipDetectEnabled = true, + .txEnIoTag = IO_TAG(RX_CC2500_SPI_TX_EN_PIN), + .lnaEnIoTag = IO_TAG(RX_CC2500_SPI_LNA_EN_PIN), + .antSelIoTag = IO_TAG(RX_CC2500_SPI_ANT_SEL_PIN), +); +#endif diff --git a/src/main/pg/rx_spi_cc2500.h b/src/main/pg/rx_spi_cc2500.h new file mode 100644 index 0000000000..eb8beeb8a0 --- /dev/null +++ b/src/main/pg/rx_spi_cc2500.h @@ -0,0 +1,46 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include "drivers/io_types.h" + +#include "pg/pg.h" + +typedef enum { + FRSKY_SPI_A1_SOURCE_VBAT = 0, + FRSKY_SPI_A1_SOURCE_EXTADC, + FRSKY_SPI_A1_SOURCE_CONST +} frSkySpiA1Source_e; + +typedef struct rxFrSkySpiConfig_s { + uint8_t autoBind; + uint8_t bindTxId[2]; + int8_t bindOffset; + uint8_t bindHopData[50]; + uint8_t rxNum; + uint8_t a1Source; + uint8_t chipDetectEnabled; + ioTag_t txEnIoTag; + ioTag_t lnaEnIoTag; + ioTag_t antSelIoTag; +} rxCc2500SpiConfig_t; + +PG_DECLARE(rxCc2500SpiConfig_t, rxCc2500SpiConfig); diff --git a/src/main/rx/cc2500_common.c b/src/main/rx/cc2500_common.c index e282b66ac9..ebb907d709 100755 --- a/src/main/rx/cc2500_common.c +++ b/src/main/rx/cc2500_common.c @@ -26,28 +26,31 @@ #include "common/maths.h" -#include "pg/pg.h" -#include "pg/pg_ids.h" -#include "pg/rx.h" -#include "pg/rx_spi.h" - -#include "drivers/rx/rx_cc2500.h" #include "drivers/io.h" +#include "drivers/rx/rx_cc2500.h" #include "drivers/time.h" #include "fc/config.h" +#include "pg/pg.h" +#include "pg/pg_ids.h" +#include "pg/rx.h" +#include "pg/rx_spi.h" +#include "pg/rx_spi_cc2500.h" + #include "rx/rx.h" #include "rx/rx_spi.h" -#include "rx/cc2500_common.h" +#include "cc2500_common.h" static IO_t gdoPin; #if defined(USE_RX_CC2500_SPI_PA_LNA) static IO_t txEnPin; static IO_t rxLnaEnPin; +#if defined(USE_RX_CC2500_SPI_DIVERSITY) static IO_t antSelPin; #endif +#endif static int16_t rssiDbm; uint16_t cc2500getRssiDbm(void) @@ -76,23 +79,29 @@ void cc2500switchAntennae(void) { static bool alternativeAntennaSelected = true; - if (alternativeAntennaSelected) { - IOLo(antSelPin); - } else { - IOHi(antSelPin); + if (antSelPin) { + if (alternativeAntennaSelected) { + IOLo(antSelPin); + } else { + IOHi(antSelPin); + } + alternativeAntennaSelected = !alternativeAntennaSelected; } - alternativeAntennaSelected = !alternativeAntennaSelected; } #endif #if defined(USE_RX_CC2500_SPI_PA_LNA) void cc2500TxEnable(void) { - IOHi(txEnPin); + if (txEnPin) { + IOHi(txEnPin); + } } void cc2500TxDisable(void) { - IOLo(txEnPin); + if (txEnPin) { + IOLo(txEnPin); + } } #endif @@ -110,7 +119,7 @@ static bool cc2500SpiDetect(void) bool cc2500SpiInit(void) { #if !defined(RX_CC2500_SPI_DISABLE_CHIP_DETECTION) - if (!cc2500SpiDetect()) { + if (rxCc2500SpiConfig()->chipDetectEnabled && !cc2500SpiDetect()) { return false; } #else @@ -122,28 +131,39 @@ bool cc2500SpiInit(void) } // gpio init here - gdoPin = IOGetByTag(IO_TAG(RX_CC2500_SPI_GDO_0_PIN)); + gdoPin = IOGetByTag(rxSpiConfig()->extiIoTag); + + if (!gdoPin) { + return false; + } + IOInit(gdoPin, OWNER_RX_SPI, 0); IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING); #if defined(USE_RX_CC2500_SPI_PA_LNA) - rxLnaEnPin = IOGetByTag(IO_TAG(RX_CC2500_SPI_LNA_EN_PIN)); - IOInit(rxLnaEnPin, OWNER_RX_SPI, 0); - IOConfigGPIO(rxLnaEnPin, IOCFG_OUT_PP); - IOHi(rxLnaEnPin); // always on at the moment - txEnPin = IOGetByTag(IO_TAG(RX_CC2500_SPI_TX_EN_PIN)); - IOInit(txEnPin, OWNER_RX_SPI, 0); - IOConfigGPIO(txEnPin, IOCFG_OUT_PP); + if (rxCc2500SpiConfig()->lnaEnIoTag) { + rxLnaEnPin = IOGetByTag(rxCc2500SpiConfig()->lnaEnIoTag); + IOInit(rxLnaEnPin, OWNER_RX_SPI, 0); + IOConfigGPIO(rxLnaEnPin, IOCFG_OUT_PP); + + IOHi(rxLnaEnPin); // always on at the moment + } + if (rxCc2500SpiConfig()->txEnIoTag) { + txEnPin = IOGetByTag(rxCc2500SpiConfig()->txEnIoTag); + IOInit(txEnPin, OWNER_RX_SPI, 0); + IOConfigGPIO(txEnPin, IOCFG_OUT_PP); + } #if defined(USE_RX_CC2500_SPI_DIVERSITY) - antSelPin = IOGetByTag(IO_TAG(RX_CC2500_SPI_ANT_SEL_PIN)); - IOInit(antSelPin, OWNER_RX_SPI, 0); - IOConfigGPIO(antSelPin, IOCFG_OUT_PP); + if (rxCc2500SpiConfig()->antSelIoTag) { + antSelPin = IOGetByTag(rxCc2500SpiConfig()->antSelIoTag); + IOInit(antSelPin, OWNER_RX_SPI, 0); + IOConfigGPIO(antSelPin, IOCFG_OUT_PP); + + IOHi(antSelPin); + } #endif #endif // USE_RX_CC2500_SPI_PA_LNA #if defined(USE_RX_CC2500_SPI_PA_LNA) -#if defined(USE_RX_CC2500_SPI_DIVERSITY) - IOHi(antSelPin); -#endif cc2500TxDisable(); #endif // USE_RX_CC2500_SPI_PA_LNA diff --git a/src/main/rx/cc2500_common.h b/src/main/rx/cc2500_common.h index 26ded6ccdc..7fdfe528c9 100755 --- a/src/main/rx/cc2500_common.h +++ b/src/main/rx/cc2500_common.h @@ -20,8 +20,6 @@ #pragma once -#include "pg/pg.h" - #include "rx/rx_spi.h" uint16_t cc2500getRssiDbm(void); diff --git a/src/main/rx/cc2500_frsky_common.h b/src/main/rx/cc2500_frsky_common.h index 88630fbc26..c67e63608c 100644 --- a/src/main/rx/cc2500_frsky_common.h +++ b/src/main/rx/cc2500_frsky_common.h @@ -20,27 +20,8 @@ #pragma once -#include "pg/pg.h" - #include "rx/rx_spi.h" -typedef enum { - FRSKY_SPI_A1_SOURCE_VBAT = 0, - FRSKY_SPI_A1_SOURCE_EXTADC, - FRSKY_SPI_A1_SOURCE_CONST -} frSkySpiA1Source_e; - -typedef struct rxFrSkySpiConfig_s { - uint8_t autoBind; - uint8_t bindTxId[2]; - int8_t bindOffset; - uint8_t bindHopData[50]; - uint8_t rxNum; - uint8_t a1Source; -} rxFrSkySpiConfig_t; - -PG_DECLARE(rxFrSkySpiConfig_t, rxFrSkySpiConfig); - bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeConfig); rx_spi_received_e frSkySpiDataReceived(uint8_t *packet); void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload); diff --git a/src/main/rx/cc2500_frsky_d.c b/src/main/rx/cc2500_frsky_d.c index 177e98dbab..32a9a169ba 100644 --- a/src/main/rx/cc2500_frsky_d.c +++ b/src/main/rx/cc2500_frsky_d.c @@ -30,9 +30,6 @@ #include "build/build_config.h" #include "build/debug.h" -#include "pg/rx.h" -#include "pg/rx_spi.h" - #include "common/maths.h" #include "common/utils.h" @@ -46,6 +43,10 @@ #include "fc/config.h" +#include "pg/rx.h" +#include "pg/rx_spi.h" +#include "pg/rx_spi_cc2500.h" + #include "rx/rx_spi_common.h" #include "rx/cc2500_common.h" #include "rx/cc2500_frsky_common.h" @@ -115,7 +116,7 @@ static void frSkyDTelemetryWriteByte(const char data) static void buildTelemetryFrame(uint8_t *packet) { uint8_t a1Value; - switch (rxFrSkySpiConfig()->a1Source) { + switch (rxCc2500SpiConfig()->a1Source) { case FRSKY_SPI_A1_SOURCE_VBAT: a1Value = (getBatteryVoltage() / 5) & 0xff; break; @@ -129,8 +130,8 @@ static void buildTelemetryFrame(uint8_t *packet) const uint8_t a2Value = (adcGetChannel(ADC_RSSI)) >> 4; telemetryId = packet[4]; frame[0] = 0x11; // length - frame[1] = rxFrSkySpiConfig()->bindTxId[0]; - frame[2] = rxFrSkySpiConfig()->bindTxId[1]; + frame[1] = rxCc2500SpiConfig()->bindTxId[0]; + frame[2] = rxCc2500SpiConfig()->bindTxId[1]; frame[3] = a1Value; frame[4] = a2Value; frame[5] = (uint8_t)cc2500getRssiDbm(); @@ -223,8 +224,8 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro missingPackets = 0; timeoutUs = 1; if (packet[0] == 0x11) { - if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) && - (packet[2] == rxFrSkySpiConfig()->bindTxId[1])) { + if ((packet[1] == rxCc2500SpiConfig()->bindTxId[0]) && + (packet[2] == rxCc2500SpiConfig()->bindTxId[1])) { rxSpiLedOn(); nextChannel(1); cc2500setRssiDbm(packet[18]); diff --git a/src/main/rx/cc2500_frsky_shared.c b/src/main/rx/cc2500_frsky_shared.c index 8895a48737..f24b39b03d 100644 --- a/src/main/rx/cc2500_frsky_shared.c +++ b/src/main/rx/cc2500_frsky_shared.c @@ -26,17 +26,16 @@ #include "common/maths.h" -#include "pg/pg.h" -#include "pg/pg_ids.h" -#include "pg/rx.h" -#include "pg/rx_spi.h" - #include "drivers/rx/rx_cc2500.h" #include "drivers/io.h" #include "drivers/time.h" #include "fc/config.h" +#include "pg/rx.h" +#include "pg/rx_spi.h" +#include "pg/rx_spi_cc2500.h" + #include "rx/rx.h" #include "rx/rx_spi.h" #include "rx/rx_spi_common.h" @@ -68,20 +67,6 @@ typedef void setRcDataFn(uint16_t *rcData, const uint8_t *payload); static handlePacketFn *handlePacket; static setRcDataFn *setRcData; -PG_REGISTER_WITH_RESET_TEMPLATE(rxFrSkySpiConfig_t, rxFrSkySpiConfig, PG_RX_FRSKY_SPI_CONFIG, 1); - -PG_RESET_TEMPLATE(rxFrSkySpiConfig_t, rxFrSkySpiConfig, - .autoBind = false, - .bindTxId = {0, 0}, - .bindOffset = 0, - .bindHopData = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - .rxNum = 0, - .a1Source = FRSKY_SPI_A1_SOURCE_VBAT, -); - static void initialise() { cc2500Reset(); cc2500WriteReg(CC2500_02_IOCFG0, 0x01); @@ -169,9 +154,9 @@ static void initialise() { void initialiseData(uint8_t adr) { - cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)rxFrSkySpiConfig()->bindOffset); + cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)rxCc2500SpiConfig()->bindOffset); cc2500WriteReg(CC2500_18_MCSM0, 0x8); - cc2500WriteReg(CC2500_09_ADDR, adr ? 0x03 : rxFrSkySpiConfig()->bindTxId[0]); + cc2500WriteReg(CC2500_09_ADDR, adr ? 0x03 : rxCc2500SpiConfig()->bindTxId[0]); cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D); cc2500WriteReg(CC2500_19_FOCCFG, 0x16); delay(10); @@ -214,7 +199,7 @@ static bool tuneRx(uint8_t *packet) uint8_t Lqi = packet[ccLen - 1] & 0x7F; // higher lqi represent better link quality if (Lqi > 50) { - rxFrSkySpiConfigMutable()->bindOffset = bindOffset; + rxCc2500SpiConfigMutable()->bindOffset = bindOffset; return true; } } @@ -252,14 +237,14 @@ static bool getBind1(uint8_t *packet) if (packet[ccLen - 1] & 0x80) { if (packet[2] == 0x01) { if (packet[5] == 0x00) { - rxFrSkySpiConfigMutable()->bindTxId[0] = packet[3]; - rxFrSkySpiConfigMutable()->bindTxId[1] = packet[4]; + rxCc2500SpiConfigMutable()->bindTxId[0] = packet[3]; + rxCc2500SpiConfigMutable()->bindTxId[1] = packet[4]; for (uint8_t n = 0; n < 5; n++) { - rxFrSkySpiConfigMutable()->bindHopData[packet[5] + n] = + rxCc2500SpiConfigMutable()->bindHopData[packet[5] + n] = packet[6 + n]; } - rxFrSkySpiConfigMutable()->rxNum = packet[12]; + rxCc2500SpiConfigMutable()->rxNum = packet[12]; return true; } @@ -280,13 +265,13 @@ static bool getBind2(uint8_t *packet) cc2500ReadFifo(packet, ccLen); if (packet[ccLen - 1] & 0x80) { if (packet[2] == 0x01) { - if ((packet[3] == rxFrSkySpiConfig()->bindTxId[0]) && - (packet[4] == rxFrSkySpiConfig()->bindTxId[1])) { + if ((packet[3] == rxCc2500SpiConfig()->bindTxId[0]) && + (packet[4] == rxCc2500SpiConfig()->bindTxId[1])) { if (packet[5] == bindIdx) { #if defined(DJTS) if (packet[5] == 0x2D) { for (uint8_t i = 0; i < 2; i++) { - rxFrSkySpiConfigMutable()->bindHopData[packet[5] + i] = packet[6 + i]; + rxCc2500SpiConfigMutable()->bindHopData[packet[5] + i] = packet[6 + i]; } listLength = 47; @@ -303,7 +288,7 @@ static bool getBind2(uint8_t *packet) } } - rxFrSkySpiConfigMutable()->bindHopData[packet[5] + n] = packet[6 + n]; + rxCc2500SpiConfigMutable()->bindHopData[packet[5] + n] = packet[6 + n]; } bindIdx = bindIdx + 5; @@ -336,7 +321,7 @@ rx_spi_received_e frSkySpiDataReceived(uint8_t *packet) break; case STATE_BIND: - if (rxSpiCheckBindRequested(true) || rxFrSkySpiConfig()->autoBind) { + if (rxSpiCheckBindRequested(true) || rxCc2500SpiConfig()->autoBind) { rxSpiLedOn(); initTuneRx(); @@ -370,7 +355,7 @@ rx_spi_received_e frSkySpiDataReceived(uint8_t *packet) break; case STATE_BIND_COMPLETE: - if (!rxFrSkySpiConfig()->autoBind) { + if (!rxCc2500SpiConfig()->autoBind) { writeEEPROM(); } else { uint8_t ctr = 80; @@ -408,12 +393,12 @@ void nextChannel(uint8_t skip) } cc2500Strobe(CC2500_SIDLE); cc2500WriteReg(CC2500_23_FSCAL3, - calData[rxFrSkySpiConfig()->bindHopData[channr]][0]); + calData[rxCc2500SpiConfig()->bindHopData[channr]][0]); cc2500WriteReg(CC2500_24_FSCAL2, - calData[rxFrSkySpiConfig()->bindHopData[channr]][1]); + calData[rxCc2500SpiConfig()->bindHopData[channr]][1]); cc2500WriteReg(CC2500_25_FSCAL1, - calData[rxFrSkySpiConfig()->bindHopData[channr]][2]); - cc2500WriteReg(CC2500_0A_CHANNR, rxFrSkySpiConfig()->bindHopData[channr]); + calData[rxCc2500SpiConfig()->bindHopData[channr]][2]); + cc2500WriteReg(CC2500_0A_CHANNR, rxCc2500SpiConfig()->bindHopData[channr]); if (spiProtocol == RX_SPI_FRSKY_D) { cc2500Strobe(CC2500_SFRX); } diff --git a/src/main/rx/cc2500_frsky_x.c b/src/main/rx/cc2500_frsky_x.c index be214ae338..45232aa246 100644 --- a/src/main/rx/cc2500_frsky_x.c +++ b/src/main/rx/cc2500_frsky_x.c @@ -19,7 +19,6 @@ */ #include -#include #include "platform.h" @@ -28,9 +27,6 @@ #include "build/build_config.h" #include "build/debug.h" -#include "pg/rx.h" -#include "pg/rx_spi.h" - #include "common/maths.h" #include "common/utils.h" @@ -47,6 +43,10 @@ #include "fc/config.h" +#include "pg/rx.h" +#include "pg/rx_spi.h" +#include "pg/rx_spi_cc2500.h" + #include "rx/rx_spi_common.h" #include "rx/cc2500_common.h" #include "rx/cc2500_frsky_common.h" @@ -191,15 +191,15 @@ static void buildTelemetryFrame(uint8_t *packet) static bool evenRun = false; frame[0] = 0x0E;//length - frame[1] = rxFrSkySpiConfig()->bindTxId[0]; - frame[2] = rxFrSkySpiConfig()->bindTxId[1]; + frame[1] = rxCc2500SpiConfig()->bindTxId[0]; + frame[2] = rxCc2500SpiConfig()->bindTxId[1]; frame[3] = packet[3]; if (evenRun) { frame[4] = (uint8_t)cc2500getRssiDbm() | 0x80; } else { uint8_t a1Value; - switch (rxFrSkySpiConfig()->a1Source) { + switch (rxCc2500SpiConfig()->a1Source) { case FRSKY_SPI_A1_SOURCE_VBAT: a1Value = getLegacyBatteryVoltage() & 0x7f; break; @@ -309,9 +309,9 @@ bool isValidPacket(const uint8_t *packet) uint16_t lcrc = calculateCrc(&packet[3], (packetLength - 7)); if ((lcrc >> 8) == packet[packetLength - 4] && (lcrc & 0x00FF) == packet[packetLength - 3] && (packet[0] == packetLength - 3) && - (packet[1] == rxFrSkySpiConfig()->bindTxId[0]) && - (packet[2] == rxFrSkySpiConfig()->bindTxId[1]) && - (rxFrSkySpiConfig()->rxNum == 0 || packet[6] == 0 || packet[6] == rxFrSkySpiConfig()->rxNum)) { + (packet[1] == rxCc2500SpiConfig()->bindTxId[0]) && + (packet[2] == rxCc2500SpiConfig()->bindTxId[1]) && + (rxCc2500SpiConfig()->rxNum == 0 || packet[6] == 0 || packet[6] == rxCc2500SpiConfig()->rxNum)) { return true; } return false; diff --git a/src/main/rx/cc2500_sfhss.c b/src/main/rx/cc2500_sfhss.c index 5754f075e1..640229ce56 100755 --- a/src/main/rx/cc2500_sfhss.c +++ b/src/main/rx/cc2500_sfhss.c @@ -29,26 +29,22 @@ #include "common/maths.h" -#include "pg/pg.h" -#include "pg/pg_ids.h" -#include "pg/rx.h" -#include "pg/rx_spi.h" - #include "drivers/rx/rx_cc2500.h" #include "drivers/io.h" #include "drivers/time.h" #include "fc/config.h" +#include "pg/rx.h" +#include "pg/rx_spi.h" +#include "pg/rx_spi_cc2500.h" + +#include "rx/cc2500_common.h" #include "rx/rx.h" #include "rx/rx_spi.h" #include "rx/rx_spi_common.h" -#include "rx/cc2500_common.h" -#include "rx/cc2500_frsky_common.h" -#include "rx/cc2500_sfhss.h" - -//void cliPrintLinef(const char *format, ...); +#include "cc2500_sfhss.h" #define BIND_CH 15 #define SFHSS_PACKET_LEN 15 @@ -97,7 +93,7 @@ static void initialise() cc2500WriteReg(CC2500_08_PKTCTRL0, 0x0C); cc2500WriteReg(CC2500_09_ADDR, 0x29); cc2500WriteReg(CC2500_0B_FSCTRL1, 0x06); - cc2500WriteReg(CC2500_0C_FSCTRL0, (rxFrSkySpiConfigMutable()->bindOffset)); + cc2500WriteReg(CC2500_0C_FSCTRL0, rxCc2500SpiConfig()->bindOffset); cc2500WriteReg(CC2500_0D_FREQ2, 0x5C); cc2500WriteReg(CC2500_0E_FREQ1, 0x4E); cc2500WriteReg(CC2500_0F_FREQ0, 0xC4); @@ -167,8 +163,8 @@ static bool sfhssPacketParse(uint8_t *packet, bool check_txid) } if (check_txid) { - if ((rxFrSkySpiConfigMutable()->bindTxId[0] != GET_TXID1(packet)) || - (rxFrSkySpiConfigMutable()->bindTxId[1] != GET_TXID2(packet))) { + if ((rxCc2500SpiConfigMutable()->bindTxId[0] != GET_TXID1(packet)) || + (rxCc2500SpiConfigMutable()->bindTxId[1] != GET_TXID2(packet))) { return false; /* txid fail */ } } @@ -216,8 +212,8 @@ static bool tune1Rx(uint8_t *packet) if (sfhssRecv(packet)) { if (sfhssPacketParse(packet, false)) { if ((packet[SFHSS_PACKET_LEN - 1] & 0x7F) > 40 ) { /* lqi */ - rxFrSkySpiConfigMutable()->bindTxId[0] = GET_TXID1(packet); - rxFrSkySpiConfigMutable()->bindTxId[1] = GET_TXID2(packet); + rxCc2500SpiConfigMutable()->bindTxId[0] = GET_TXID1(packet); + rxCc2500SpiConfigMutable()->bindTxId[1] = GET_TXID2(packet); bindOffset_max = bindOffset_min; DEBUG_SET(DEBUG_RX_SFHSS_SPI, DEBUG_DATA_OFFSET_MAX, bindOffset_max); cc2500Strobe(CC2500_SRX); @@ -343,7 +339,7 @@ rx_spi_received_e sfhssSpiDataReceived(uint8_t *packet) initTuneRx(); SET_STATE(STATE_BIND_TUNING1); // retry } else { - rxFrSkySpiConfigMutable()->bindOffset = ((int16_t)bindOffset_max + (int16_t)bindOffset_min) / 2 ; + rxCc2500SpiConfigMutable()->bindOffset = ((int16_t)bindOffset_max + (int16_t)bindOffset_min) / 2 ; SET_STATE(STATE_BIND_COMPLETE); } } diff --git a/src/main/rx/rx_spi_common.c b/src/main/rx/rx_spi_common.c index 7134d1f5ce..0672f3c816 100644 --- a/src/main/rx/rx_spi_common.c +++ b/src/main/rx/rx_spi_common.c @@ -39,31 +39,45 @@ static bool lastBindPinStatus; void rxSpiCommonIOInit(const rxSpiConfig_t *rxSpiConfig) { - ledPin = IOGetByTag(rxSpiConfig->ledIoTag); - IOInit(ledPin, OWNER_LED, 0); - IOConfigGPIO(ledPin, IOCFG_OUT_PP); - ledInversion = rxSpiConfig->ledInversion; - rxSpiLedOff(); + if (rxSpiConfig->ledIoTag) { + ledPin = IOGetByTag(rxSpiConfig->ledIoTag); + IOInit(ledPin, OWNER_LED, 0); + IOConfigGPIO(ledPin, IOCFG_OUT_PP); + ledInversion = rxSpiConfig->ledInversion; + rxSpiLedOff(); + } else { + ledPin = IO_NONE; + } - bindPin = IOGetByTag(rxSpiConfig->bindIoTag); - IOInit(bindPin, OWNER_RX_BIND, 0); - IOConfigGPIO(bindPin, IOCFG_IPU); - lastBindPinStatus = IORead(bindPin); + if (rxSpiConfig->bindIoTag) { + bindPin = IOGetByTag(rxSpiConfig->bindIoTag); + IOInit(bindPin, OWNER_RX_BIND, 0); + IOConfigGPIO(bindPin, IOCFG_IPU); + lastBindPinStatus = IORead(bindPin); + } else { + bindPin = IO_NONE; + } } void rxSpiLedOn(void) { - ledInversion ? IOLo(ledPin) : IOHi(ledPin); + if (ledPin) { + ledInversion ? IOLo(ledPin) : IOHi(ledPin); + } } void rxSpiLedOff(void) { - ledInversion ? IOHi(ledPin) : IOLo(ledPin); + if (ledPin) { + ledInversion ? IOHi(ledPin) : IOLo(ledPin); + } } void rxSpiLedToggle(void) { - IOToggle(ledPin); + if (ledPin) { + IOToggle(ledPin); + } } void rxSpiLedBlink(timeMs_t blinkMs) @@ -81,14 +95,17 @@ void rxSpiLedBlink(timeMs_t blinkMs) void rxSpiLedBlinkRxLoss(rx_spi_received_e result) { static uint16_t rxLossCount = 0; - if (result == RX_SPI_RECEIVED_DATA) { - rxLossCount = 0; - rxSpiLedOn(); - } else { - if (rxLossCount < RX_LOSS_COUNT) { - rxLossCount++; + + if (ledPin) { + if (result == RX_SPI_RECEIVED_DATA) { + rxLossCount = 0; + rxSpiLedOn(); } else { - rxSpiLedBlink(INTERVAL_RX_LOSS_MS); + if (rxLossCount < RX_LOSS_COUNT) { + rxLossCount++; + } else { + rxSpiLedBlink(INTERVAL_RX_LOSS_MS); + } } } } @@ -123,4 +140,4 @@ bool rxSpiCheckBindRequested(bool reset) return true; } } -#endif \ No newline at end of file +#endif diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index af0e65110f..27553ea5b4 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -67,7 +67,7 @@ // Nordic Semiconductor uses 'CSN', STM uses 'NSS' #define RX_CE_PIN PA4 #define RX_NSS_PIN PA11 -#define RX_IRQ_PIN PA8 +#define RX_EXTI_PIN PA8 // CJMCU has NSS on PA11, rather than the standard PA4 #define SPI1_NSS_PIN RX_NSS_PIN diff --git a/src/main/target/CRAZYBEEF3FR/target.h b/src/main/target/CRAZYBEEF3FR/target.h index bf30f3323f..c890e698e0 100644 --- a/src/main/target/CRAZYBEEF3FR/target.h +++ b/src/main/target/CRAZYBEEF3FR/target.h @@ -117,7 +117,7 @@ #define FLYSKY_2A_CHANNEL_COUNT 14 #define RX_SPI_INSTANCE SPI2 #define RX_NSS_PIN SPI2_NSS_PIN -#define RX_IRQ_PIN PA8 +#define RX_EXTI_PIN PA8 #define BINDPLUG_PIN PA9 #define RX_SPI_LED_PIN PA10 #define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_MOTOR_STOP) @@ -137,7 +137,7 @@ #define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_X #define RX_SPI_INSTANCE SPI2 #define RX_NSS_PIN SPI2_NSS_PIN -#define RX_CC2500_SPI_GDO_0_PIN PA8 +#define RX_SPI_EXTI_PIN PA8 #define RX_SPI_LED_PIN PA10 #define BINDPLUG_PIN PA9 #define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_MOTOR_STOP) diff --git a/src/main/target/EACHIF3/target.h b/src/main/target/EACHIF3/target.h index 968f125288..5af0dbdcd5 100644 --- a/src/main/target/EACHIF3/target.h +++ b/src/main/target/EACHIF3/target.h @@ -58,7 +58,7 @@ #define FLYSKY_2A_CHANNEL_COUNT 10 #define RX_SPI_INSTANCE SPI2 -#define RX_IRQ_PIN PB12 +#define RX_EXTI_PIN PB12 #define SPI2_NSS_PIN PA4 #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 diff --git a/src/main/target/MATEKF411RX/target.h b/src/main/target/MATEKF411RX/target.h index a92c05f149..a5d2d80e91 100644 --- a/src/main/target/MATEKF411RX/target.h +++ b/src/main/target/MATEKF411RX/target.h @@ -106,13 +106,13 @@ #define RX_CHANNELS_AETR #define RX_SPI_DEFAULT_PROTOCOL RX_SPI_A7105_FLYSKY_2A #define FLYSKY_2A_CHANNEL_COUNT 14 -#define RX_IRQ_PIN PA14 +#define RX_EXTI_PIN PA14 #define USE_RX_FLYSKY_SPI_LED -#define RX_FLYSKY_SPI_LED_PIN PB9 +#define RX_SPI_LED_PIN PB9 #elif defined(CRAZYBEEF4FR) #define RX_CC2500_SPI_DISABLE_CHIP_DETECTION -#define RX_CC2500_SPI_GDO_0_PIN PC14 +#define RX_SPI_EXTI_PIN PC14 #define RX_SPI_LED_PIN PB9 #define USE_RX_FRSKY_SPI_D #define USE_RX_FRSKY_SPI_X @@ -122,7 +122,7 @@ #else // MATEKF411RX #define RX_CC2500_SPI_DISABLE_CHIP_DETECTION -#define RX_CC2500_SPI_GDO_0_PIN PC14 +#define RX_SPI_EXTI_PIN PC14 #define RX_SPI_LED_PIN PB9 #define RX_SPI_LED_INVERTED diff --git a/src/main/target/MIDELICF3/target.h b/src/main/target/MIDELICF3/target.h index 86d400a262..aa83fbdae3 100644 --- a/src/main/target/MIDELICF3/target.h +++ b/src/main/target/MIDELICF3/target.h @@ -102,11 +102,11 @@ #define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_X #define USE_RX_FRSKY_SPI_TELEMETRY -#define RX_NSS_PIN PA4 +#define RX_NSS_PIN PA4 -#define RX_CC2500_SPI_GDO_0_PIN PB0 +#define RX_SPI_EXTI_PIN PB0 -#define RX_SPI_LED_PIN PB6 +#define RX_SPI_LED_PIN PB6 #define USE_RX_CC2500_SPI_PA_LNA diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index bfcd0f5a17..e2d0e43f99 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -181,7 +181,7 @@ #define RX_SPI_DEFAULT_PROTOCOL RX_SPI_CYRF6936_DSM #define RX_SPI_INSTANCE SPI3 #define RX_NSS_PIN PD2 -#define RX_IRQ_PIN PA0 // instead of rssi input +#define RX_EXTI_PIN PA0 // instead of rssi input #endif #define USE_VCP diff --git a/src/main/target/STM32F411/target.h b/src/main/target/STM32F411/target.h index 9090968603..a5499b7ce7 100644 --- a/src/main/target/STM32F411/target.h +++ b/src/main/target/STM32F411/target.h @@ -101,17 +101,21 @@ #define USE_ADC -//TODO: Make this work with runtime configurability -//#define USE_RX_FRSKY_SPI_D -//#define USE_RX_FRSKY_SPI_X -//#define USE_RX_SFHSS_SPI -//#define USE_RX_FRSKY_SPI_TELEMETRY -//#define USE_RX_CC2500_SPI_PA_LNA -//#define USE_RX_CC2500_SPI_DIVERSITY +#define USE_RX_SPI + +#define USE_RX_FRSKY_SPI_D +#define USE_RX_FRSKY_SPI_X +#define USE_RX_SFHSS_SPI +#define USE_RX_FRSKY_SPI_TELEMETRY +#define USE_RX_CC2500_SPI_PA_LNA +#define USE_RX_CC2500_SPI_DIVERSITY //TODO: Make this work with runtime configurability //#define USE_RX_FLYSKY -//#define USE_RX_FLYSKY_SPI_LED + +//TODO: Make this work with runtime configurability +//#define USE_RX_SPEKTRUM +//#define USE_RX_SPEKTRUM_TELEMETRY #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff diff --git a/src/main/target/common_defaults_post.h b/src/main/target/common_defaults_post.h index 9e6829000e..4303d1f9ec 100644 --- a/src/main/target/common_defaults_post.h +++ b/src/main/target/common_defaults_post.h @@ -262,9 +262,35 @@ #endif #ifdef USE_RX_SPI +#if !defined(RX_SPI_INSTANCE) +#define RX_SPI_INSTANCE NULL +#endif + +#if !defined(RX_NSS_PIN) +#define RX_NSS_PIN NONE +#endif + #ifndef RX_SPI_LED_PIN #define RX_SPI_LED_PIN NONE #endif + +#if !defined(RX_SPI_EXTI_PIN) +#define RX_SPI_EXTI_PIN NONE +#endif + +#if defined(USE_RX_CC2500) +#if !defined(RX_CC2500_SPI_TX_EN_PIN) +#define RX_CC2500_SPI_TX_EN_PIN NONE +#endif + +#if !defined(RX_CC2500_SPI_LNA_EN_PIN) +#define RX_CC2500_SPI_LNA_EN_PIN NONE +#endif + +#if !defined(RX_CC2500_SPI_ANT_SEL_PIN) +#define RX_CC2500_SPI_ANT_SEL_PIN NONE +#endif +#endif #endif // F4 and F7 single gyro boards diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 1787d1f16d..7473a968ba 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -145,6 +145,14 @@ #define USE_RX_CC2500 #endif +#if !defined(USE_RX_CC2500) +#undef USE_RX_CC2500_SPI_PA_LNA +#endif + +#if !defined(USE_RX_CC2500_SPI_PA_LNA) +#undef USE_RX_CC2500_SPI_DIVERSITY +#endif + // Burst dshot to default off if not configured explicitly by target #ifndef ENABLE_DSHOT_DMAR #define ENABLE_DSHOT_DMAR false diff --git a/unified_targets/configs/CRAZYBEEF4FR.config b/unified_targets/configs/CRAZYBEEF4FR.config index 0696788739..c388d23be4 100644 --- a/unified_targets/configs/CRAZYBEEF4FR.config +++ b/unified_targets/configs/CRAZYBEEF4FR.config @@ -1,6 +1,4 @@ -# Betaflight / CRAZYBEEF4FR (C4FR) 4.0.0 Feb 28 2019 / 18:10:50 (b63a3e117) MSP API: 1.41 - -# This is not usable yet, as the SPI RX is not fully runtime configurable yet +# Betaflight / CRAZYBEEF4FR (C4FR) 4.0.0 Mar 2 2019 / 21:03:37 (48d32d8b1) MSP API: 1.41 board_name CRAZYBEEF4FR manufacturer_id HAMO @@ -35,6 +33,7 @@ resource ADC_BATT 1 B00 resource ADC_CURR 1 B01 resource OSD_CS 1 B12 resource RX_SPI_CS 1 A15 +resource RX_SPI_EXTI 1 C14 resource RX_SPI_BIND 1 B02 resource RX_SPI_LED 1 B09 resource GYRO_EXTI 1 A01 @@ -70,12 +69,7 @@ dmaopt pin A10 0 # pin A10: DMA2 Stream 6 Channel 0 # feature -feature TELEMETRY -feature OSD -feature AIRMODE feature RX_SPI -feature ANTI_GRAVITY -feature DYNAMIC_FILTER # master set rx_spi_protocol = FRSKY_X @@ -83,13 +77,12 @@ set rx_spi_bus = 3 set rx_spi_led_inversion = OFF set adc_device = 1 set motor_pwm_protocol = DSHOT600 +set current_meter = ADC +set battery_meter = ADC set beeper_inversion = ON set beeper_od = OFF -set max7456_clock = DEFAULT +set system_hse_mhz = 8 set max7456_spi_bus = 2 -set max7456_preinit_opu = OFF +set cc2500_spi_chip_detect = ON set gyro_1_bustype = SPI set gyro_1_spibus = 1 -set gyro_1_i2cBus = 0 -set gyro_1_i2c_address = 0 -set gyro_1_sensor_align = CW90 From 96d5b5dcacb62083725e86cb14dc5a5e3a0fead1 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 2 Mar 2019 23:54:02 +1300 Subject: [PATCH 2/3] Added configurability for FlySky. --- src/main/cli/settings.c | 1 - src/main/drivers/rx/rx_a7105.c | 34 +++++++++++++++--------------- src/main/drivers/rx/rx_a7105.h | 2 +- src/main/rx/a7105_flysky.c | 19 +++++++++-------- src/main/rx/a7105_flysky.h | 1 - src/main/rx/cc2500_common.c | 12 +++++++---- src/main/target/STM32F411/target.h | 3 +-- 7 files changed, 37 insertions(+), 35 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 2b699459b9..9bfbec5682 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1412,7 +1412,6 @@ const clivalue_t valueTable[] = { #ifdef USE_RX_FLYSKY { "flysky_spi_tx_id", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_FLYSKY_CONFIG, offsetof(flySkyConfig_t, txId) }, { "flysky_spi_rf_channels", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 16, PG_FLYSKY_CONFIG, offsetof(flySkyConfig_t, rfChannelMap) }, - { "flysky_spi_protocol", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_FLYSKY_CONFIG, offsetof(flySkyConfig_t, protocol) }, #endif }; diff --git a/src/main/drivers/rx/rx_a7105.c b/src/main/drivers/rx/rx_a7105.c index 48bcb143e5..cd9ce7d91e 100644 --- a/src/main/drivers/rx/rx_a7105.c +++ b/src/main/drivers/rx/rx_a7105.c @@ -35,9 +35,7 @@ #include "drivers/rx/rx_spi.h" #include "drivers/time.h" -#ifdef RX_PA_TXEN_PIN static IO_t txEnIO = IO_NONE; -#endif static IO_t rxIntIO = IO_NONE; static extiCallbackRec_t a7105extiCallbackRec; @@ -48,26 +46,28 @@ void a7105extiHandler(extiCallbackRec_t* cb) { UNUSED(cb); - if (IORead (rxIntIO) != 0) { + if (IORead(rxIntIO) != 0) { timeEvent = micros(); occurEvent = true; } } -void A7105Init(uint32_t id) +void A7105Init(uint32_t id, IO_t extiPin, IO_t txEnPin) { spiDeviceByInstance(RX_SPI_INSTANCE); - rxIntIO = IOGetByTag(IO_TAG(RX_EXTI_PIN)); /* config receiver IRQ pin */ - IOInit(rxIntIO, OWNER_RX_SPI_CS, 0); + rxIntIO = extiPin; /* config receiver IRQ pin */ + IOInit(rxIntIO, OWNER_RX_SPI, 0); EXTIHandlerInit(&a7105extiCallbackRec, a7105extiHandler); EXTIConfig(rxIntIO, &a7105extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IPD, EXTI_TRIGGER_RISING); EXTIEnable(rxIntIO, false); -#ifdef RX_PA_TXEN_PIN - txEnIO = IOGetByTag(IO_TAG(RX_PA_TXEN_PIN)); - IOInit(txEnIO, OWNER_RX_SPI_CS, 0); - IOConfigGPIO(txEnIO, IOCFG_OUT_PP); -#endif + if (txEnPin) { + txEnIO = txEnPin; + IOInit(txEnIO, OWNER_RX_SPI_CS, 0); + IOConfigGPIO(txEnIO, IOCFG_OUT_PP); + } else { + txEnIO = IO_NONE; + } A7105SoftReset(); A7105WriteID(id); @@ -135,13 +135,13 @@ void A7105Strobe(A7105State_t state) EXTIEnable(rxIntIO, false); } -#ifdef RX_PA_TXEN_PIN - if (A7105_TX == state) { - IOHi(txEnIO); /* enable PA */ - } else { - IOLo(txEnIO); /* disable PA */ + if (txEnIO) { + if (A7105_TX == state) { + IOHi(txEnIO); /* enable PA */ + } else { + IOLo(txEnIO); /* disable PA */ + } } -#endif rxSpiWriteByte((uint8_t)state); } diff --git a/src/main/drivers/rx/rx_a7105.h b/src/main/drivers/rx/rx_a7105.h index 1db51e0e3c..88f89b8d1b 100644 --- a/src/main/drivers/rx/rx_a7105.h +++ b/src/main/drivers/rx/rx_a7105.h @@ -97,7 +97,7 @@ typedef enum { #define A7105_MODE_TRSR 0x02 // [0]: RX state. [1]: TX state. Serviceable if TRER=1 (TRX is enable). #define A7105_MODE_TRER 0x01 // [0]: TRX is disabled. [1]: TRX is enabled. -void A7105Init(uint32_t id); +void A7105Init(uint32_t id, IO_t extiPin, IO_t txEnPin); void A7105SoftReset(void); void A7105Config(const uint8_t *regsTable, uint8_t size); diff --git a/src/main/rx/a7105_flysky.c b/src/main/rx/a7105_flysky.c index 83d79dc247..894b6e5cbd 100644 --- a/src/main/rx/a7105_flysky.c +++ b/src/main/rx/a7105_flysky.c @@ -19,6 +19,7 @@ */ #include + #include "platform.h" #ifdef USE_RX_FLYSKY @@ -57,8 +58,8 @@ #error "FlySky AFHDS 2A protocol support 14 channel max" #endif -PG_REGISTER_WITH_RESET_TEMPLATE(flySkyConfig_t, flySkyConfig, PG_FLYSKY_CONFIG, 0); -PG_RESET_TEMPLATE(flySkyConfig_t, flySkyConfig, .txId = 0, .rfChannelMap = {0}, .protocol = 0); +PG_REGISTER_WITH_RESET_TEMPLATE(flySkyConfig_t, flySkyConfig, PG_FLYSKY_CONFIG, 1); +PG_RESET_TEMPLATE(flySkyConfig_t, flySkyConfig, .txId = 0, .rfChannelMap = {0}); static const uint8_t flySkyRegs[] = { 0xff, 0x42, 0x00, 0x14, 0x00, 0xff, 0xff, 0x00, @@ -355,12 +356,13 @@ static rx_spi_received_e flySkyReadAndProcess(uint8_t *payload, const uint32_t t bool flySkyInit(const rxSpiConfig_t *rxSpiConfig, struct rxRuntimeConfig_s *rxRuntimeConfig) { - protocol = rxSpiConfig->rx_spi_protocol; - - if (protocol != flySkyConfig()->protocol) { - PG_RESET(flySkyConfig); + IO_t extiPin = IOGetByTag(rxSpiConfig->extiIoTag); + if (!extiPin) { + return false; } + protocol = rxSpiConfig->rx_spi_protocol; + rxSpiCommonIOInit(rxSpiConfig); uint8_t startRxChannel; @@ -370,13 +372,13 @@ bool flySkyInit(const rxSpiConfig_t *rxSpiConfig, struct rxRuntimeConfig_s *rxRu timings = &flySky2ATimings; rxId = U_ID_0 ^ U_ID_1 ^ U_ID_2; startRxChannel = flySky2ABindChannels[0]; - A7105Init(0x5475c52A); + A7105Init(0x5475c52A, extiPin, IO_NONE); A7105Config(flySky2ARegs, sizeof(flySky2ARegs)); } else { rxRuntimeConfig->channelCount = FLYSKY_CHANNEL_COUNT; timings = &flySkyTimings; startRxChannel = 0; - A7105Init(0x5475c52A); + A7105Init(0x5475c52A, extiPin, IO_NONE); A7105Config(flySkyRegs, sizeof(flySkyRegs)); } @@ -459,7 +461,6 @@ rx_spi_received_e flySkyDataReceived(uint8_t *payload) bound = true; flySkyConfigMutable()->txId = txId; // store TXID memcpy (flySkyConfigMutable()->rfChannelMap, rfChannelMap, FLYSKY_FREQUENCY_COUNT);// store channel map - flySkyConfigMutable()->protocol = protocol; writeEEPROM(); } rxSpiLedBlinkBind(); diff --git a/src/main/rx/a7105_flysky.h b/src/main/rx/a7105_flysky.h index 69f15ae1cd..537ef70f5d 100644 --- a/src/main/rx/a7105_flysky.h +++ b/src/main/rx/a7105_flysky.h @@ -26,7 +26,6 @@ typedef struct flySkyConfig_s { uint32_t txId; uint8_t rfChannelMap[16]; - rx_spi_protocol_e protocol; } flySkyConfig_t; PG_DECLARE(flySkyConfig_t, flySkyConfig); diff --git a/src/main/rx/cc2500_common.c b/src/main/rx/cc2500_common.c index ebb907d709..9acc798a65 100755 --- a/src/main/rx/cc2500_common.c +++ b/src/main/rx/cc2500_common.c @@ -126,10 +126,6 @@ bool cc2500SpiInit(void) UNUSED(cc2500SpiDetect); #endif - if (rssiSource == RSSI_SOURCE_NONE) { - rssiSource = RSSI_SOURCE_RX_PROTOCOL; - } - // gpio init here gdoPin = IOGetByTag(rxSpiConfig()->extiIoTag); @@ -151,6 +147,8 @@ bool cc2500SpiInit(void) txEnPin = IOGetByTag(rxCc2500SpiConfig()->txEnIoTag); IOInit(txEnPin, OWNER_RX_SPI, 0); IOConfigGPIO(txEnPin, IOCFG_OUT_PP); + } else { + txEnPin = IO_NONE; } #if defined(USE_RX_CC2500_SPI_DIVERSITY) if (rxCc2500SpiConfig()->antSelIoTag) { @@ -159,6 +157,8 @@ bool cc2500SpiInit(void) IOConfigGPIO(antSelPin, IOCFG_OUT_PP); IOHi(antSelPin); + } else { + antSelPin = IO_NONE; } #endif #endif // USE_RX_CC2500_SPI_PA_LNA @@ -167,6 +167,10 @@ bool cc2500SpiInit(void) cc2500TxDisable(); #endif // USE_RX_CC2500_SPI_PA_LNA + if (rssiSource == RSSI_SOURCE_NONE) { + rssiSource = RSSI_SOURCE_RX_PROTOCOL; + } + return true; } #endif diff --git a/src/main/target/STM32F411/target.h b/src/main/target/STM32F411/target.h index a5499b7ce7..22cb65c7f7 100644 --- a/src/main/target/STM32F411/target.h +++ b/src/main/target/STM32F411/target.h @@ -110,8 +110,7 @@ #define USE_RX_CC2500_SPI_PA_LNA #define USE_RX_CC2500_SPI_DIVERSITY -//TODO: Make this work with runtime configurability -//#define USE_RX_FLYSKY +#define USE_RX_FLYSKY //TODO: Make this work with runtime configurability //#define USE_RX_SPEKTRUM From 1c05c7ead68b05ab28dd7c750596fd09353ab263 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 3 Mar 2019 00:24:54 +1300 Subject: [PATCH 3/3] Added Spektrum SPI driver. --- src/main/drivers/rx/rx_a7105.c | 5 +++-- src/main/drivers/rx/rx_cyrf6936.c | 6 +++--- src/main/drivers/rx/rx_cyrf6936.h | 4 ++-- src/main/pg/rx_spi_cc2500.h | 2 +- src/main/rx/cc2500_common.c | 8 ++++---- src/main/rx/cyrf6936_spektrum.c | 9 +++++++-- src/main/target/CJMCU/target.h | 2 +- src/main/target/CRAZYBEEF3FR/target.h | 2 +- src/main/target/EACHIF3/target.h | 2 +- src/main/target/MATEKF411RX/target.h | 2 +- src/main/target/OMNIBUSF4/target.h | 2 +- src/main/target/STM32F411/target.h | 5 ++--- src/main/target/STM32F411/target.mk | 6 ++++-- src/test/unit/rx_spi_spektrum_unittest.cc | 20 ++++++++++++-------- unified_targets/configs/CRAZYBEEF4FR.config | 3 +++ 15 files changed, 46 insertions(+), 32 deletions(-) diff --git a/src/main/drivers/rx/rx_a7105.c b/src/main/drivers/rx/rx_a7105.c index cd9ce7d91e..b67d850106 100644 --- a/src/main/drivers/rx/rx_a7105.c +++ b/src/main/drivers/rx/rx_a7105.c @@ -56,14 +56,15 @@ void A7105Init(uint32_t id, IO_t extiPin, IO_t txEnPin) { spiDeviceByInstance(RX_SPI_INSTANCE); rxIntIO = extiPin; /* config receiver IRQ pin */ - IOInit(rxIntIO, OWNER_RX_SPI, 0); + IOInit(rxIntIO, OWNER_RX_SPI_EXTI, 0); EXTIHandlerInit(&a7105extiCallbackRec, a7105extiHandler); EXTIConfig(rxIntIO, &a7105extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IPD, EXTI_TRIGGER_RISING); EXTIEnable(rxIntIO, false); if (txEnPin) { txEnIO = txEnPin; - IOInit(txEnIO, OWNER_RX_SPI_CS, 0); + //TODO: Create resource for this if it ever gets used + IOInit(txEnIO, OWNER_RX_SPI_CC2500_TX_EN, 0); IOConfigGPIO(txEnIO, IOCFG_OUT_PP); } else { txEnIO = IO_NONE; diff --git a/src/main/drivers/rx/rx_cyrf6936.c b/src/main/drivers/rx/rx_cyrf6936.c index 5b188d7033..92ea0f9f3c 100644 --- a/src/main/drivers/rx/rx_cyrf6936.c +++ b/src/main/drivers/rx/rx_cyrf6936.c @@ -69,11 +69,11 @@ bool cyrf6936RxFinished(uint32_t *timeStamp) return false; } -bool cyrf6936Init(void) +bool cyrf6936Init(IO_t extiPin) { spiDeviceByInstance(RX_SPI_INSTANCE); - rxIntIO = IOGetByTag(IO_TAG(RX_EXTI_PIN)); - IOInit(rxIntIO, OWNER_RX_SPI_CS, 0); + rxIntIO = extiPin; + IOInit(rxIntIO, OWNER_RX_SPI_EXTI, 0); EXTIHandlerInit(&cyrf6936extiCallbackRec, cyrf6936ExtiHandler); EXTIConfig(rxIntIO, &cyrf6936extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IPD, EXTI_TRIGGER_FALLING); EXTIEnable(rxIntIO, false); diff --git a/src/main/drivers/rx/rx_cyrf6936.h b/src/main/drivers/rx/rx_cyrf6936.h index 99a2c728a9..b8b8d54862 100644 --- a/src/main/drivers/rx/rx_cyrf6936.h +++ b/src/main/drivers/rx/rx_cyrf6936.h @@ -203,7 +203,7 @@ enum { extern volatile bool isError; -bool cyrf6936Init(void); +bool cyrf6936Init(IO_t extiPin); bool cyrf6936RxFinished(uint32_t *timeStamp); @@ -223,4 +223,4 @@ void cyrf6936SetDataCode(const uint8_t *datacode); void cyrf6936SendLen(const uint8_t *data, const uint8_t length); void cyrf6936StartRecv(void); -void cyrf6936RecvLen(uint8_t *data, const uint8_t length); \ No newline at end of file +void cyrf6936RecvLen(uint8_t *data, const uint8_t length); diff --git a/src/main/pg/rx_spi_cc2500.h b/src/main/pg/rx_spi_cc2500.h index eb8beeb8a0..e76d0522fd 100644 --- a/src/main/pg/rx_spi_cc2500.h +++ b/src/main/pg/rx_spi_cc2500.h @@ -30,7 +30,7 @@ typedef enum { FRSKY_SPI_A1_SOURCE_CONST } frSkySpiA1Source_e; -typedef struct rxFrSkySpiConfig_s { +typedef struct rxCc2500SpiConfig_s { uint8_t autoBind; uint8_t bindTxId[2]; int8_t bindOffset; diff --git a/src/main/rx/cc2500_common.c b/src/main/rx/cc2500_common.c index 9acc798a65..e9594672cf 100755 --- a/src/main/rx/cc2500_common.c +++ b/src/main/rx/cc2500_common.c @@ -133,19 +133,19 @@ bool cc2500SpiInit(void) return false; } - IOInit(gdoPin, OWNER_RX_SPI, 0); + IOInit(gdoPin, OWNER_RX_SPI_EXTI, 0); IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING); #if defined(USE_RX_CC2500_SPI_PA_LNA) if (rxCc2500SpiConfig()->lnaEnIoTag) { rxLnaEnPin = IOGetByTag(rxCc2500SpiConfig()->lnaEnIoTag); - IOInit(rxLnaEnPin, OWNER_RX_SPI, 0); + IOInit(rxLnaEnPin, OWNER_RX_SPI_CC2500_LNA_EN, 0); IOConfigGPIO(rxLnaEnPin, IOCFG_OUT_PP); IOHi(rxLnaEnPin); // always on at the moment } if (rxCc2500SpiConfig()->txEnIoTag) { txEnPin = IOGetByTag(rxCc2500SpiConfig()->txEnIoTag); - IOInit(txEnPin, OWNER_RX_SPI, 0); + IOInit(txEnPin, OWNER_RX_SPI_CC2500_TX_EN, 0); IOConfigGPIO(txEnPin, IOCFG_OUT_PP); } else { txEnPin = IO_NONE; @@ -153,7 +153,7 @@ bool cc2500SpiInit(void) #if defined(USE_RX_CC2500_SPI_DIVERSITY) if (rxCc2500SpiConfig()->antSelIoTag) { antSelPin = IOGetByTag(rxCc2500SpiConfig()->antSelIoTag); - IOInit(antSelPin, OWNER_RX_SPI, 0); + IOInit(antSelPin, OWNER_RX_SPI_CC2500_ANT_SEL, 0); IOConfigGPIO(antSelPin, IOCFG_OUT_PP); IOHi(antSelPin); diff --git a/src/main/rx/cyrf6936_spektrum.c b/src/main/rx/cyrf6936_spektrum.c index 2dca084925..d54131d383 100644 --- a/src/main/rx/cyrf6936_spektrum.c +++ b/src/main/rx/cyrf6936_spektrum.c @@ -374,11 +374,16 @@ static void dsmReceiverStartTransfer(void) bool spektrumSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig) { + IO_t extiPin = IOGetByTag(rxConfig->extiIoTag); + if (!extiPin) { + return false; + } + rxSpiCommonIOInit(rxConfig); rxRuntimeConfig->channelCount = DSM_MAX_CHANNEL_COUNT; - if (!cyrf6936Init()) { + if (!cyrf6936Init(extiPin)) { return false; } @@ -607,4 +612,4 @@ rx_spi_received_e spektrumSpiDataReceived(uint8_t *payload) return result; } -#endif /* USE_RX_SPEKTRUM */ \ No newline at end of file +#endif /* USE_RX_SPEKTRUM */ diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 27553ea5b4..d231da4a06 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -67,7 +67,7 @@ // Nordic Semiconductor uses 'CSN', STM uses 'NSS' #define RX_CE_PIN PA4 #define RX_NSS_PIN PA11 -#define RX_EXTI_PIN PA8 +#define RX_SPI_EXTI_PIN PA8 // CJMCU has NSS on PA11, rather than the standard PA4 #define SPI1_NSS_PIN RX_NSS_PIN diff --git a/src/main/target/CRAZYBEEF3FR/target.h b/src/main/target/CRAZYBEEF3FR/target.h index c890e698e0..f6d167ec18 100644 --- a/src/main/target/CRAZYBEEF3FR/target.h +++ b/src/main/target/CRAZYBEEF3FR/target.h @@ -117,7 +117,7 @@ #define FLYSKY_2A_CHANNEL_COUNT 14 #define RX_SPI_INSTANCE SPI2 #define RX_NSS_PIN SPI2_NSS_PIN -#define RX_EXTI_PIN PA8 +#define RX_SPI_EXTI_PIN PA8 #define BINDPLUG_PIN PA9 #define RX_SPI_LED_PIN PA10 #define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_MOTOR_STOP) diff --git a/src/main/target/EACHIF3/target.h b/src/main/target/EACHIF3/target.h index 5af0dbdcd5..07ee8f846a 100644 --- a/src/main/target/EACHIF3/target.h +++ b/src/main/target/EACHIF3/target.h @@ -58,7 +58,7 @@ #define FLYSKY_2A_CHANNEL_COUNT 10 #define RX_SPI_INSTANCE SPI2 -#define RX_EXTI_PIN PB12 +#define RX_SPI_EXTI_PIN PB12 #define SPI2_NSS_PIN PA4 #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 diff --git a/src/main/target/MATEKF411RX/target.h b/src/main/target/MATEKF411RX/target.h index a5d2d80e91..133538d0d1 100644 --- a/src/main/target/MATEKF411RX/target.h +++ b/src/main/target/MATEKF411RX/target.h @@ -106,7 +106,7 @@ #define RX_CHANNELS_AETR #define RX_SPI_DEFAULT_PROTOCOL RX_SPI_A7105_FLYSKY_2A #define FLYSKY_2A_CHANNEL_COUNT 14 -#define RX_EXTI_PIN PA14 +#define RX_SPI_EXTI_PIN PA14 #define USE_RX_FLYSKY_SPI_LED #define RX_SPI_LED_PIN PB9 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index e2d0e43f99..c10e25cc97 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -181,7 +181,7 @@ #define RX_SPI_DEFAULT_PROTOCOL RX_SPI_CYRF6936_DSM #define RX_SPI_INSTANCE SPI3 #define RX_NSS_PIN PD2 -#define RX_EXTI_PIN PA0 // instead of rssi input +#define RX_SPI_EXTI_PIN PA0 // instead of rssi input #endif #define USE_VCP diff --git a/src/main/target/STM32F411/target.h b/src/main/target/STM32F411/target.h index 22cb65c7f7..8ff794f62e 100644 --- a/src/main/target/STM32F411/target.h +++ b/src/main/target/STM32F411/target.h @@ -112,9 +112,8 @@ #define USE_RX_FLYSKY -//TODO: Make this work with runtime configurability -//#define USE_RX_SPEKTRUM -//#define USE_RX_SPEKTRUM_TELEMETRY +#define USE_RX_SPEKTRUM +#define USE_RX_SPEKTRUM_TELEMETRY #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff diff --git a/src/main/target/STM32F411/target.mk b/src/main/target/STM32F411/target.mk index 47a7d89b33..a20da33d03 100644 --- a/src/main/target/STM32F411/target.mk +++ b/src/main/target/STM32F411/target.mk @@ -7,11 +7,13 @@ TARGET_SRC = \ $(addprefix drivers/barometer/,$(notdir $(wildcard $(SRC_DIR)/drivers/barometer/*.c))) \ $(addprefix drivers/compass/,$(notdir $(wildcard $(SRC_DIR)/drivers/compass/*.c))) \ drivers/max7456.c \ - drivers/rx/rx_cc2500.c \ rx/cc2500_common.c \ rx/cc2500_frsky_shared.c \ rx/cc2500_frsky_d.c \ rx/cc2500_frsky_x.c \ rx/cc2500_sfhss.c \ + rx/a7105_flysky.c \ + rx/cyrf6936_spektrum.c \ + drivers/rx/rx_cc2500.c \ drivers/rx/rx_a7105.c \ - rx/a7105_flysky.c + drivers/rx/rx_cyrf6936.c diff --git a/src/test/unit/rx_spi_spektrum_unittest.cc b/src/test/unit/rx_spi_spektrum_unittest.cc index 6b0045e24c..f01ed597ff 100644 --- a/src/test/unit/rx_spi_spektrum_unittest.cc +++ b/src/test/unit/rx_spi_spektrum_unittest.cc @@ -23,6 +23,8 @@ extern "C" { #include "platform.h" + #include "drivers/io.h" + #include "pg/pg.h" #include "pg/pg_ids.h" #include "pg/rx_spi.h" @@ -127,6 +129,10 @@ extern "C" { pkt[i * 2 + 1] = (value >> 0) & 0xff; } } + + static const rxSpiConfig_t injectedConfig = { + .extiIoTag = IO_TAG(PA0), + }; } #include "unittest_macros.h" @@ -136,7 +142,7 @@ extern "C" { TEST(RxSpiSpektrumUnitTest, TestInitUnbound) { dsmReceiver = empty; - spektrumSpiInit(nullptr, &config); + spektrumSpiInit(&injectedConfig, &config); EXPECT_FALSE(dsmReceiver.bound); EXPECT_EQ(DSM_RECEIVER_BIND, dsmReceiver.status); EXPECT_EQ(DSM_INITIAL_BIND_CHANNEL, dsmReceiver.rfChannel); @@ -154,7 +160,7 @@ TEST(RxSpiSpektrumUnitTest, TestInitBound) spektrumConfigMutable()->protocol = DSMX_11; - bool result = spektrumSpiInit(nullptr, &config); + bool result = spektrumSpiInit(&injectedConfig, &config); EXPECT_TRUE(result); EXPECT_TRUE(dsmReceiver.bound); @@ -174,7 +180,7 @@ TEST(RxSpiSpektrumUnitTest, TestInitBound) dsmReceiver = empty; spektrumConfigMutable()->protocol = DSM2_11; - spektrumSpiInit(nullptr, &config); + spektrumSpiInit(&injectedConfig, &config); EXPECT_TRUE(dsmReceiver.bound); EXPECT_EQ(DSM2_11, dsmReceiver.protocol); @@ -340,15 +346,13 @@ extern "C" { uint32_t micros(void) { return 0; } uint32_t millis(void) { return 0; } - void IOConfigGPIO(void) {} bool IORead(IO_t ) { return true; } - void IOInit(void) {} - IO_t IOGetByTag(ioTag_t ) { return IO_NONE; } + IO_t IOGetByTag(ioTag_t ) { return (IO_t)1; } void IOHi(IO_t ) {} void IOLo(IO_t ) {} void writeEEPROM(void) {} - bool cyrf6936Init(void) { return true; } + bool cyrf6936Init(IO_t ) { return true; } bool cyrf6936RxFinished (uint32_t *timestamp) { *timestamp = 0; @@ -388,4 +392,4 @@ extern "C" { { return false; } -} \ No newline at end of file +} diff --git a/unified_targets/configs/CRAZYBEEF4FR.config b/unified_targets/configs/CRAZYBEEF4FR.config index c388d23be4..0123da36d2 100644 --- a/unified_targets/configs/CRAZYBEEF4FR.config +++ b/unified_targets/configs/CRAZYBEEF4FR.config @@ -69,6 +69,7 @@ dmaopt pin A10 0 # pin A10: DMA2 Stream 6 Channel 0 # feature +feature OSD feature RX_SPI # master @@ -79,6 +80,7 @@ set adc_device = 1 set motor_pwm_protocol = DSHOT600 set current_meter = ADC set battery_meter = ADC +set ibata_scale = 179 set beeper_inversion = ON set beeper_od = OFF set system_hse_mhz = 8 @@ -86,3 +88,4 @@ set max7456_spi_bus = 2 set cc2500_spi_chip_detect = ON set gyro_1_bustype = SPI set gyro_1_spibus = 1 +set gyro_1_sensor_align = CW90