mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-17 05:15:25 +03:00
Unified the handling of the EXTI pin for SPI RX.
This commit is contained in:
parent
dad2b244e9
commit
0dcdeaa105
33 changed files with 203 additions and 124 deletions
|
@ -27,39 +27,17 @@
|
||||||
#ifdef USE_RX_FLYSKY
|
#ifdef USE_RX_FLYSKY
|
||||||
|
|
||||||
#include "drivers/bus_spi.h"
|
#include "drivers/bus_spi.h"
|
||||||
#include "drivers/exti.h"
|
|
||||||
#include "drivers/io.h"
|
#include "drivers/io.h"
|
||||||
#include "drivers/io_impl.h"
|
#include "drivers/io_impl.h"
|
||||||
#include "drivers/nvic.h"
|
|
||||||
#include "drivers/rx/rx_a7105.h"
|
#include "drivers/rx/rx_a7105.h"
|
||||||
#include "drivers/rx/rx_spi.h"
|
#include "drivers/rx/rx_spi.h"
|
||||||
#include "drivers/time.h"
|
|
||||||
|
|
||||||
static IO_t txEnIO = IO_NONE;
|
static IO_t txEnIO = IO_NONE;
|
||||||
|
|
||||||
static IO_t rxIntIO = IO_NONE;
|
static bool consumeExti = true;
|
||||||
static extiCallbackRec_t a7105extiCallbackRec;
|
|
||||||
static volatile uint32_t timeEvent = 0;
|
|
||||||
static volatile bool occurEvent = false;
|
|
||||||
|
|
||||||
void a7105extiHandler(extiCallbackRec_t* cb)
|
void A7105Init(uint32_t id, IO_t txEnPin)
|
||||||
{
|
{
|
||||||
UNUSED(cb);
|
|
||||||
|
|
||||||
if (IORead(rxIntIO) != 0) {
|
|
||||||
timeEvent = micros();
|
|
||||||
occurEvent = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void A7105Init(uint32_t id, IO_t extiPin, IO_t txEnPin)
|
|
||||||
{
|
|
||||||
rxIntIO = extiPin; /* config receiver IRQ pin */
|
|
||||||
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) {
|
if (txEnPin) {
|
||||||
txEnIO = txEnPin;
|
txEnIO = txEnPin;
|
||||||
//TODO: Create resource for this if it ever gets used
|
//TODO: Create resource for this if it ever gets used
|
||||||
|
@ -98,15 +76,16 @@ void A7105Config(const uint8_t *regsTable, uint8_t size)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool A7105RxTxFinished(uint32_t *timeStamp) {
|
bool A7105RxTxFinished(timeUs_t *timeStamp) {
|
||||||
bool result = false;
|
bool result = false;
|
||||||
|
|
||||||
if (occurEvent) {
|
if (consumeExti && rxSpiPollExti()) {
|
||||||
if (timeStamp) {
|
if (rxSpiGetLastExtiTimeUs()) {
|
||||||
*timeStamp = timeEvent;
|
*timeStamp = rxSpiGetLastExtiTimeUs();
|
||||||
}
|
}
|
||||||
|
|
||||||
occurEvent = false;
|
rxSpiResetExti();
|
||||||
|
|
||||||
result = true;
|
result = true;
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
|
@ -130,9 +109,10 @@ void A7105WriteReg(A7105Reg_t reg, uint8_t data)
|
||||||
void A7105Strobe(A7105State_t state)
|
void A7105Strobe(A7105State_t state)
|
||||||
{
|
{
|
||||||
if (A7105_TX == state || A7105_RX == state) {
|
if (A7105_TX == state || A7105_RX == state) {
|
||||||
EXTIEnable(rxIntIO, true);
|
consumeExti = true;
|
||||||
|
rxSpiResetExti();
|
||||||
} else {
|
} else {
|
||||||
EXTIEnable(rxIntIO, false);
|
consumeExti = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (txEnIO) {
|
if (txEnIO) {
|
||||||
|
|
|
@ -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_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.
|
#define A7105_MODE_TRER 0x01 // [0]: TRX is disabled. [1]: TRX is enabled.
|
||||||
|
|
||||||
void A7105Init(uint32_t id, IO_t extiPin, IO_t txEnPin);
|
void A7105Init(uint32_t id, IO_t txEnPin);
|
||||||
void A7105SoftReset(void);
|
void A7105SoftReset(void);
|
||||||
void A7105Config(const uint8_t *regsTable, uint8_t size);
|
void A7105Config(const uint8_t *regsTable, uint8_t size);
|
||||||
|
|
||||||
|
|
|
@ -27,35 +27,18 @@
|
||||||
#ifdef USE_RX_SPEKTRUM
|
#ifdef USE_RX_SPEKTRUM
|
||||||
|
|
||||||
#include "drivers/bus_spi.h"
|
#include "drivers/bus_spi.h"
|
||||||
#include "drivers/exti.h"
|
|
||||||
#include "drivers/io.h"
|
#include "drivers/io.h"
|
||||||
#include "drivers/io_impl.h"
|
#include "drivers/io_impl.h"
|
||||||
#include "drivers/nvic.h"
|
|
||||||
#include "drivers/rx/rx_cyrf6936.h"
|
#include "drivers/rx/rx_cyrf6936.h"
|
||||||
#include "drivers/rx/rx_spi.h"
|
#include "drivers/rx/rx_spi.h"
|
||||||
#include "drivers/time.h"
|
|
||||||
|
|
||||||
static IO_t rxIntIO = IO_NONE;
|
bool isError = false;
|
||||||
static extiCallbackRec_t cyrf6936extiCallbackRec;
|
|
||||||
static volatile uint32_t timeEvent = 0;
|
|
||||||
static volatile bool occurEvent = false;
|
|
||||||
volatile bool isError = false;
|
|
||||||
|
|
||||||
void cyrf6936ExtiHandler(extiCallbackRec_t *cb)
|
bool cyrf6936RxFinished(timeUs_t *timeStamp)
|
||||||
{
|
{
|
||||||
UNUSED(cb);
|
if (rxSpiPollExti()) {
|
||||||
|
if (rxSpiGetLastExtiTimeUs()) {
|
||||||
if (IORead(rxIntIO) == 0) {
|
*timeStamp = rxSpiGetLastExtiTimeUs();
|
||||||
timeEvent = micros();
|
|
||||||
occurEvent = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool cyrf6936RxFinished(uint32_t *timeStamp)
|
|
||||||
{
|
|
||||||
if (occurEvent) {
|
|
||||||
if (timeStamp) {
|
|
||||||
*timeStamp = timeEvent;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t rxIrqStatus = cyrf6936ReadRegister(CYRF6936_RX_IRQ_STATUS);
|
uint8_t rxIrqStatus = cyrf6936ReadRegister(CYRF6936_RX_IRQ_STATUS);
|
||||||
|
@ -63,19 +46,18 @@ bool cyrf6936RxFinished(uint32_t *timeStamp)
|
||||||
isError = (rxIrqStatus & CYRF6936_RXE_IRQ) > 0x0;
|
isError = (rxIrqStatus & CYRF6936_RXE_IRQ) > 0x0;
|
||||||
}
|
}
|
||||||
|
|
||||||
occurEvent = false;
|
rxSpiResetExti();
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool cyrf6936Init(IO_t extiPin)
|
bool cyrf6936Init(void)
|
||||||
{
|
{
|
||||||
rxIntIO = extiPin;
|
if (!rxSpiExtiConfigured()) {
|
||||||
IOInit(rxIntIO, OWNER_RX_SPI_EXTI, 0);
|
return false;
|
||||||
EXTIHandlerInit(&cyrf6936extiCallbackRec, cyrf6936ExtiHandler);
|
}
|
||||||
EXTIConfig(rxIntIO, &cyrf6936extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IPD, EXTI_TRIGGER_FALLING);
|
|
||||||
EXTIEnable(rxIntIO, false);
|
|
||||||
|
|
||||||
uint16_t timeout = 1000;
|
uint16_t timeout = 1000;
|
||||||
do { // Check if chip has waken up
|
do { // Check if chip has waken up
|
||||||
|
@ -168,7 +150,6 @@ void cyrf6936StartRecv(void)
|
||||||
{
|
{
|
||||||
cyrf6936WriteRegister(CYRF6936_RX_IRQ_STATUS, CYRF6936_RXOW_IRQ);
|
cyrf6936WriteRegister(CYRF6936_RX_IRQ_STATUS, CYRF6936_RXOW_IRQ);
|
||||||
cyrf6936WriteRegister(CYRF6936_RX_CTRL, CYRF6936_RX_GO | CYRF6936_RXC_IRQEN | CYRF6936_RXE_IRQEN);
|
cyrf6936WriteRegister(CYRF6936_RX_CTRL, CYRF6936_RX_GO | CYRF6936_RXC_IRQEN | CYRF6936_RXE_IRQEN);
|
||||||
EXTIEnable(rxIntIO, true);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void cyrf6936RecvLen(uint8_t *data, const uint8_t length)
|
void cyrf6936RecvLen(uint8_t *data, const uint8_t length)
|
||||||
|
|
|
@ -201,9 +201,9 @@ enum {
|
||||||
};
|
};
|
||||||
#define CYRF6936_DATA_CODE_LENGTH (1<<5)
|
#define CYRF6936_DATA_CODE_LENGTH (1<<5)
|
||||||
|
|
||||||
extern volatile bool isError;
|
extern bool isError;
|
||||||
|
|
||||||
bool cyrf6936Init(IO_t extiPin);
|
bool cyrf6936Init(void);
|
||||||
|
|
||||||
bool cyrf6936RxFinished(uint32_t *timeStamp);
|
bool cyrf6936RxFinished(uint32_t *timeStamp);
|
||||||
|
|
||||||
|
|
|
@ -34,8 +34,10 @@
|
||||||
#include "drivers/bus_spi.h"
|
#include "drivers/bus_spi.h"
|
||||||
#include "drivers/io.h"
|
#include "drivers/io.h"
|
||||||
#include "drivers/io_impl.h"
|
#include "drivers/io_impl.h"
|
||||||
|
#include "drivers/nvic.h"
|
||||||
#include "drivers/rcc.h"
|
#include "drivers/rcc.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
|
#include "drivers/time.h"
|
||||||
|
|
||||||
#include "pg/rx_spi.h"
|
#include "pg/rx_spi.h"
|
||||||
|
|
||||||
|
@ -47,11 +49,30 @@
|
||||||
static busDevice_t rxSpiDevice;
|
static busDevice_t rxSpiDevice;
|
||||||
static busDevice_t *busdev = &rxSpiDevice;
|
static busDevice_t *busdev = &rxSpiDevice;
|
||||||
|
|
||||||
|
static IO_t extiPin = IO_NONE;
|
||||||
|
static extiCallbackRec_t rxSpiExtiCallbackRec;
|
||||||
|
static bool extiLevel = true;
|
||||||
|
|
||||||
|
static volatile bool extiHasOccurred = false;
|
||||||
|
static volatile timeUs_t lastExtiTimeUs = 0;
|
||||||
|
|
||||||
void rxSpiDevicePreInit(const rxSpiConfig_t *rxSpiConfig)
|
void rxSpiDevicePreInit(const rxSpiConfig_t *rxSpiConfig)
|
||||||
{
|
{
|
||||||
spiPreinitRegister(rxSpiConfig->csnTag, IOCFG_IPU, 1);
|
spiPreinitRegister(rxSpiConfig->csnTag, IOCFG_IPU, 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void rxSpiExtiHandler(extiCallbackRec_t* callback)
|
||||||
|
{
|
||||||
|
UNUSED(callback);
|
||||||
|
|
||||||
|
const timeUs_t extiTimeUs = microsISR();
|
||||||
|
|
||||||
|
if (IORead(extiPin) == extiLevel) {
|
||||||
|
lastExtiTimeUs = extiTimeUs;
|
||||||
|
extiHasOccurred = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
bool rxSpiDeviceInit(const rxSpiConfig_t *rxSpiConfig)
|
bool rxSpiDeviceInit(const rxSpiConfig_t *rxSpiConfig)
|
||||||
{
|
{
|
||||||
SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(rxSpiConfig->spibus));
|
SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(rxSpiConfig->spibus));
|
||||||
|
@ -74,9 +95,28 @@ bool rxSpiDeviceInit(const rxSpiConfig_t *rxSpiConfig)
|
||||||
spiBusSetDivisor(busdev, SPI_CLOCK_STANDARD);
|
spiBusSetDivisor(busdev, SPI_CLOCK_STANDARD);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
extiPin = IOGetByTag(rxSpiConfig->extiIoTag);
|
||||||
|
|
||||||
|
if (extiPin) {
|
||||||
|
IOInit(extiPin, OWNER_RX_SPI_EXTI, 0);
|
||||||
|
}
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void rxSpiExtiInit(ioConfig_t rxSpiExtiPinConfig, extiTrigger_t rxSpiExtiPinTrigger)
|
||||||
|
{
|
||||||
|
if (extiPin) {
|
||||||
|
if (rxSpiExtiPinTrigger == EXTI_TRIGGER_FALLING) {
|
||||||
|
extiLevel = false;
|
||||||
|
}
|
||||||
|
EXTIHandlerInit(&rxSpiExtiCallbackRec, rxSpiExtiHandler);
|
||||||
|
EXTIConfig(extiPin, &rxSpiExtiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, rxSpiExtiPinConfig, rxSpiExtiPinTrigger);
|
||||||
|
EXTIEnable(extiPin, true);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
uint8_t rxSpiTransferByte(uint8_t data)
|
uint8_t rxSpiTransferByte(uint8_t data)
|
||||||
{
|
{
|
||||||
return spiBusTransferByte(busdev, data);
|
return spiBusTransferByte(busdev, data);
|
||||||
|
@ -108,4 +148,29 @@ void rxSpiReadCommandMulti(uint8_t command, uint8_t commandData, uint8_t *retDat
|
||||||
UNUSED(commandData);
|
UNUSED(commandData);
|
||||||
spiBusRawReadRegisterBuffer(busdev, command, retData, length);
|
spiBusRawReadRegisterBuffer(busdev, command, retData, length);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool rxSpiExtiConfigured(void)
|
||||||
|
{
|
||||||
|
return extiPin != IO_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool rxSpiGetExtiState(void)
|
||||||
|
{
|
||||||
|
return IORead(extiPin);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool rxSpiPollExti(void)
|
||||||
|
{
|
||||||
|
return extiHasOccurred;
|
||||||
|
}
|
||||||
|
|
||||||
|
void rxSpiResetExti(void)
|
||||||
|
{
|
||||||
|
extiHasOccurred = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
timeUs_t rxSpiGetLastExtiTimeUs(void)
|
||||||
|
{
|
||||||
|
return lastExtiTimeUs;
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -22,6 +22,10 @@
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#include "common/time.h"
|
||||||
|
|
||||||
|
#include "drivers/exti.h"
|
||||||
|
|
||||||
#define RX_SPI_MAX_PAYLOAD_SIZE 35
|
#define RX_SPI_MAX_PAYLOAD_SIZE 35
|
||||||
|
|
||||||
struct rxSpiConfig_s;
|
struct rxSpiConfig_s;
|
||||||
|
@ -34,3 +38,9 @@ void rxSpiWriteCommand(uint8_t command, uint8_t data);
|
||||||
void rxSpiWriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length);
|
void rxSpiWriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length);
|
||||||
uint8_t rxSpiReadCommand(uint8_t command, uint8_t commandData);
|
uint8_t rxSpiReadCommand(uint8_t command, uint8_t commandData);
|
||||||
void rxSpiReadCommandMulti(uint8_t command, uint8_t commandData, uint8_t *retData, uint8_t length);
|
void rxSpiReadCommandMulti(uint8_t command, uint8_t commandData, uint8_t *retData, uint8_t length);
|
||||||
|
void rxSpiExtiInit(ioConfig_t rxSpiExtiPinConfig, extiTrigger_t rxSpiExtiPinTrigger);
|
||||||
|
bool rxSpiExtiConfigured(void);
|
||||||
|
bool rxSpiGetExtiState(void);
|
||||||
|
bool rxSpiPollExti(void);
|
||||||
|
void rxSpiResetExti(void);
|
||||||
|
timeUs_t rxSpiGetLastExtiTimeUs(void);
|
||||||
|
|
|
@ -32,6 +32,7 @@
|
||||||
|
|
||||||
#include "drivers/io.h"
|
#include "drivers/io.h"
|
||||||
#include "drivers/rx/rx_a7105.h"
|
#include "drivers/rx/rx_a7105.h"
|
||||||
|
#include "drivers/rx/rx_spi.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
|
|
||||||
|
@ -354,10 +355,9 @@ static rx_spi_received_e flySkyReadAndProcess(uint8_t *payload, const uint32_t t
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool flySkyInit(const rxSpiConfig_t *rxSpiConfig, struct rxRuntimeState_s *rxRuntimeState)
|
bool flySkyInit(const rxSpiConfig_t *rxSpiConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
|
||||||
{
|
{
|
||||||
IO_t extiPin = IOGetByTag(rxSpiConfig->extiIoTag);
|
if (!rxSpiExtiConfigured()) {
|
||||||
if (!extiPin) {
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -365,6 +365,9 @@ bool flySkyInit(const rxSpiConfig_t *rxSpiConfig, struct rxRuntimeState_s *rxRun
|
||||||
|
|
||||||
rxSpiCommonIOInit(rxSpiConfig);
|
rxSpiCommonIOInit(rxSpiConfig);
|
||||||
|
|
||||||
|
extiConfig->ioConfig = IOCFG_IPD;
|
||||||
|
extiConfig->trigger = EXTI_TRIGGER_RISING;
|
||||||
|
|
||||||
uint8_t startRxChannel;
|
uint8_t startRxChannel;
|
||||||
|
|
||||||
if (protocol == RX_SPI_A7105_FLYSKY_2A) {
|
if (protocol == RX_SPI_A7105_FLYSKY_2A) {
|
||||||
|
@ -372,13 +375,13 @@ bool flySkyInit(const rxSpiConfig_t *rxSpiConfig, struct rxRuntimeState_s *rxRun
|
||||||
timings = &flySky2ATimings;
|
timings = &flySky2ATimings;
|
||||||
rxId = U_ID_0 ^ U_ID_1 ^ U_ID_2;
|
rxId = U_ID_0 ^ U_ID_1 ^ U_ID_2;
|
||||||
startRxChannel = flySky2ABindChannels[0];
|
startRxChannel = flySky2ABindChannels[0];
|
||||||
A7105Init(0x5475c52A, extiPin, IO_NONE);
|
A7105Init(0x5475c52A, IO_NONE);
|
||||||
A7105Config(flySky2ARegs, sizeof(flySky2ARegs));
|
A7105Config(flySky2ARegs, sizeof(flySky2ARegs));
|
||||||
} else {
|
} else {
|
||||||
rxRuntimeState->channelCount = FLYSKY_CHANNEL_COUNT;
|
rxRuntimeState->channelCount = FLYSKY_CHANNEL_COUNT;
|
||||||
timings = &flySkyTimings;
|
timings = &flySkyTimings;
|
||||||
startRxChannel = 0;
|
startRxChannel = 0;
|
||||||
A7105Init(0x5475c52A, extiPin, IO_NONE);
|
A7105Init(0x5475c52A, IO_NONE);
|
||||||
A7105Config(flySkyRegs, sizeof(flySkyRegs));
|
A7105Config(flySkyRegs, sizeof(flySkyRegs));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -32,6 +32,6 @@ PG_DECLARE(flySkyConfig_t, flySkyConfig);
|
||||||
|
|
||||||
struct rxSpiConfig_s;
|
struct rxSpiConfig_s;
|
||||||
struct rxRuntimeState_s;
|
struct rxRuntimeState_s;
|
||||||
bool flySkyInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState);
|
bool flySkyInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
|
||||||
void flySkySetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
void flySkySetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
||||||
rx_spi_received_e flySkyDataReceived(uint8_t *payload);
|
rx_spi_received_e flySkyDataReceived(uint8_t *payload);
|
||||||
|
|
|
@ -28,6 +28,7 @@
|
||||||
|
|
||||||
#include "drivers/io.h"
|
#include "drivers/io.h"
|
||||||
#include "drivers/rx/rx_cc2500.h"
|
#include "drivers/rx/rx_cc2500.h"
|
||||||
|
#include "drivers/rx/rx_spi.h"
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
|
|
||||||
#include "config/config.h"
|
#include "config/config.h"
|
||||||
|
@ -43,7 +44,6 @@
|
||||||
|
|
||||||
#include "cc2500_common.h"
|
#include "cc2500_common.h"
|
||||||
|
|
||||||
static IO_t gdoPin;
|
|
||||||
#if defined(USE_RX_CC2500_SPI_PA_LNA)
|
#if defined(USE_RX_CC2500_SPI_PA_LNA)
|
||||||
static IO_t txEnPin;
|
static IO_t txEnPin;
|
||||||
static IO_t rxLnaEnPin;
|
static IO_t rxLnaEnPin;
|
||||||
|
@ -69,11 +69,6 @@ void cc2500setRssiDbm(uint8_t value)
|
||||||
setRssi(rssiDbm << 3, RSSI_SOURCE_RX_PROTOCOL);
|
setRssi(rssiDbm << 3, RSSI_SOURCE_RX_PROTOCOL);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool cc2500getGdo(void)
|
|
||||||
{
|
|
||||||
return IORead(gdoPin);
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY)
|
#if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY)
|
||||||
void cc2500switchAntennae(void)
|
void cc2500switchAntennae(void)
|
||||||
{
|
{
|
||||||
|
@ -122,15 +117,10 @@ bool cc2500SpiInit(void)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// gpio init here
|
if (!rxSpiExtiConfigured()) {
|
||||||
gdoPin = IOGetByTag(rxSpiConfig()->extiIoTag);
|
|
||||||
|
|
||||||
if (!gdoPin) {
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
IOInit(gdoPin, OWNER_RX_SPI_EXTI, 0);
|
|
||||||
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);
|
||||||
|
|
|
@ -24,7 +24,6 @@
|
||||||
|
|
||||||
uint16_t cc2500getRssiDbm(void);
|
uint16_t cc2500getRssiDbm(void);
|
||||||
void cc2500setRssiDbm(uint8_t value);
|
void cc2500setRssiDbm(uint8_t value);
|
||||||
bool cc2500getGdo(void);
|
|
||||||
#if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY)
|
#if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY)
|
||||||
void cc2500switchAntennae(void);
|
void cc2500switchAntennae(void);
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -22,7 +22,7 @@
|
||||||
|
|
||||||
#include "rx/rx_spi.h"
|
#include "rx/rx_spi.h"
|
||||||
|
|
||||||
bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState);
|
bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
|
||||||
rx_spi_received_e frSkySpiDataReceived(uint8_t *packet);
|
rx_spi_received_e frSkySpiDataReceived(uint8_t *packet);
|
||||||
rx_spi_received_e frSkySpiProcessFrame(uint8_t *packet);
|
rx_spi_received_e frSkySpiProcessFrame(uint8_t *packet);
|
||||||
void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload);
|
void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload);
|
||||||
|
|
|
@ -36,8 +36,9 @@
|
||||||
#include "config/feature.h"
|
#include "config/feature.h"
|
||||||
|
|
||||||
#include "drivers/adc.h"
|
#include "drivers/adc.h"
|
||||||
#include "drivers/rx/rx_cc2500.h"
|
|
||||||
#include "drivers/io.h"
|
#include "drivers/io.h"
|
||||||
|
#include "drivers/rx/rx_cc2500.h"
|
||||||
|
#include "drivers/rx/rx_spi.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
|
|
||||||
|
@ -216,7 +217,7 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro
|
||||||
FALLTHROUGH; //!!TODO -check this fall through is correct
|
FALLTHROUGH; //!!TODO -check this fall through is correct
|
||||||
// here FS code could be
|
// here FS code could be
|
||||||
case STATE_DATA:
|
case STATE_DATA:
|
||||||
if (cc2500getGdo()) {
|
if (rxSpiGetExtiState()) {
|
||||||
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
||||||
bool packetOk = false;
|
bool packetOk = false;
|
||||||
if (ccLen >= 20) {
|
if (ccLen >= 20) {
|
||||||
|
|
|
@ -26,8 +26,9 @@
|
||||||
|
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
|
|
||||||
#include "drivers/rx/rx_cc2500.h"
|
|
||||||
#include "drivers/io.h"
|
#include "drivers/io.h"
|
||||||
|
#include "drivers/rx/rx_cc2500.h"
|
||||||
|
#include "drivers/rx/rx_spi.h"
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
|
|
||||||
#include "config/config.h"
|
#include "config/config.h"
|
||||||
|
@ -195,7 +196,7 @@ static bool tuneRx(uint8_t *packet)
|
||||||
bindOffset += 5;
|
bindOffset += 5;
|
||||||
cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
|
cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
|
||||||
}
|
}
|
||||||
if (cc2500getGdo()) {
|
if (rxSpiGetExtiState()) {
|
||||||
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
||||||
if (ccLen) {
|
if (ccLen) {
|
||||||
cc2500ReadFifo(packet, ccLen);
|
cc2500ReadFifo(packet, ccLen);
|
||||||
|
@ -235,7 +236,7 @@ static bool getBind1(uint8_t *packet)
|
||||||
// len|bind |tx
|
// len|bind |tx
|
||||||
// id|03|01|idx|h0|h1|h2|h3|h4|00|00|00|00|00|00|00|00|00|00|00|00|00|00|00|CHK1|CHK2|RSSI|LQI/CRC|
|
// id|03|01|idx|h0|h1|h2|h3|h4|00|00|00|00|00|00|00|00|00|00|00|00|00|00|00|CHK1|CHK2|RSSI|LQI/CRC|
|
||||||
// Start by getting bind packet 0 and the txid
|
// Start by getting bind packet 0 and the txid
|
||||||
if (cc2500getGdo()) {
|
if (rxSpiGetExtiState()) {
|
||||||
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
||||||
if (ccLen) {
|
if (ccLen) {
|
||||||
cc2500ReadFifo(packet, ccLen);
|
cc2500ReadFifo(packet, ccLen);
|
||||||
|
@ -264,7 +265,7 @@ static bool getBind1(uint8_t *packet)
|
||||||
static bool getBind2(uint8_t *packet)
|
static bool getBind2(uint8_t *packet)
|
||||||
{
|
{
|
||||||
if (bindIdx <= 120) {
|
if (bindIdx <= 120) {
|
||||||
if (cc2500getGdo()) {
|
if (rxSpiGetExtiState()) {
|
||||||
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
||||||
if (ccLen) {
|
if (ccLen) {
|
||||||
cc2500ReadFifo(packet, ccLen);
|
cc2500ReadFifo(packet, ccLen);
|
||||||
|
@ -418,8 +419,10 @@ void nextChannel(uint8_t skip)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState)
|
bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
|
||||||
{
|
{
|
||||||
|
UNUSED(extiConfig);
|
||||||
|
|
||||||
rxSpiCommonIOInit(rxSpiConfig);
|
rxSpiCommonIOInit(rxSpiConfig);
|
||||||
if (!cc2500SpiInit()) {
|
if (!cc2500SpiInit()) {
|
||||||
return false;
|
return false;
|
||||||
|
|
|
@ -33,11 +33,12 @@
|
||||||
#include "config/feature.h"
|
#include "config/feature.h"
|
||||||
|
|
||||||
#include "drivers/adc.h"
|
#include "drivers/adc.h"
|
||||||
#include "drivers/rx/rx_cc2500.h"
|
|
||||||
#include "drivers/io.h"
|
#include "drivers/io.h"
|
||||||
#include "drivers/io_def.h"
|
#include "drivers/io_def.h"
|
||||||
#include "drivers/io_types.h"
|
#include "drivers/io_types.h"
|
||||||
#include "drivers/resource.h"
|
#include "drivers/resource.h"
|
||||||
|
#include "drivers/rx/rx_cc2500.h"
|
||||||
|
#include "drivers/rx/rx_spi.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
|
|
||||||
|
@ -368,7 +369,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
|
||||||
FALLTHROUGH;
|
FALLTHROUGH;
|
||||||
// here FS code could be
|
// here FS code could be
|
||||||
case STATE_DATA:
|
case STATE_DATA:
|
||||||
if (cc2500getGdo() && (frameReceived == false)){
|
if (rxSpiGetExtiState() && (frameReceived == false)){
|
||||||
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
||||||
if (ccLen >= packetLength) {
|
if (ccLen >= packetLength) {
|
||||||
cc2500ReadFifo(packet, packetLength);
|
cc2500ReadFifo(packet, packetLength);
|
||||||
|
|
|
@ -29,8 +29,9 @@
|
||||||
|
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
|
|
||||||
#include "drivers/rx/rx_cc2500.h"
|
|
||||||
#include "drivers/io.h"
|
#include "drivers/io.h"
|
||||||
|
#include "drivers/rx/rx_cc2500.h"
|
||||||
|
#include "drivers/rx/rx_spi.h"
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
|
|
||||||
#include "config/config.h"
|
#include "config/config.h"
|
||||||
|
@ -138,7 +139,7 @@ static bool sfhssRecv(uint8_t *packet)
|
||||||
{
|
{
|
||||||
uint8_t ccLen;
|
uint8_t ccLen;
|
||||||
|
|
||||||
if (!(cc2500getGdo())) {
|
if (!(rxSpiGetExtiState())) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
||||||
|
@ -423,8 +424,10 @@ rx_spi_received_e sfhssSpiDataReceived(uint8_t *packet)
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool sfhssSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState)
|
bool sfhssSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
|
||||||
{
|
{
|
||||||
|
UNUSED(extiConfig);
|
||||||
|
|
||||||
rxSpiCommonIOInit(rxSpiConfig);
|
rxSpiCommonIOInit(rxSpiConfig);
|
||||||
|
|
||||||
cc2500SpiInit();
|
cc2500SpiInit();
|
||||||
|
|
|
@ -48,7 +48,7 @@ typedef struct rxSfhssSpiConfig_s {
|
||||||
|
|
||||||
PG_DECLARE(rxSfhssSpiConfig_t, rxSfhssSpiConfig);
|
PG_DECLARE(rxSfhssSpiConfig_t, rxSfhssSpiConfig);
|
||||||
|
|
||||||
bool sfhssSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState);
|
bool sfhssSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
|
||||||
rx_spi_received_e sfhssSpiDataReceived(uint8_t *packet);
|
rx_spi_received_e sfhssSpiDataReceived(uint8_t *packet);
|
||||||
void sfhssSpiSetRcData(uint16_t *rcData, const uint8_t *payload);
|
void sfhssSpiSetRcData(uint16_t *rcData, const uint8_t *payload);
|
||||||
|
|
||||||
|
|
|
@ -27,6 +27,7 @@
|
||||||
|
|
||||||
#include "drivers/io.h"
|
#include "drivers/io.h"
|
||||||
#include "drivers/rx/rx_cyrf6936.h"
|
#include "drivers/rx/rx_cyrf6936.h"
|
||||||
|
#include "drivers/rx/rx_spi.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
|
|
||||||
|
@ -372,18 +373,23 @@ static void dsmReceiverStartTransfer(void)
|
||||||
cyrf6936StartRecv();
|
cyrf6936StartRecv();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool spektrumSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState)
|
bool spektrumSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
|
||||||
{
|
{
|
||||||
IO_t extiPin = IOGetByTag(rxConfig->extiIoTag);
|
UNUSED(extiConfig);
|
||||||
if (!extiPin) {
|
|
||||||
|
if (!rxSpiExtiConfigured()) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
rxSpiCommonIOInit(rxConfig);
|
rxSpiCommonIOInit(rxConfig);
|
||||||
|
|
||||||
|
|
||||||
rxRuntimeState->channelCount = DSM_MAX_CHANNEL_COUNT;
|
rxRuntimeState->channelCount = DSM_MAX_CHANNEL_COUNT;
|
||||||
|
|
||||||
if (!cyrf6936Init(extiPin)) {
|
extiConfig->ioConfig = IOCFG_IPD;
|
||||||
|
extiConfig->trigger = EXTI_TRIGGER_FALLING;
|
||||||
|
|
||||||
|
if (!cyrf6936Init()) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -453,6 +459,7 @@ void spektrumSpiSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
|
||||||
static bool isValidPacket(const uint8_t *packet)
|
static bool isValidPacket(const uint8_t *packet)
|
||||||
{
|
{
|
||||||
DEBUG_SET(DEBUG_RX_SPEKTRUM_SPI, 1, isError);
|
DEBUG_SET(DEBUG_RX_SPEKTRUM_SPI, 1, isError);
|
||||||
|
|
||||||
if (isError) {
|
if (isError) {
|
||||||
if (dsmReceiver.status != DSM_RECEIVER_RECV && (cyrf6936GetRxStatus() & CYRF6936_BAD_CRC)) {
|
if (dsmReceiver.status != DSM_RECEIVER_RECV && (cyrf6936GetRxStatus() & CYRF6936_BAD_CRC)) {
|
||||||
dsmReceiver.crcSeed = ~dsmReceiver.crcSeed;
|
dsmReceiver.crcSeed = ~dsmReceiver.crcSeed;
|
||||||
|
|
|
@ -48,6 +48,6 @@ typedef struct spektrumConfig_s {
|
||||||
|
|
||||||
PG_DECLARE(spektrumConfig_t, spektrumConfig);
|
PG_DECLARE(spektrumConfig_t, spektrumConfig);
|
||||||
|
|
||||||
bool spektrumSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState);
|
bool spektrumSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
|
||||||
void spektrumSpiSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
void spektrumSpiSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
||||||
rx_spi_received_e spektrumSpiDataReceived(uint8_t *payload);
|
rx_spi_received_e spektrumSpiDataReceived(uint8_t *payload);
|
||||||
|
|
|
@ -299,8 +299,10 @@ static void cx10Nrf24Setup(rx_spi_protocol_e protocol)
|
||||||
NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
|
NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
|
||||||
}
|
}
|
||||||
|
|
||||||
bool cx10Nrf24Init(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState)
|
bool cx10Nrf24Init(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
|
||||||
{
|
{
|
||||||
|
UNUSED(extiConfig);
|
||||||
|
|
||||||
rxRuntimeState->channelCount = RC_CHANNEL_COUNT;
|
rxRuntimeState->channelCount = RC_CHANNEL_COUNT;
|
||||||
cx10Nrf24Setup((rx_spi_protocol_e)rxSpiConfig->rx_spi_protocol);
|
cx10Nrf24Setup((rx_spi_protocol_e)rxSpiConfig->rx_spi_protocol);
|
||||||
|
|
||||||
|
|
|
@ -25,6 +25,6 @@
|
||||||
|
|
||||||
struct rxSpiConfig_s;
|
struct rxSpiConfig_s;
|
||||||
struct rxRuntimeState_s;
|
struct rxRuntimeState_s;
|
||||||
bool cx10Nrf24Init(const struct rxSpiConfig_s *rxSpiConfig, struct rxRuntimeState_s *rxRuntimeState);
|
bool cx10Nrf24Init(const struct rxSpiConfig_s *rxSpiConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
|
||||||
void cx10Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
void cx10Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
||||||
rx_spi_received_e cx10Nrf24DataReceived(uint8_t *payload);
|
rx_spi_received_e cx10Nrf24DataReceived(uint8_t *payload);
|
||||||
|
|
|
@ -283,8 +283,10 @@ static void h8_3dNrf24Setup(rx_spi_protocol_e protocol, const uint32_t *rxSpiId)
|
||||||
NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
|
NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
|
||||||
}
|
}
|
||||||
|
|
||||||
bool h8_3dNrf24Init(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState)
|
bool h8_3dNrf24Init(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
|
||||||
{
|
{
|
||||||
|
UNUSED(extiConfig);
|
||||||
|
|
||||||
rxRuntimeState->channelCount = RC_CHANNEL_COUNT;
|
rxRuntimeState->channelCount = RC_CHANNEL_COUNT;
|
||||||
h8_3dNrf24Setup((rx_spi_protocol_e)rxSpiConfig->rx_spi_protocol, &rxSpiConfig->rx_spi_id);
|
h8_3dNrf24Setup((rx_spi_protocol_e)rxSpiConfig->rx_spi_protocol, &rxSpiConfig->rx_spi_id);
|
||||||
|
|
||||||
|
|
|
@ -25,6 +25,6 @@
|
||||||
|
|
||||||
struct rxSpiConfig_s;
|
struct rxSpiConfig_s;
|
||||||
struct rxRuntimeState_s;
|
struct rxRuntimeState_s;
|
||||||
bool h8_3dNrf24Init(const struct rxSpiConfig_s *rxSpiConfig, struct rxRuntimeState_s *rxRuntimeState);
|
bool h8_3dNrf24Init(const struct rxSpiConfig_s *rxSpiConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
|
||||||
void h8_3dNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
void h8_3dNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
||||||
rx_spi_received_e h8_3dNrf24DataReceived(uint8_t *payload);
|
rx_spi_received_e h8_3dNrf24DataReceived(uint8_t *payload);
|
||||||
|
|
|
@ -424,8 +424,10 @@ static void inavNrf24Setup(rx_spi_protocol_e protocol, const uint32_t *rxSpiId,
|
||||||
writeAckPayload(ackPayload, payloadSize);
|
writeAckPayload(ackPayload, payloadSize);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool inavNrf24Init(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState)
|
bool inavNrf24Init(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
|
||||||
{
|
{
|
||||||
|
UNUSED(extiConfig);
|
||||||
|
|
||||||
rxRuntimeState->channelCount = RC_CHANNEL_COUNT_MAX;
|
rxRuntimeState->channelCount = RC_CHANNEL_COUNT_MAX;
|
||||||
inavNrf24Setup((rx_spi_protocol_e)rxSpiConfig->rx_spi_protocol, &rxSpiConfig->rx_spi_id, rxSpiConfig->rx_spi_rf_channel_count);
|
inavNrf24Setup((rx_spi_protocol_e)rxSpiConfig->rx_spi_protocol, &rxSpiConfig->rx_spi_id, rxSpiConfig->rx_spi_rf_channel_count);
|
||||||
|
|
||||||
|
|
|
@ -25,7 +25,7 @@
|
||||||
|
|
||||||
struct rxSpiConfig_s;
|
struct rxSpiConfig_s;
|
||||||
struct rxRuntimeState_s;
|
struct rxRuntimeState_s;
|
||||||
bool inavNrf24Init(const struct rxSpiConfig_s *rxSpiConfig, struct rxRuntimeState_s *rxRuntimeState);
|
bool inavNrf24Init(const struct rxSpiConfig_s *rxSpiConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
|
||||||
void inavNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
void inavNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
||||||
rx_spi_received_e inavNrf24DataReceived(uint8_t *payload);
|
rx_spi_received_e inavNrf24DataReceived(uint8_t *payload);
|
||||||
|
|
||||||
|
|
|
@ -204,8 +204,10 @@ static void knNrf24Setup(rx_spi_protocol_e protocol)
|
||||||
NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
|
NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
|
||||||
}
|
}
|
||||||
|
|
||||||
bool knNrf24Init(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
|
bool knNrf24Init(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
|
||||||
{
|
{
|
||||||
|
UNUSED(extiConfig);
|
||||||
|
|
||||||
rxRuntimeState->channelCount = KN_RC_CHANNEL_COUNT;
|
rxRuntimeState->channelCount = KN_RC_CHANNEL_COUNT;
|
||||||
knNrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol);
|
knNrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol);
|
||||||
|
|
||||||
|
|
|
@ -25,6 +25,6 @@
|
||||||
|
|
||||||
struct rxConfig_s;
|
struct rxConfig_s;
|
||||||
struct rxRuntimeState_s;
|
struct rxRuntimeState_s;
|
||||||
bool knNrf24Init(const struct rxConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState);
|
bool knNrf24Init(const struct rxConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
|
||||||
void knNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
void knNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
||||||
rx_spi_received_e knNrf24DataReceived(uint8_t *payload);
|
rx_spi_received_e knNrf24DataReceived(uint8_t *payload);
|
||||||
|
|
|
@ -299,8 +299,10 @@ static void symaNrf24Setup(rx_spi_protocol_e protocol)
|
||||||
NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
|
NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
|
||||||
}
|
}
|
||||||
|
|
||||||
bool symaNrf24Init(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState)
|
bool symaNrf24Init(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
|
||||||
{
|
{
|
||||||
|
UNUSED(extiConfig);
|
||||||
|
|
||||||
rxRuntimeState->channelCount = RC_CHANNEL_COUNT;
|
rxRuntimeState->channelCount = RC_CHANNEL_COUNT;
|
||||||
symaNrf24Setup((rx_spi_protocol_e)rxSpiConfig->rx_spi_protocol);
|
symaNrf24Setup((rx_spi_protocol_e)rxSpiConfig->rx_spi_protocol);
|
||||||
|
|
||||||
|
|
|
@ -25,7 +25,7 @@
|
||||||
|
|
||||||
struct rxSpiConfig_s;
|
struct rxSpiConfig_s;
|
||||||
struct rxRuntimeState_s;
|
struct rxRuntimeState_s;
|
||||||
bool symaNrf24Init(const struct rxSpiConfig_s *rxSpiConfig, struct rxRuntimeState_s *rxRuntimeState);
|
bool symaNrf24Init(const struct rxSpiConfig_s *rxSpiConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
|
||||||
void symaNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
void symaNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
||||||
rx_spi_received_e symaNrf24DataReceived(uint8_t *payload);
|
rx_spi_received_e symaNrf24DataReceived(uint8_t *payload);
|
||||||
|
|
||||||
|
|
|
@ -259,8 +259,10 @@ static void v202Nrf24Setup(rx_spi_protocol_e protocol)
|
||||||
NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
|
NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
|
||||||
}
|
}
|
||||||
|
|
||||||
bool v202Nrf24Init(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState)
|
bool v202Nrf24Init(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
|
||||||
{
|
{
|
||||||
|
UNUSED(extiConfig);
|
||||||
|
|
||||||
rxRuntimeState->channelCount = V2X2_RC_CHANNEL_COUNT;
|
rxRuntimeState->channelCount = V2X2_RC_CHANNEL_COUNT;
|
||||||
v202Nrf24Setup((rx_spi_protocol_e)rxSpiConfig->rx_spi_protocol);
|
v202Nrf24Setup((rx_spi_protocol_e)rxSpiConfig->rx_spi_protocol);
|
||||||
|
|
||||||
|
|
|
@ -25,6 +25,6 @@
|
||||||
|
|
||||||
struct rxSpiConfig_s;
|
struct rxSpiConfig_s;
|
||||||
struct rxRuntimeState_s;
|
struct rxRuntimeState_s;
|
||||||
bool v202Nrf24Init(const struct rxSpiConfig_s *rxSpiConfig, struct rxRuntimeState_s *rxRuntimeState);
|
bool v202Nrf24Init(const struct rxSpiConfig_s *rxSpiConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
|
||||||
void v202Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
void v202Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
||||||
rx_spi_received_e v202Nrf24DataReceived(uint8_t *payload);
|
rx_spi_received_e v202Nrf24DataReceived(uint8_t *payload);
|
||||||
|
|
|
@ -31,6 +31,7 @@
|
||||||
|
|
||||||
#include "config/feature.h"
|
#include "config/feature.h"
|
||||||
|
|
||||||
|
#include "drivers/io.h"
|
||||||
#include "drivers/rx/rx_spi.h"
|
#include "drivers/rx/rx_spi.h"
|
||||||
#include "drivers/rx/rx_nrf24l01.h"
|
#include "drivers/rx/rx_nrf24l01.h"
|
||||||
|
|
||||||
|
@ -55,7 +56,7 @@ uint16_t rxSpiRcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||||
STATIC_UNIT_TESTED uint8_t rxSpiPayload[RX_SPI_MAX_PAYLOAD_SIZE];
|
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
|
STATIC_UNIT_TESTED uint8_t rxSpiNewPacketAvailable; // set true when a new packet is received
|
||||||
|
|
||||||
typedef bool (*protocolInitFnPtr)(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState);
|
typedef bool (*protocolInitFnPtr)(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
|
||||||
typedef rx_spi_received_e (*protocolDataReceivedFnPtr)(uint8_t *payload);
|
typedef rx_spi_received_e (*protocolDataReceivedFnPtr)(uint8_t *payload);
|
||||||
typedef rx_spi_received_e (*protocolProcessFrameFnPtr)(uint8_t *payload);
|
typedef rx_spi_received_e (*protocolProcessFrameFnPtr)(uint8_t *payload);
|
||||||
typedef void (*protocolSetRcDataFromPayloadFnPtr)(uint16_t *rcData, const uint8_t *payload);
|
typedef void (*protocolSetRcDataFromPayloadFnPtr)(uint16_t *rcData, const uint8_t *payload);
|
||||||
|
@ -82,7 +83,6 @@ STATIC_UNIT_TESTED uint16_t rxSpiReadRawRC(const rxRuntimeState_t *rxRuntimeStat
|
||||||
STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
|
STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
|
||||||
{
|
{
|
||||||
switch (protocol) {
|
switch (protocol) {
|
||||||
default:
|
|
||||||
#ifdef USE_RX_V202
|
#ifdef USE_RX_V202
|
||||||
case RX_SPI_NRF24_V202_250K:
|
case RX_SPI_NRF24_V202_250K:
|
||||||
case RX_SPI_NRF24_V202_1M:
|
case RX_SPI_NRF24_V202_1M:
|
||||||
|
@ -165,7 +165,10 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
|
||||||
protocolSetRcDataFromPayload = spektrumSpiSetRcDataFromPayload;
|
protocolSetRcDataFromPayload = spektrumSpiSetRcDataFromPayload;
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif
|
||||||
|
default:
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -224,8 +227,19 @@ bool rxSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeStat
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (rxSpiSetProtocol(rxSpiConfig->rx_spi_protocol)) {
|
if (!rxSpiSetProtocol(rxSpiConfig->rx_spi_protocol)) {
|
||||||
ret = protocolInit(rxSpiConfig, rxRuntimeState);
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
rxSpiExtiConfig_t extiConfig = {
|
||||||
|
.ioConfig = IOCFG_IN_FLOATING,
|
||||||
|
.trigger = EXTI_TRIGGER_RISING,
|
||||||
|
};
|
||||||
|
|
||||||
|
ret = protocolInit(rxSpiConfig, rxRuntimeState, &extiConfig);
|
||||||
|
|
||||||
|
if (rxSpiExtiConfigured()) {
|
||||||
|
rxSpiExtiInit(extiConfig.ioConfig, extiConfig.trigger);
|
||||||
}
|
}
|
||||||
rxSpiNewPacketAvailable = false;
|
rxSpiNewPacketAvailable = false;
|
||||||
rxRuntimeState->rxRefreshRate = 20000;
|
rxRuntimeState->rxRefreshRate = 20000;
|
||||||
|
|
|
@ -20,10 +20,13 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "drivers/exti.h"
|
||||||
|
|
||||||
#include "pg/rx.h"
|
#include "pg/rx.h"
|
||||||
#include "rx/rx.h"
|
|
||||||
#include "pg/rx_spi.h"
|
#include "pg/rx_spi.h"
|
||||||
|
|
||||||
|
#include "rx/rx.h"
|
||||||
|
|
||||||
// Used in MSP. Append at end.
|
// Used in MSP. Append at end.
|
||||||
typedef enum {
|
typedef enum {
|
||||||
RX_SPI_NRF24_V202_250K = 0,
|
RX_SPI_NRF24_V202_250K = 0,
|
||||||
|
@ -74,6 +77,11 @@ typedef enum {
|
||||||
RC_SPI_AUX14
|
RC_SPI_AUX14
|
||||||
} rc_spi_aetr_e;
|
} rc_spi_aetr_e;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
ioConfig_t ioConfig;
|
||||||
|
extiTrigger_t trigger;
|
||||||
|
} rxSpiExtiConfig_t;
|
||||||
|
|
||||||
// RC channels as used by deviation
|
// RC channels as used by deviation
|
||||||
#define RC_CHANNEL_RATE RC_SPI_AUX1
|
#define RC_CHANNEL_RATE RC_SPI_AUX1
|
||||||
#define RC_CHANNEL_FLIP RC_SPI_AUX2
|
#define RC_CHANNEL_FLIP RC_SPI_AUX2
|
||||||
|
|
|
@ -86,6 +86,7 @@ extern "C" {
|
||||||
|
|
||||||
static const dsmReceiver_t empty = dsmReceiver_t();
|
static const dsmReceiver_t empty = dsmReceiver_t();
|
||||||
static rxRuntimeState_t config = rxRuntimeState_t();
|
static rxRuntimeState_t config = rxRuntimeState_t();
|
||||||
|
static rxSpiExtiConfig_t extiConfig;
|
||||||
static uint8_t packetLen;
|
static uint8_t packetLen;
|
||||||
static uint8_t packet[16];
|
static uint8_t packet[16];
|
||||||
static uint16_t rssi = 0;
|
static uint16_t rssi = 0;
|
||||||
|
@ -142,7 +143,7 @@ extern "C" {
|
||||||
TEST(RxSpiSpektrumUnitTest, TestInitUnbound)
|
TEST(RxSpiSpektrumUnitTest, TestInitUnbound)
|
||||||
{
|
{
|
||||||
dsmReceiver = empty;
|
dsmReceiver = empty;
|
||||||
spektrumSpiInit(&injectedConfig, &config);
|
spektrumSpiInit(&injectedConfig, &config, &extiConfig);
|
||||||
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);
|
||||||
|
@ -160,7 +161,7 @@ TEST(RxSpiSpektrumUnitTest, TestInitBound)
|
||||||
|
|
||||||
spektrumConfigMutable()->protocol = DSMX_11;
|
spektrumConfigMutable()->protocol = DSMX_11;
|
||||||
|
|
||||||
bool result = spektrumSpiInit(&injectedConfig, &config);
|
bool result = spektrumSpiInit(&injectedConfig, &config, &extiConfig);
|
||||||
|
|
||||||
EXPECT_TRUE(result);
|
EXPECT_TRUE(result);
|
||||||
EXPECT_TRUE(dsmReceiver.bound);
|
EXPECT_TRUE(dsmReceiver.bound);
|
||||||
|
@ -180,7 +181,7 @@ TEST(RxSpiSpektrumUnitTest, TestInitBound)
|
||||||
dsmReceiver = empty;
|
dsmReceiver = empty;
|
||||||
spektrumConfigMutable()->protocol = DSM2_11;
|
spektrumConfigMutable()->protocol = DSM2_11;
|
||||||
|
|
||||||
spektrumSpiInit(&injectedConfig, &config);
|
spektrumSpiInit(&injectedConfig, &config, &extiConfig);
|
||||||
|
|
||||||
EXPECT_TRUE(dsmReceiver.bound);
|
EXPECT_TRUE(dsmReceiver.bound);
|
||||||
EXPECT_EQ(DSM2_11, dsmReceiver.protocol);
|
EXPECT_EQ(DSM2_11, dsmReceiver.protocol);
|
||||||
|
@ -388,8 +389,9 @@ extern "C" {
|
||||||
void rxSpiCommonIOInit(const rxSpiConfig_t *) {}
|
void rxSpiCommonIOInit(const rxSpiConfig_t *) {}
|
||||||
void rxSpiLedBlinkRxLoss(rx_spi_received_e ) {}
|
void rxSpiLedBlinkRxLoss(rx_spi_received_e ) {}
|
||||||
void rxSpiLedBlinkBind(void) {};
|
void rxSpiLedBlinkBind(void) {};
|
||||||
bool rxSpiCheckBindRequested(bool )
|
bool rxSpiCheckBindRequested(bool)
|
||||||
{
|
{
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
bool rxSpiExtiConfigured(void) { return true; }
|
||||||
}
|
}
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue