Refactored SPI RX led blink and bind plug functionality to rx_spi_common

This commit is contained in:
phobos- 2018-11-17 21:56:46 +01:00
parent 4246f06529
commit c88a5a3a22
21 changed files with 253 additions and 197 deletions

View File

@ -96,6 +96,7 @@ COMMON_SRC = \
rx/pwm.c \ rx/pwm.c \
rx/rx.c \ rx/rx.c \
rx/rx_spi.c \ rx/rx_spi.c \
rx/rx_spi_common.c \
rx/crsf.c \ rx/crsf.c \
rx/sbus.c \ rx/sbus.c \
rx/sbus_channels.c \ rx/sbus_channels.c \

View File

@ -77,4 +77,6 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
"SPI_PREINIT_IPU", "SPI_PREINIT_IPU",
"SPI_PREINIT_OPU", "SPI_PREINIT_OPU",
"MCO", "MCO",
"RX_SPI_BIND",
"RX_SPI_LED",
}; };

View File

@ -77,6 +77,8 @@ typedef enum {
OWNER_SPI_PREINIT_IPU, OWNER_SPI_PREINIT_IPU,
OWNER_SPI_PREINIT_OPU, OWNER_SPI_PREINIT_OPU,
OWNER_MCO, OWNER_MCO,
OWNER_RX_SPI_BIND,
OWNER_RX_SPI_LED,
OWNER_TOTAL_COUNT OWNER_TOTAL_COUNT
} resourceOwner_e; } resourceOwner_e;

View File

@ -140,9 +140,7 @@ extern uint8_t __config_end;
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/spektrum.h" #include "rx/spektrum.h"
#include "rx/cc2500_frsky_common.h" #include "rx/rx_spi_common.h"
#include "rx/cc2500_frsky_x.h"
#include "rx/cc2500_common.h"
#include "scheduler/scheduler.h" #include "scheduler/scheduler.h"
@ -2597,21 +2595,32 @@ static void cliBeeper(char *cmdline)
#endif #endif
#ifdef USE_RX_SPI #ifdef USE_RX_SPI
void cliRxBind(char *cmdline){ void cliRxSpiBind(char *cmdline){
UNUSED(cmdline); UNUSED(cmdline);
switch (rxSpiConfig()->rx_spi_protocol) { switch (rxSpiConfig()->rx_spi_protocol) {
#ifdef USE_RX_CC2500_BIND default:
cliPrint("Not supported.");
break;
#if defined(USE_RX_FRSKY_SPI)
#if defined(USE_RX_FRSKY_SPI_D)
case RX_SPI_FRSKY_D: case RX_SPI_FRSKY_D:
#endif
#if defined(USE_RX_FRSKY_SPI_X)
case RX_SPI_FRSKY_X: case RX_SPI_FRSKY_X:
#endif
#endif // USE_RX_FRSKY_SPI
#ifdef USE_RX_SFHSS_SPI
case RX_SPI_SFHSS: case RX_SPI_SFHSS:
cc2500SpiBind(); #endif
#ifdef USE_RX_FLYSKY
case RX_SPI_A7105_FLYSKY:
case RX_SPI_A7105_FLYSKY_2A:
#endif
#if defined(USE_RX_FRSKY_SPI) || defined(USE_RX_SFHSS_SPI) || defined(USE_RX_FLYSKY)
rxSpiBind();
cliPrint("Binding..."); cliPrint("Binding...");
break; break;
#endif #endif
default:
cliPrint("Not supported.");
break;
} }
} }
#endif #endif
@ -3907,6 +3916,8 @@ const cliResourceValue_t resourceTable[] = {
#endif #endif
#ifdef USE_RX_SPI #ifdef USE_RX_SPI
DEFS( OWNER_RX_SPI_CS, PG_RX_SPI_CONFIG, rxSpiConfig_t, csnTag ), DEFS( OWNER_RX_SPI_CS, PG_RX_SPI_CONFIG, rxSpiConfig_t, csnTag ),
DEFS( OWNER_RX_SPI_BIND, PG_RX_SPI_CONFIG, rxSpiConfig_t, bindIoTag ),
DEFS( OWNER_RX_SPI_LED, PG_RX_SPI_CONFIG, rxSpiConfig_t, ledIoTag ),
#endif #endif
#ifdef USE_GYRO_EXTI #ifdef USE_GYRO_EXTI
DEFW( OWNER_GYRO_EXTI, PG_GYRO_DEVICE_CONFIG, gyroDeviceConfig_t, extiTag, 2 ), DEFW( OWNER_GYRO_EXTI, PG_GYRO_DEVICE_CONFIG, gyroDeviceConfig_t, extiTag, 2 ),
@ -4504,6 +4515,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("beeper", "enable/disable beeper for a condition", "list\r\n" CLI_COMMAND_DEF("beeper", "enable/disable beeper for a condition", "list\r\n"
"\t<->[name]", cliBeeper), "\t<->[name]", cliBeeper),
#endif // USE_BEEPER #endif // USE_BEEPER
#ifdef USE_RX_SPI
CLI_COMMAND_DEF("bind_rx_spi", "initiate binding for RX SPI", NULL, cliRxSpiBind),
#endif
CLI_COMMAND_DEF("bl", "reboot into bootloader", NULL, cliBootloader), CLI_COMMAND_DEF("bl", "reboot into bootloader", NULL, cliBootloader),
#if defined(USE_BOARD_INFO) #if defined(USE_BOARD_INFO)
CLI_COMMAND_DEF("board_name", "get / set the name of the board model", "[board name]", cliBoardName), CLI_COMMAND_DEF("board_name", "get / set the name of the board model", "[board name]", cliBoardName),
@ -4535,9 +4549,6 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("flash_read", NULL, "<length> <address>", cliFlashRead), CLI_COMMAND_DEF("flash_read", NULL, "<length> <address>", cliFlashRead),
CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite), CLI_COMMAND_DEF("flash_write", NULL, "<address> <message>", cliFlashWrite),
#endif #endif
#endif
#ifdef USE_RX_CC2500_BIND
CLI_COMMAND_DEF("bind", "initiate binding for RX", NULL, cliRxBind),
#endif #endif
CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet), CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet),
#ifdef USE_GPS #ifdef USE_GPS

View File

@ -652,6 +652,7 @@ const clivalue_t valueTable[] = {
#ifdef USE_RX_SPI #ifdef USE_RX_SPI
{ "rx_spi_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RX_SPI }, PG_RX_SPI_CONFIG, offsetof(rxSpiConfig_t, rx_spi_protocol) }, { "rx_spi_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RX_SPI }, PG_RX_SPI_CONFIG, offsetof(rxSpiConfig_t, rx_spi_protocol) },
{ "rx_spi_bus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, SPIDEV_COUNT }, PG_RX_SPI_CONFIG, offsetof(rxSpiConfig_t, spibus) }, { "rx_spi_bus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, SPIDEV_COUNT }, PG_RX_SPI_CONFIG, offsetof(rxSpiConfig_t, spibus) },
{ "rx_spi_led_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_SPI_CONFIG, offsetof(rxSpiConfig_t, ledInversion) },
#endif #endif
// PG_ADC_CONFIG // PG_ADC_CONFIG

View File

@ -31,6 +31,10 @@
#include "rx/rx_spi.h" #include "rx/rx_spi.h"
#ifndef RX_SPI_LED_PIN
#define RX_SPI_LED_PIN NONE
#endif
PG_REGISTER_WITH_RESET_FN(rxSpiConfig_t, rxSpiConfig, PG_RX_SPI_CONFIG, 0); PG_REGISTER_WITH_RESET_FN(rxSpiConfig_t, rxSpiConfig, PG_RX_SPI_CONFIG, 0);
void pgResetFn_rxSpiConfig(rxSpiConfig_t *rxSpiConfig) void pgResetFn_rxSpiConfig(rxSpiConfig_t *rxSpiConfig)
@ -40,5 +44,13 @@ void pgResetFn_rxSpiConfig(rxSpiConfig_t *rxSpiConfig)
// Basic SPI // Basic SPI
rxSpiConfig->csnTag = IO_TAG(RX_NSS_PIN); rxSpiConfig->csnTag = IO_TAG(RX_NSS_PIN);
rxSpiConfig->spibus = SPI_DEV_TO_CFG(spiDeviceByInstance(RX_SPI_INSTANCE)); rxSpiConfig->spibus = SPI_DEV_TO_CFG(spiDeviceByInstance(RX_SPI_INSTANCE));
rxSpiConfig->bindIoTag = IO_TAG(BINDPLUG_PIN);
rxSpiConfig->ledIoTag = IO_TAG(RX_SPI_LED_PIN);
#ifdef RX_SPI_LED_INVERTED
rxSpiConfig->ledInversion = true;
#else
rxSpiConfig->ledInversion = false;
#endif
} }
#endif #endif

View File

@ -35,6 +35,10 @@ typedef struct rxSpiConfig_s {
ioTag_t csnTag; ioTag_t csnTag;
uint8_t spibus; uint8_t spibus;
ioTag_t bindIoTag;
ioTag_t ledIoTag;
uint8_t ledInversion;
} rxSpiConfig_t; } rxSpiConfig_t;
PG_DECLARE(rxSpiConfig_t, rxSpiConfig); PG_DECLARE(rxSpiConfig_t, rxSpiConfig);

View File

@ -43,11 +43,6 @@
#include "rx/cc2500_common.h" #include "rx/cc2500_common.h"
static IO_t gdoPin; static IO_t gdoPin;
static IO_t bindPin = DEFIO_IO(NONE);
static IO_t cc2500LedPin;
static bool bindRequested;
static bool lastBindPinStatus;
#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;
@ -71,32 +66,6 @@ void cc2500setRssiDbm(uint8_t value)
setRssi(rssiDbm << 3, RSSI_SOURCE_RX_PROTOCOL); setRssi(rssiDbm << 3, RSSI_SOURCE_RX_PROTOCOL);
} }
void cc2500SpiBind(void)
{
bindRequested = true;
}
bool cc2500checkBindRequested(bool reset)
{
if (bindPin) {
bool bindPinStatus = IORead(bindPin);
if (lastBindPinStatus && !bindPinStatus) {
bindRequested = true;
}
lastBindPinStatus = bindPinStatus;
}
if (!bindRequested) {
return false;
} else {
if (reset) {
bindRequested = false;
}
return true;
}
}
bool cc2500getGdo(void) bool cc2500getGdo(void)
{ {
return IORead(gdoPin); return IORead(gdoPin);
@ -127,42 +96,6 @@ void cc2500TxDisable(void)
} }
#endif #endif
void cc2500LedOn(void)
{
#if defined(RX_CC2500_SPI_LED_PIN_INVERTED)
IOLo(cc2500LedPin);
#else
IOHi(cc2500LedPin);
#endif
}
void cc2500LedOff(void)
{
#if defined(RX_CC2500_SPI_LED_PIN_INVERTED)
IOHi(cc2500LedPin);
#else
IOLo(cc2500LedPin);
#endif
}
void cc2500LedBlink(timeMs_t blinkms)
{
static bool ledIsOn = true;
static timeMs_t ledBlinkMs = 0;
if ( (ledBlinkMs + blinkms) > millis() ) {
return;
}
ledBlinkMs = millis();
if (ledIsOn) {
cc2500LedOff();
} else {
cc2500LedOn();
}
ledIsOn = !ledIsOn;
}
static bool cc2500SpiDetect(void) static bool cc2500SpiDetect(void)
{ {
const uint8_t chipPartNum = cc2500ReadReg(CC2500_30_PARTNUM | CC2500_READ_BURST); //CC2500 read registers chip part num const uint8_t chipPartNum = cc2500ReadReg(CC2500_30_PARTNUM | CC2500_READ_BURST); //CC2500 read registers chip part num
@ -192,9 +125,6 @@ bool cc2500SpiInit(void)
gdoPin = IOGetByTag(IO_TAG(RX_CC2500_SPI_GDO_0_PIN)); gdoPin = IOGetByTag(IO_TAG(RX_CC2500_SPI_GDO_0_PIN));
IOInit(gdoPin, OWNER_RX_SPI, 0); IOInit(gdoPin, OWNER_RX_SPI, 0);
IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING); IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING);
cc2500LedPin = IOGetByTag(IO_TAG(RX_CC2500_SPI_LED_PIN));
IOInit(cc2500LedPin, OWNER_LED, 0);
IOConfigGPIO(cc2500LedPin, IOCFG_OUT_PP);
#if defined(USE_RX_CC2500_SPI_PA_LNA) #if defined(USE_RX_CC2500_SPI_PA_LNA)
rxLnaEnPin = IOGetByTag(IO_TAG(RX_CC2500_SPI_LNA_EN_PIN)); rxLnaEnPin = IOGetByTag(IO_TAG(RX_CC2500_SPI_LNA_EN_PIN));
IOInit(rxLnaEnPin, OWNER_RX_SPI, 0); IOInit(rxLnaEnPin, OWNER_RX_SPI, 0);
@ -209,13 +139,6 @@ bool cc2500SpiInit(void)
IOConfigGPIO(antSelPin, IOCFG_OUT_PP); IOConfigGPIO(antSelPin, IOCFG_OUT_PP);
#endif #endif
#endif // USE_RX_CC2500_SPI_PA_LNA #endif // USE_RX_CC2500_SPI_PA_LNA
#if defined(BINDPLUG_PIN)
bindPin = IOGetByTag(IO_TAG(BINDPLUG_PIN));
IOInit(bindPin, OWNER_RX_BIND, 0);
IOConfigGPIO(bindPin, IOCFG_IPU);
lastBindPinStatus = IORead(bindPin);
#endif
#if defined(USE_RX_CC2500_SPI_PA_LNA) #if defined(USE_RX_CC2500_SPI_PA_LNA)
#if defined(USE_RX_CC2500_SPI_DIVERSITY) #if defined(USE_RX_CC2500_SPI_DIVERSITY)

View File

@ -26,8 +26,6 @@
uint16_t cc2500getRssiDbm(void); uint16_t cc2500getRssiDbm(void);
void cc2500setRssiDbm(uint8_t value); void cc2500setRssiDbm(uint8_t value);
void cc2500SpiBind(void);
bool cc2500checkBindRequested(bool reset);
bool cc2500getGdo(void); 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);
@ -36,7 +34,4 @@ void cc2500switchAntennae(void);
void cc2500TxEnable(void); void cc2500TxEnable(void);
void cc2500TxDisable(void); void cc2500TxDisable(void);
#endif #endif
void cc2500LedOn(void);
void cc2500LedOff(void);
void cc2500LedBlink(timeMs_t blinkms);
bool cc2500SpiInit(void); bool cc2500SpiInit(void);

View File

@ -46,6 +46,7 @@
#include "fc/config.h" #include "fc/config.h"
#include "rx/rx_spi_common.h"
#include "rx/cc2500_common.h" #include "rx/cc2500_common.h"
#include "rx/cc2500_frsky_common.h" #include "rx/cc2500_frsky_common.h"
#include "rx/cc2500_frsky_shared.h" #include "rx/cc2500_frsky_shared.h"
@ -177,8 +178,6 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro
static timeUs_t lastPacketReceivedTime = 0; static timeUs_t lastPacketReceivedTime = 0;
static timeUs_t telemetryTimeUs; static timeUs_t telemetryTimeUs;
static bool ledIsOn;
rx_spi_received_e ret = RX_SPI_RECEIVED_NONE; rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
const timeUs_t currentPacketReceivedTime = micros(); const timeUs_t currentPacketReceivedTime = micros();
@ -196,7 +195,7 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro
lastPacketReceivedTime = currentPacketReceivedTime; lastPacketReceivedTime = currentPacketReceivedTime;
*protocolState = STATE_DATA; *protocolState = STATE_DATA;
if (cc2500checkBindRequested(false)) { if (rxSpiCheckBindRequested(false)) {
lastPacketReceivedTime = 0; lastPacketReceivedTime = 0;
timeoutUs = 50; timeoutUs = 50;
missingPackets = 0; missingPackets = 0;
@ -218,7 +217,7 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro
if (packet[0] == 0x11) { if (packet[0] == 0x11) {
if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) && if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) &&
(packet[2] == rxFrSkySpiConfig()->bindTxId[1])) { (packet[2] == rxFrSkySpiConfig()->bindTxId[1])) {
cc2500LedOn(); rxSpiLedOn();
nextChannel(1); nextChannel(1);
cc2500setRssiDbm(packet[18]); cc2500setRssiDbm(packet[18]);
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
@ -260,12 +259,7 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro
missingPackets++; missingPackets++;
nextChannel(1); nextChannel(1);
} else { } else {
if (ledIsOn) { rxSpiLedToggle();
cc2500LedOff();
} else {
cc2500LedOn();
}
ledIsOn = !ledIsOn;
setRssi(0, RSSI_SOURCE_RX_PROTOCOL); setRssi(0, RSSI_SOURCE_RX_PROTOCOL);
nextChannel(13); nextChannel(13);

View File

@ -39,6 +39,7 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/rx_spi.h" #include "rx/rx_spi.h"
#include "rx/rx_spi_common.h"
#include "rx/cc2500_common.h" #include "rx/cc2500_common.h"
#include "rx/cc2500_frsky_common.h" #include "rx/cc2500_frsky_common.h"
@ -81,11 +82,6 @@ PG_RESET_TEMPLATE(rxFrSkySpiConfig_t, rxFrSkySpiConfig,
.useExternalAdc = false, .useExternalAdc = false,
); );
void frSkySpiBind(void)
{
cc2500SpiBind();
}
static void initialise() { static void initialise() {
cc2500Reset(); cc2500Reset();
cc2500WriteReg(CC2500_02_IOCFG0, 0x01); cc2500WriteReg(CC2500_02_IOCFG0, 0x01);
@ -324,8 +320,8 @@ rx_spi_received_e frSkySpiDataReceived(uint8_t *packet)
break; break;
case STATE_BIND: case STATE_BIND:
if (cc2500checkBindRequested(true) || rxFrSkySpiConfig()->autoBind) { if (rxSpiCheckBindRequested(true) || rxFrSkySpiConfig()->autoBind) {
cc2500LedOn(); rxSpiLedOn();
initTuneRx(); initTuneRx();
protocolState = STATE_BIND_TUNING; protocolState = STATE_BIND_TUNING;
@ -361,11 +357,9 @@ rx_spi_received_e frSkySpiDataReceived(uint8_t *packet)
if (!rxFrSkySpiConfig()->autoBind) { if (!rxFrSkySpiConfig()->autoBind) {
writeEEPROM(); writeEEPROM();
} else { } else {
uint8_t ctr = 40; uint8_t ctr = 80;
while (ctr--) { while (ctr--) {
cc2500LedOn(); rxSpiLedToggle();
delay(50);
cc2500LedOff();
delay(50); delay(50);
} }
} }
@ -411,6 +405,7 @@ void nextChannel(uint8_t skip)
bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeConfig) bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{ {
rxSpiCommonIOInit(rxSpiConfig);
cc2500SpiInit(); cc2500SpiInit();
spiProtocol = rxSpiConfig->rx_spi_protocol; spiProtocol = rxSpiConfig->rx_spi_protocol;

View File

@ -47,6 +47,7 @@
#include "fc/config.h" #include "fc/config.h"
#include "rx/rx_spi_common.h"
#include "rx/cc2500_common.h" #include "rx/cc2500_common.h"
#include "rx/cc2500_frsky_common.h" #include "rx/cc2500_frsky_common.h"
#include "rx/cc2500_frsky_shared.h" #include "rx/cc2500_frsky_shared.h"
@ -294,7 +295,6 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
static unsigned receiveTelemetryRetryCount = 0; static unsigned receiveTelemetryRetryCount = 0;
static timeMs_t pollingTimeMs = 0; static timeMs_t pollingTimeMs = 0;
static bool skipChannels = true; static bool skipChannels = true;
static bool ledIsOn;
static uint8_t remoteProcessedId = 0; static uint8_t remoteProcessedId = 0;
static uint8_t remoteAckId = 0; static uint8_t remoteAckId = 0;
@ -329,7 +329,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
*protocolState = STATE_DATA; *protocolState = STATE_DATA;
frameReceived = false; // again set for receive frameReceived = false; // again set for receive
receiveDelayUs = 5300; receiveDelayUs = 5300;
if (cc2500checkBindRequested(false)) { if (rxSpiCheckBindRequested(false)) {
packetTimerUs = 0; packetTimerUs = 0;
timeoutUs = 50; timeoutUs = 50;
missingPackets = 0; missingPackets = 0;
@ -358,7 +358,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
missingPackets = 0; missingPackets = 0;
timeoutUs = 1; timeoutUs = 1;
receiveDelayUs = 0; receiveDelayUs = 0;
cc2500LedOn(); rxSpiLedOn();
if (skipChannels) { if (skipChannels) {
channelsToSkip = packet[5] << 2; channelsToSkip = packet[5] << 2;
if (packet[4] >= listLength) { if (packet[4] >= listLength) {
@ -439,12 +439,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
} }
} }
if (cmpTimeUs(micros(), packetTimerUs) > timeoutUs * SYNC_DELAY_MAX) { if (cmpTimeUs(micros(), packetTimerUs) > timeoutUs * SYNC_DELAY_MAX) {
if (ledIsOn) { rxSpiLedToggle();
cc2500LedOff();
} else {
cc2500LedOn();
}
ledIsOn = !ledIsOn;
setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL); setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
nextChannel(1); nextChannel(1);

View File

@ -42,6 +42,7 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/rx_spi.h" #include "rx/rx_spi.h"
#include "rx/rx_spi_common.h"
#include "rx/cc2500_common.h" #include "rx/cc2500_common.h"
#include "rx/cc2500_frsky_common.h" #include "rx/cc2500_frsky_common.h"
@ -231,7 +232,7 @@ static bool tune1Rx(uint8_t *packet)
static bool tune2Rx(uint8_t *packet) static bool tune2Rx(uint8_t *packet)
{ {
cc2500LedBlink(100); rxSpiLedBlink(100);
if (((millis() - timeTunedMs) > 880) || bindOffset_max > (126 - BIND_TUNE_STEP)) { // 220ms *4 if (((millis() - timeTunedMs) > 880) || bindOffset_max > (126 - BIND_TUNE_STEP)) { // 220ms *4
timeTunedMs = millis(); timeTunedMs = millis();
cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset_min); cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset_min);
@ -252,7 +253,7 @@ static bool tune2Rx(uint8_t *packet)
static bool tune3Rx(uint8_t *packet) static bool tune3Rx(uint8_t *packet)
{ {
cc2500LedBlink(100); rxSpiLedBlink(100);
if (((millis() - timeTunedMs) > 880) || bindOffset_min < (-126 + BIND_TUNE_STEP)) { // 220ms *4 if (((millis() - timeTunedMs) > 880) || bindOffset_min < (-126 + BIND_TUNE_STEP)) { // 220ms *4
return true; return true;
} }
@ -307,7 +308,7 @@ rx_spi_received_e sfhssSpiDataReceived(uint8_t *packet)
switch (protocolState) { switch (protocolState) {
case STATE_INIT: case STATE_INIT:
if ((millis() - start_time) > 10) { if ((millis() - start_time) > 10) {
cc2500LedOff(); rxSpiLedOff();
dataMissingFrame = 0; dataMissingFrame = 0;
initialise(); initialise();
SET_STATE(STATE_BIND); SET_STATE(STATE_BIND);
@ -315,8 +316,8 @@ rx_spi_received_e sfhssSpiDataReceived(uint8_t *packet)
} }
break; break;
case STATE_BIND: case STATE_BIND:
if (cc2500checkBindRequested(true)) { if (rxSpiCheckBindRequested(true)) {
cc2500LedOn(); rxSpiLedOn();
initTuneRx(); initTuneRx();
SET_STATE(STATE_BIND_TUNING1); SET_STATE(STATE_BIND_TUNING1);
} else { } else {
@ -357,7 +358,7 @@ rx_spi_received_e sfhssSpiDataReceived(uint8_t *packet)
if (sfhssPacketParse(packet, true)) { if (sfhssPacketParse(packet, true)) {
if (GET_COMMAND(packet) & 0x8) { /* ch=5-8 */ if (GET_COMMAND(packet) & 0x8) { /* ch=5-8 */
missingPackets = 0; missingPackets = 0;
cc2500LedOn(); rxSpiLedOn();
frame_recvd = 0x3; frame_recvd = 0x3;
SET_STATE(STATE_SYNC); SET_STATE(STATE_SYNC);
nextFrameReceiveStartTime = currentPacketReceivedTime + NEXT_CH_TIME_SYNC2; nextFrameReceiveStartTime = currentPacketReceivedTime + NEXT_CH_TIME_SYNC2;
@ -366,13 +367,13 @@ rx_spi_received_e sfhssSpiDataReceived(uint8_t *packet)
} }
cc2500Strobe(CC2500_SRX); cc2500Strobe(CC2500_SRX);
} else if (cmpTimeUs(currentPacketReceivedTime, nextFrameReceiveStartTime) > 0) { } else if (cmpTimeUs(currentPacketReceivedTime, nextFrameReceiveStartTime) > 0) {
cc2500LedBlink(500); rxSpiLedBlink(500);
#if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY) // SE4311 chip #if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY) // SE4311 chip
cc2500switchAntennae(); cc2500switchAntennae();
#endif #endif
sfhssnextChannel(); sfhssnextChannel();
nextFrameReceiveStartTime += NEXT_CH_TIME_HUNT; nextFrameReceiveStartTime += NEXT_CH_TIME_HUNT;
} else if (cc2500checkBindRequested(false)) { } else if (rxSpiCheckBindRequested(false)) {
SET_STATE(STATE_INIT); SET_STATE(STATE_INIT);
break; break;
} }
@ -416,7 +417,7 @@ rx_spi_received_e sfhssSpiDataReceived(uint8_t *packet)
} }
frame_recvd = 0; frame_recvd = 0;
sfhssnextChannel(); sfhssnextChannel();
} else if (cc2500checkBindRequested(false)) { } else if (rxSpiCheckBindRequested(false)) {
SET_STATE(STATE_INIT); SET_STATE(STATE_INIT);
break; break;
} }

View File

@ -43,6 +43,7 @@
#include "rx/flysky_defs.h" #include "rx/flysky_defs.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/rx_spi.h" #include "rx/rx_spi.h"
#include "rx/rx_spi_common.h"
#include "sensors/battery.h" #include "sensors/battery.h"
@ -121,9 +122,6 @@ static bool waitTx = false;
static uint16_t errorRate = 0; static uint16_t errorRate = 0;
static uint16_t rssi_dBm = 0; static uint16_t rssi_dBm = 0;
static uint8_t rfChannelMap[FLYSKY_FREQUENCY_COUNT] = {0}; static uint8_t rfChannelMap[FLYSKY_FREQUENCY_COUNT] = {0};
#ifdef USE_RX_FLYSKY_SPI_LED
static IO_t flySkyLedPin;
#endif /* USE_RX_FLYSKY_SPI_LED */
static uint8_t getNextChannel (uint8_t step) static uint8_t getNextChannel (uint8_t step)
@ -364,15 +362,7 @@ bool flySkyInit (const rxSpiConfig_t *rxSpiConfig, struct rxRuntimeConfig_s *rxR
PG_RESET(flySkyConfig); PG_RESET(flySkyConfig);
} }
IO_t bindPin = IOGetByTag(IO_TAG(BINDPLUG_PIN)); rxSpiCommonIOInit(rxSpiConfig);
IOInit(bindPin, OWNER_RX_BIND, 0);
IOConfigGPIO(bindPin, IOCFG_IPU);
#ifdef USE_RX_FLYSKY_SPI_LED
flySkyLedPin = IOGetByTag(IO_TAG(RX_FLYSKY_SPI_LED_PIN));
IOInit(flySkyLedPin, OWNER_LED, 0);
IOConfigGPIO(flySkyLedPin, IOCFG_OUT_PP);
IOLo(flySkyLedPin);
#endif /* USE_RX_FLYSKY_SPI_LED */
uint8_t startRxChannel; uint8_t startRxChannel;
@ -391,7 +381,7 @@ bool flySkyInit (const rxSpiConfig_t *rxSpiConfig, struct rxRuntimeConfig_s *rxR
A7105Config(flySkyRegs, sizeof(flySkyRegs)); A7105Config(flySkyRegs, sizeof(flySkyRegs));
} }
if ( !IORead(bindPin) || flySkyConfig()->txId == 0) { if (flySkyConfig()->txId == 0) {
bound = false; bound = false;
} else { } else {
bound = true; bound = true;
@ -426,11 +416,6 @@ 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)
{ {
#ifdef USE_RX_FLYSKY_SPI_LED
static uint16_t rxLossCount = 0;
static timeMs_t ledLastUpdate = 0;
static bool ledOn = false;
#endif /* USE_RX_FLYSKY_SPI_LED */
rx_spi_received_e result = RX_SPI_RECEIVED_NONE; rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
uint32_t timeStamp; uint32_t timeStamp;
@ -458,29 +443,17 @@ rx_spi_received_e flySkyDataReceived (uint8_t *payload)
waitTx = false; waitTx = false;
} }
if (rxSpiCheckBindRequested(true)) {
bound = false;
txId = 0;
memset(rfChannelMap, 0, FLYSKY_FREQUENCY_COUNT);
uint8_t bindChannel = (protocol == RX_SPI_A7105_FLYSKY_2A) ? flySky2ABindChannels[0] : 0;
A7105WriteReg(A7105_0F_CHANNEL, bindChannel);
}
if (bound) { if (bound) {
checkTimeout(); checkTimeout();
#ifdef USE_RX_FLYSKY_SPI_LED rxSpiLedBlinkRxLoss(result);
if (result == RX_SPI_RECEIVED_DATA) {
rxLossCount = 0;
IOHi(flySkyLedPin);
} else {
if (rxLossCount < RX_LOSS_COUNT) {
rxLossCount++;
} else {
timeMs_t now = millis();
if (now - ledLastUpdate > INTERVAL_RX_LOSS_MS) {
ledLastUpdate = now;
if (ledOn) {
IOLo(flySkyLedPin);
} else {
IOHi(flySkyLedPin);
}
ledOn = !ledOn;
}
}
}
#endif /* USE_RX_FLYSKY_SPI_LED */
} else { } else {
if ((micros() - timeLastBind) > BIND_TIMEOUT && rfChannelMap[0] != 0 && txId != 0) { if ((micros() - timeLastBind) > BIND_TIMEOUT && rfChannelMap[0] != 0 && txId != 0) {
result = RX_SPI_RECEIVED_BIND; result = RX_SPI_RECEIVED_BIND;
@ -490,18 +463,7 @@ rx_spi_received_e flySkyDataReceived (uint8_t *payload)
flySkyConfigMutable()->protocol = protocol; flySkyConfigMutable()->protocol = protocol;
writeEEPROM(); writeEEPROM();
} }
#ifdef USE_RX_FLYSKY_SPI_LED rxSpiLedBlinkBind();
timeMs_t now = millis();
if (now - ledLastUpdate > INTERVAL_RX_BIND_MS) {
ledLastUpdate = now;
if (ledOn) {
IOLo(flySkyLedPin);
} else {
IOHi(flySkyLedPin);
}
ledOn = !ledOn;
}
#endif /* USE_RX_FLYSKY_SPI_LED */
} }
return result; return result;

View File

@ -42,9 +42,6 @@
#define TX_DELAY 500 #define TX_DELAY 500
#define BIND_TIMEOUT 200000 #define BIND_TIMEOUT 200000
#define INTERVAL_RX_LOSS_MS 1000
#define INTERVAL_RX_BIND_MS 250
#define RX_LOSS_COUNT 1000
typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) {
uint8_t type; uint8_t type;

126
src/main/rx/rx_spi_common.c Normal file
View File

@ -0,0 +1,126 @@
/*
* 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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#ifdef USE_RX_SPI
#include "drivers/io.h"
#include "drivers/time.h"
#include "rx/rx_spi_common.h"
#include "rx/rx_spi.h"
static IO_t ledPin;
static bool ledInversion = false;
static IO_t bindPin;
static bool bindRequested;
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();
bindPin = IOGetByTag(rxSpiConfig->bindIoTag);
IOInit(bindPin, OWNER_RX_BIND, 0);
IOConfigGPIO(bindPin, IOCFG_IPU);
lastBindPinStatus = IORead(bindPin);
}
void rxSpiLedOn(void)
{
ledInversion ? IOLo(ledPin) : IOHi(ledPin);
}
void rxSpiLedOff(void)
{
ledInversion ? IOHi(ledPin) : IOLo(ledPin);
}
void rxSpiLedToggle(void)
{
IOToggle(ledPin);
}
void rxSpiLedBlink(timeMs_t blinkMs)
{
static timeMs_t ledBlinkMs = 0;
if ((ledBlinkMs + blinkMs) > millis()) {
return;
}
ledBlinkMs = millis();
rxSpiLedToggle();
}
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++;
} else {
rxSpiLedBlink(INTERVAL_RX_LOSS_MS);
}
}
}
void rxSpiLedBlinkBind(void)
{
rxSpiLedBlink(INTERVAL_RX_BIND_MS);
}
void rxSpiBind(void)
{
bindRequested = true;
}
bool rxSpiCheckBindRequested(bool reset)
{
if (bindPin) {
bool bindPinStatus = IORead(bindPin);
if (lastBindPinStatus && !bindPinStatus) {
bindRequested = true;
}
lastBindPinStatus = bindPinStatus;
}
if (!bindRequested) {
return false;
} else {
if (reset) {
bindRequested = false;
}
return true;
}
}
#endif

View File

@ -0,0 +1,39 @@
/*
* 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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "rx/rx_spi.h"
#define INTERVAL_RX_LOSS_MS 1000
#define INTERVAL_RX_BIND_MS 250
#define RX_LOSS_COUNT 1000
void rxSpiCommonIOInit(const rxSpiConfig_t *rxSpiConfig);
void rxSpiLedOn(void);
void rxSpiLedOff(void);
void rxSpiLedToggle(void);
void rxSpiLedBlink(timeMs_t blinkMs);
void rxSpiLedBlinkRxLoss(rx_spi_received_e result);
void rxSpiLedBlinkBind(void);
void rxSpiBind(void);
bool rxSpiCheckBindRequested(bool reset);

View File

@ -141,8 +141,7 @@
#define RX_NSS_PIN SPI2_NSS_PIN #define RX_NSS_PIN SPI2_NSS_PIN
#define RX_IRQ_PIN PA8 #define RX_IRQ_PIN PA8
#define BINDPLUG_PIN PA9 #define BINDPLUG_PIN PA9
#define USE_RX_FLYSKY_SPI_LED #define RX_SPI_LED_PIN PA10
#define RX_FLYSKY_SPI_LED_PIN PA10
#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_MOTOR_STOP) #define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_MOTOR_STOP)
#elif defined(CRAZYBEEF3DX) #elif defined(CRAZYBEEF3DX)
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
@ -161,7 +160,7 @@
#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_CC2500_SPI_GDO_0_PIN PA8 #define RX_CC2500_SPI_GDO_0_PIN PA8
#define RX_CC2500_SPI_LED_PIN PA10 #define RX_SPI_LED_PIN PA10
#define BINDPLUG_PIN PA9 #define BINDPLUG_PIN PA9
#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_MOTOR_STOP) #define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_MOTOR_STOP)
#endif #endif

View File

@ -74,8 +74,8 @@
#define RX_CC2500_SPI_DISABLE_CHIP_DETECTION #define RX_CC2500_SPI_DISABLE_CHIP_DETECTION
#define RX_CC2500_SPI_GDO_0_PIN PC14 #define RX_CC2500_SPI_GDO_0_PIN PC14
#define RX_CC2500_SPI_LED_PIN PB9 #define RX_SPI_LED_PIN PB9
#define RX_CC2500_SPI_LED_PIN_INVERTED #define RX_SPI_LED_INVERTED
#define USE_RX_CC2500_SPI_PA_LNA #define USE_RX_CC2500_SPI_PA_LNA
#define RX_CC2500_SPI_TX_EN_PIN PA8 #define RX_CC2500_SPI_TX_EN_PIN PA8

View File

@ -113,8 +113,7 @@
#define RX_CC2500_SPI_GDO_0_PIN PB0 #define RX_CC2500_SPI_GDO_0_PIN PB0
#define RX_CC2500_SPI_LED_PIN PB6 #define RX_SPI_LED_PIN PB6
#define USE_RX_CC2500_SPI_PA_LNA #define USE_RX_CC2500_SPI_PA_LNA

View File

@ -138,13 +138,11 @@
#if defined(USE_RX_FRSKY_SPI_D) || defined(USE_RX_FRSKY_SPI_X) #if defined(USE_RX_FRSKY_SPI_D) || defined(USE_RX_FRSKY_SPI_X)
#define USE_RX_CC2500 #define USE_RX_CC2500
#define USE_RX_CC2500_BIND
#define USE_RX_FRSKY_SPI #define USE_RX_FRSKY_SPI
#endif #endif
#if defined(USE_RX_SFHSS_SPI) #if defined(USE_RX_SFHSS_SPI)
#define USE_RX_CC2500 #define USE_RX_CC2500
#define USE_RX_CC2500_BIND
#endif #endif
// Burst dshot to default off if not configured explicitly by target // Burst dshot to default off if not configured explicitly by target