mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 22:35:23 +03:00
Added Spektrum SPI driver.
This commit is contained in:
parent
96d5b5dcac
commit
1c05c7ead6
15 changed files with 46 additions and 32 deletions
|
@ -56,14 +56,15 @@ void A7105Init(uint32_t id, IO_t extiPin, IO_t txEnPin)
|
||||||
{
|
{
|
||||||
spiDeviceByInstance(RX_SPI_INSTANCE);
|
spiDeviceByInstance(RX_SPI_INSTANCE);
|
||||||
rxIntIO = extiPin; /* config receiver IRQ pin */
|
rxIntIO = extiPin; /* config receiver IRQ pin */
|
||||||
IOInit(rxIntIO, OWNER_RX_SPI, 0);
|
IOInit(rxIntIO, OWNER_RX_SPI_EXTI, 0);
|
||||||
EXTIHandlerInit(&a7105extiCallbackRec, a7105extiHandler);
|
EXTIHandlerInit(&a7105extiCallbackRec, a7105extiHandler);
|
||||||
EXTIConfig(rxIntIO, &a7105extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IPD, EXTI_TRIGGER_RISING);
|
EXTIConfig(rxIntIO, &a7105extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IPD, EXTI_TRIGGER_RISING);
|
||||||
EXTIEnable(rxIntIO, false);
|
EXTIEnable(rxIntIO, false);
|
||||||
|
|
||||||
if (txEnPin) {
|
if (txEnPin) {
|
||||||
txEnIO = 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);
|
IOConfigGPIO(txEnIO, IOCFG_OUT_PP);
|
||||||
} else {
|
} else {
|
||||||
txEnIO = IO_NONE;
|
txEnIO = IO_NONE;
|
||||||
|
|
|
@ -69,11 +69,11 @@ bool cyrf6936RxFinished(uint32_t *timeStamp)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool cyrf6936Init(void)
|
bool cyrf6936Init(IO_t extiPin)
|
||||||
{
|
{
|
||||||
spiDeviceByInstance(RX_SPI_INSTANCE);
|
spiDeviceByInstance(RX_SPI_INSTANCE);
|
||||||
rxIntIO = IOGetByTag(IO_TAG(RX_EXTI_PIN));
|
rxIntIO = extiPin;
|
||||||
IOInit(rxIntIO, OWNER_RX_SPI_CS, 0);
|
IOInit(rxIntIO, OWNER_RX_SPI_EXTI, 0);
|
||||||
EXTIHandlerInit(&cyrf6936extiCallbackRec, cyrf6936ExtiHandler);
|
EXTIHandlerInit(&cyrf6936extiCallbackRec, cyrf6936ExtiHandler);
|
||||||
EXTIConfig(rxIntIO, &cyrf6936extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IPD, EXTI_TRIGGER_FALLING);
|
EXTIConfig(rxIntIO, &cyrf6936extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IPD, EXTI_TRIGGER_FALLING);
|
||||||
EXTIEnable(rxIntIO, false);
|
EXTIEnable(rxIntIO, false);
|
||||||
|
|
|
@ -203,7 +203,7 @@ enum {
|
||||||
|
|
||||||
extern volatile bool isError;
|
extern volatile bool isError;
|
||||||
|
|
||||||
bool cyrf6936Init(void);
|
bool cyrf6936Init(IO_t extiPin);
|
||||||
|
|
||||||
bool cyrf6936RxFinished(uint32_t *timeStamp);
|
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 cyrf6936SendLen(const uint8_t *data, const uint8_t length);
|
||||||
void cyrf6936StartRecv(void);
|
void cyrf6936StartRecv(void);
|
||||||
void cyrf6936RecvLen(uint8_t *data, const uint8_t length);
|
void cyrf6936RecvLen(uint8_t *data, const uint8_t length);
|
||||||
|
|
|
@ -30,7 +30,7 @@ typedef enum {
|
||||||
FRSKY_SPI_A1_SOURCE_CONST
|
FRSKY_SPI_A1_SOURCE_CONST
|
||||||
} frSkySpiA1Source_e;
|
} frSkySpiA1Source_e;
|
||||||
|
|
||||||
typedef struct rxFrSkySpiConfig_s {
|
typedef struct rxCc2500SpiConfig_s {
|
||||||
uint8_t autoBind;
|
uint8_t autoBind;
|
||||||
uint8_t bindTxId[2];
|
uint8_t bindTxId[2];
|
||||||
int8_t bindOffset;
|
int8_t bindOffset;
|
||||||
|
|
|
@ -133,19 +133,19 @@ bool cc2500SpiInit(void)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
IOInit(gdoPin, OWNER_RX_SPI, 0);
|
IOInit(gdoPin, OWNER_RX_SPI_EXTI, 0);
|
||||||
IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING);
|
IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING);
|
||||||
#if defined(USE_RX_CC2500_SPI_PA_LNA)
|
#if defined(USE_RX_CC2500_SPI_PA_LNA)
|
||||||
if (rxCc2500SpiConfig()->lnaEnIoTag) {
|
if (rxCc2500SpiConfig()->lnaEnIoTag) {
|
||||||
rxLnaEnPin = IOGetByTag(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);
|
IOConfigGPIO(rxLnaEnPin, IOCFG_OUT_PP);
|
||||||
|
|
||||||
IOHi(rxLnaEnPin); // always on at the moment
|
IOHi(rxLnaEnPin); // always on at the moment
|
||||||
}
|
}
|
||||||
if (rxCc2500SpiConfig()->txEnIoTag) {
|
if (rxCc2500SpiConfig()->txEnIoTag) {
|
||||||
txEnPin = IOGetByTag(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);
|
IOConfigGPIO(txEnPin, IOCFG_OUT_PP);
|
||||||
} else {
|
} else {
|
||||||
txEnPin = IO_NONE;
|
txEnPin = IO_NONE;
|
||||||
|
@ -153,7 +153,7 @@ bool cc2500SpiInit(void)
|
||||||
#if defined(USE_RX_CC2500_SPI_DIVERSITY)
|
#if defined(USE_RX_CC2500_SPI_DIVERSITY)
|
||||||
if (rxCc2500SpiConfig()->antSelIoTag) {
|
if (rxCc2500SpiConfig()->antSelIoTag) {
|
||||||
antSelPin = IOGetByTag(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);
|
IOConfigGPIO(antSelPin, IOCFG_OUT_PP);
|
||||||
|
|
||||||
IOHi(antSelPin);
|
IOHi(antSelPin);
|
||||||
|
|
|
@ -374,11 +374,16 @@ static void dsmReceiverStartTransfer(void)
|
||||||
|
|
||||||
bool spektrumSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig)
|
bool spektrumSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig)
|
||||||
{
|
{
|
||||||
|
IO_t extiPin = IOGetByTag(rxConfig->extiIoTag);
|
||||||
|
if (!extiPin) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
rxSpiCommonIOInit(rxConfig);
|
rxSpiCommonIOInit(rxConfig);
|
||||||
|
|
||||||
rxRuntimeConfig->channelCount = DSM_MAX_CHANNEL_COUNT;
|
rxRuntimeConfig->channelCount = DSM_MAX_CHANNEL_COUNT;
|
||||||
|
|
||||||
if (!cyrf6936Init()) {
|
if (!cyrf6936Init(extiPin)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -607,4 +612,4 @@ rx_spi_received_e spektrumSpiDataReceived(uint8_t *payload)
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif /* USE_RX_SPEKTRUM */
|
#endif /* USE_RX_SPEKTRUM */
|
||||||
|
|
|
@ -67,7 +67,7 @@
|
||||||
// Nordic Semiconductor uses 'CSN', STM uses 'NSS'
|
// Nordic Semiconductor uses 'CSN', STM uses 'NSS'
|
||||||
#define RX_CE_PIN PA4
|
#define RX_CE_PIN PA4
|
||||||
#define RX_NSS_PIN PA11
|
#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
|
// CJMCU has NSS on PA11, rather than the standard PA4
|
||||||
#define SPI1_NSS_PIN RX_NSS_PIN
|
#define SPI1_NSS_PIN RX_NSS_PIN
|
||||||
|
|
||||||
|
|
|
@ -117,7 +117,7 @@
|
||||||
#define FLYSKY_2A_CHANNEL_COUNT 14
|
#define FLYSKY_2A_CHANNEL_COUNT 14
|
||||||
#define RX_SPI_INSTANCE SPI2
|
#define RX_SPI_INSTANCE SPI2
|
||||||
#define RX_NSS_PIN SPI2_NSS_PIN
|
#define RX_NSS_PIN SPI2_NSS_PIN
|
||||||
#define RX_EXTI_PIN PA8
|
#define RX_SPI_EXTI_PIN PA8
|
||||||
#define BINDPLUG_PIN PA9
|
#define BINDPLUG_PIN PA9
|
||||||
#define RX_SPI_LED_PIN PA10
|
#define RX_SPI_LED_PIN PA10
|
||||||
#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_MOTOR_STOP)
|
#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_MOTOR_STOP)
|
||||||
|
|
|
@ -58,7 +58,7 @@
|
||||||
#define FLYSKY_2A_CHANNEL_COUNT 10
|
#define FLYSKY_2A_CHANNEL_COUNT 10
|
||||||
|
|
||||||
#define RX_SPI_INSTANCE SPI2
|
#define RX_SPI_INSTANCE SPI2
|
||||||
#define RX_EXTI_PIN PB12
|
#define RX_SPI_EXTI_PIN PB12
|
||||||
#define SPI2_NSS_PIN PA4
|
#define SPI2_NSS_PIN PA4
|
||||||
#define SPI2_SCK_PIN PB13
|
#define SPI2_SCK_PIN PB13
|
||||||
#define SPI2_MISO_PIN PB14
|
#define SPI2_MISO_PIN PB14
|
||||||
|
|
|
@ -106,7 +106,7 @@
|
||||||
#define RX_CHANNELS_AETR
|
#define RX_CHANNELS_AETR
|
||||||
#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_A7105_FLYSKY_2A
|
#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_A7105_FLYSKY_2A
|
||||||
#define FLYSKY_2A_CHANNEL_COUNT 14
|
#define FLYSKY_2A_CHANNEL_COUNT 14
|
||||||
#define RX_EXTI_PIN PA14
|
#define RX_SPI_EXTI_PIN PA14
|
||||||
#define USE_RX_FLYSKY_SPI_LED
|
#define USE_RX_FLYSKY_SPI_LED
|
||||||
#define RX_SPI_LED_PIN PB9
|
#define RX_SPI_LED_PIN PB9
|
||||||
|
|
||||||
|
|
|
@ -181,7 +181,7 @@
|
||||||
#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_CYRF6936_DSM
|
#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_CYRF6936_DSM
|
||||||
#define RX_SPI_INSTANCE SPI3
|
#define RX_SPI_INSTANCE SPI3
|
||||||
#define RX_NSS_PIN PD2
|
#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
|
#endif
|
||||||
|
|
||||||
#define USE_VCP
|
#define USE_VCP
|
||||||
|
|
|
@ -112,9 +112,8 @@
|
||||||
|
|
||||||
#define USE_RX_FLYSKY
|
#define USE_RX_FLYSKY
|
||||||
|
|
||||||
//TODO: Make this work with runtime configurability
|
#define USE_RX_SPEKTRUM
|
||||||
//#define USE_RX_SPEKTRUM
|
#define USE_RX_SPEKTRUM_TELEMETRY
|
||||||
//#define USE_RX_SPEKTRUM_TELEMETRY
|
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
#define TARGET_IO_PORTB 0xffff
|
#define TARGET_IO_PORTB 0xffff
|
||||||
|
|
|
@ -7,11 +7,13 @@ TARGET_SRC = \
|
||||||
$(addprefix drivers/barometer/,$(notdir $(wildcard $(SRC_DIR)/drivers/barometer/*.c))) \
|
$(addprefix drivers/barometer/,$(notdir $(wildcard $(SRC_DIR)/drivers/barometer/*.c))) \
|
||||||
$(addprefix drivers/compass/,$(notdir $(wildcard $(SRC_DIR)/drivers/compass/*.c))) \
|
$(addprefix drivers/compass/,$(notdir $(wildcard $(SRC_DIR)/drivers/compass/*.c))) \
|
||||||
drivers/max7456.c \
|
drivers/max7456.c \
|
||||||
drivers/rx/rx_cc2500.c \
|
|
||||||
rx/cc2500_common.c \
|
rx/cc2500_common.c \
|
||||||
rx/cc2500_frsky_shared.c \
|
rx/cc2500_frsky_shared.c \
|
||||||
rx/cc2500_frsky_d.c \
|
rx/cc2500_frsky_d.c \
|
||||||
rx/cc2500_frsky_x.c \
|
rx/cc2500_frsky_x.c \
|
||||||
rx/cc2500_sfhss.c \
|
rx/cc2500_sfhss.c \
|
||||||
|
rx/a7105_flysky.c \
|
||||||
|
rx/cyrf6936_spektrum.c \
|
||||||
|
drivers/rx/rx_cc2500.c \
|
||||||
drivers/rx/rx_a7105.c \
|
drivers/rx/rx_a7105.c \
|
||||||
rx/a7105_flysky.c
|
drivers/rx/rx_cyrf6936.c
|
||||||
|
|
|
@ -23,6 +23,8 @@
|
||||||
extern "C" {
|
extern "C" {
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
#include "drivers/io.h"
|
||||||
|
|
||||||
#include "pg/pg.h"
|
#include "pg/pg.h"
|
||||||
#include "pg/pg_ids.h"
|
#include "pg/pg_ids.h"
|
||||||
#include "pg/rx_spi.h"
|
#include "pg/rx_spi.h"
|
||||||
|
@ -127,6 +129,10 @@ extern "C" {
|
||||||
pkt[i * 2 + 1] = (value >> 0) & 0xff;
|
pkt[i * 2 + 1] = (value >> 0) & 0xff;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static const rxSpiConfig_t injectedConfig = {
|
||||||
|
.extiIoTag = IO_TAG(PA0),
|
||||||
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
#include "unittest_macros.h"
|
#include "unittest_macros.h"
|
||||||
|
@ -136,7 +142,7 @@ extern "C" {
|
||||||
TEST(RxSpiSpektrumUnitTest, TestInitUnbound)
|
TEST(RxSpiSpektrumUnitTest, TestInitUnbound)
|
||||||
{
|
{
|
||||||
dsmReceiver = empty;
|
dsmReceiver = empty;
|
||||||
spektrumSpiInit(nullptr, &config);
|
spektrumSpiInit(&injectedConfig, &config);
|
||||||
EXPECT_FALSE(dsmReceiver.bound);
|
EXPECT_FALSE(dsmReceiver.bound);
|
||||||
EXPECT_EQ(DSM_RECEIVER_BIND, dsmReceiver.status);
|
EXPECT_EQ(DSM_RECEIVER_BIND, dsmReceiver.status);
|
||||||
EXPECT_EQ(DSM_INITIAL_BIND_CHANNEL, dsmReceiver.rfChannel);
|
EXPECT_EQ(DSM_INITIAL_BIND_CHANNEL, dsmReceiver.rfChannel);
|
||||||
|
@ -154,7 +160,7 @@ TEST(RxSpiSpektrumUnitTest, TestInitBound)
|
||||||
|
|
||||||
spektrumConfigMutable()->protocol = DSMX_11;
|
spektrumConfigMutable()->protocol = DSMX_11;
|
||||||
|
|
||||||
bool result = spektrumSpiInit(nullptr, &config);
|
bool result = spektrumSpiInit(&injectedConfig, &config);
|
||||||
|
|
||||||
EXPECT_TRUE(result);
|
EXPECT_TRUE(result);
|
||||||
EXPECT_TRUE(dsmReceiver.bound);
|
EXPECT_TRUE(dsmReceiver.bound);
|
||||||
|
@ -174,7 +180,7 @@ TEST(RxSpiSpektrumUnitTest, TestInitBound)
|
||||||
dsmReceiver = empty;
|
dsmReceiver = empty;
|
||||||
spektrumConfigMutable()->protocol = DSM2_11;
|
spektrumConfigMutable()->protocol = DSM2_11;
|
||||||
|
|
||||||
spektrumSpiInit(nullptr, &config);
|
spektrumSpiInit(&injectedConfig, &config);
|
||||||
|
|
||||||
EXPECT_TRUE(dsmReceiver.bound);
|
EXPECT_TRUE(dsmReceiver.bound);
|
||||||
EXPECT_EQ(DSM2_11, dsmReceiver.protocol);
|
EXPECT_EQ(DSM2_11, dsmReceiver.protocol);
|
||||||
|
@ -340,15 +346,13 @@ extern "C" {
|
||||||
uint32_t micros(void) { return 0; }
|
uint32_t micros(void) { return 0; }
|
||||||
uint32_t millis(void) { return 0; }
|
uint32_t millis(void) { return 0; }
|
||||||
|
|
||||||
void IOConfigGPIO(void) {}
|
|
||||||
bool IORead(IO_t ) { return true; }
|
bool IORead(IO_t ) { return true; }
|
||||||
void IOInit(void) {}
|
IO_t IOGetByTag(ioTag_t ) { return (IO_t)1; }
|
||||||
IO_t IOGetByTag(ioTag_t ) { return IO_NONE; }
|
|
||||||
void IOHi(IO_t ) {}
|
void IOHi(IO_t ) {}
|
||||||
void IOLo(IO_t ) {}
|
void IOLo(IO_t ) {}
|
||||||
void writeEEPROM(void) {}
|
void writeEEPROM(void) {}
|
||||||
|
|
||||||
bool cyrf6936Init(void) { return true; }
|
bool cyrf6936Init(IO_t ) { return true; }
|
||||||
bool cyrf6936RxFinished (uint32_t *timestamp)
|
bool cyrf6936RxFinished (uint32_t *timestamp)
|
||||||
{
|
{
|
||||||
*timestamp = 0;
|
*timestamp = 0;
|
||||||
|
@ -388,4 +392,4 @@ extern "C" {
|
||||||
{
|
{
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -69,6 +69,7 @@ dmaopt pin A10 0
|
||||||
# pin A10: DMA2 Stream 6 Channel 0
|
# pin A10: DMA2 Stream 6 Channel 0
|
||||||
|
|
||||||
# feature
|
# feature
|
||||||
|
feature OSD
|
||||||
feature RX_SPI
|
feature RX_SPI
|
||||||
|
|
||||||
# master
|
# master
|
||||||
|
@ -79,6 +80,7 @@ set adc_device = 1
|
||||||
set motor_pwm_protocol = DSHOT600
|
set motor_pwm_protocol = DSHOT600
|
||||||
set current_meter = ADC
|
set current_meter = ADC
|
||||||
set battery_meter = ADC
|
set battery_meter = ADC
|
||||||
|
set ibata_scale = 179
|
||||||
set beeper_inversion = ON
|
set beeper_inversion = ON
|
||||||
set beeper_od = OFF
|
set beeper_od = OFF
|
||||||
set system_hse_mhz = 8
|
set system_hse_mhz = 8
|
||||||
|
@ -86,3 +88,4 @@ set max7456_spi_bus = 2
|
||||||
set cc2500_spi_chip_detect = ON
|
set cc2500_spi_chip_detect = ON
|
||||||
set gyro_1_bustype = SPI
|
set gyro_1_bustype = SPI
|
||||||
set gyro_1_spibus = 1
|
set gyro_1_spibus = 1
|
||||||
|
set gyro_1_sensor_align = CW90
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue