diff --git a/src/main/build/debug.c b/src/main/build/debug.c index a3e963c2e7..a6a83a8796 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -49,5 +49,6 @@ const char * const debugModeNames[DEBUG_COUNT] = { "ALTITUDE", "FFT", "FFT_TIME", - "FFT_FREQ" + "FFT_FREQ", + "FRSKY_D_RX" }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 120c3b8105..7de7bc247e 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -68,6 +68,7 @@ typedef enum { DEBUG_FFT, DEBUG_FFT_TIME, DEBUG_FFT_FREQ, + DEBUG_FRSKY_D_RX, DEBUG_COUNT } debugType_e; diff --git a/src/main/drivers/cc2500.c b/src/main/drivers/cc2500.c index b85b3d600c..07aecd425d 100644 --- a/src/main/drivers/cc2500.c +++ b/src/main/drivers/cc2500.c @@ -7,56 +7,56 @@ #include "platform.h" -#ifdef USE_RX_CC2500 - #include "build/build_config.h" -#include "cc2500.h" -#include "io.h" -#include "rx_spi.h" -#include "system.h" -#include "time.h" + +#include "drivers/cc2500.h" +#include "drivers/io.h" +#include "drivers/rx_spi.h" +#include "drivers/system.h" +#include "drivers/time.h" + #define NOP 0xFF -uint8_t cc2500_readFifo(uint8_t *dpbuffer, uint8_t len) +uint8_t cc2500ReadFifo(uint8_t *dpbuffer, uint8_t len) { return rxSpiReadCommandMulti(CC2500_3F_RXFIFO | CC2500_READ_BURST, NOP, dpbuffer, len); } -uint8_t cc2500_writeFifo(uint8_t *dpbuffer, uint8_t len) +uint8_t cc2500WriteFifo(uint8_t *dpbuffer, uint8_t len) { uint8_t ret; - cc2500_strobe(CC2500_SFTX); // 0x3B SFTX + cc2500Strobe(CC2500_SFTX); // 0x3B SFTX ret = rxSpiWriteCommandMulti(CC2500_3F_TXFIFO | CC2500_WRITE_BURST, dpbuffer, len); - cc2500_strobe(CC2500_STX); // 0x35 + cc2500Strobe(CC2500_STX); // 0x35 return ret; } -uint8_t cc2500_ReadRegisterMulti(uint8_t address, uint8_t *data, uint8_t length) +uint8_t cc2500ReadRegisterMulti(uint8_t address, uint8_t *data, uint8_t length) { return rxSpiReadCommandMulti(address, NOP, data, length); } -uint8_t cc2500_WriteRegisterMulti(uint8_t address, uint8_t *data, +uint8_t cc2500WriteRegisterMulti(uint8_t address, uint8_t *data, uint8_t length) { return rxSpiWriteCommandMulti(address, data, length); } -uint8_t cc2500_readReg(uint8_t reg) +uint8_t cc2500ReadReg(uint8_t reg) { return rxSpiReadCommand(reg | 0x80, NOP); } -void cc2500_strobe(uint8_t address) { rxSpiWriteByte(address); } +void cc2500Strobe(uint8_t address) { rxSpiWriteByte(address); } -uint8_t cc2500_writeReg(uint8_t address, uint8_t data) +uint8_t cc2500WriteReg(uint8_t address, uint8_t data) { return rxSpiWriteCommand(address, data); } -void CC2500_SetPower(uint8_t power) +void cc2500SetPower(uint8_t power) { const uint8_t patable[8] = { 0xC5, // -12dbm @@ -70,16 +70,15 @@ void CC2500_SetPower(uint8_t power) }; if (power > 7) power = 7; - cc2500_writeReg(CC2500_3E_PATABLE, patable[power]); + cc2500WriteReg(CC2500_3E_PATABLE, patable[power]); } -uint8_t CC2500_Reset() +uint8_t cc2500Reset() { - cc2500_strobe(CC2500_SRES); + cc2500Strobe(CC2500_SRES); delayMicroseconds(1000); // 1000us // CC2500_SetTxRxMode(TXRX_OFF); // RX_EN_off;//off tx // TX_EN_off;//off rx - return cc2500_readReg(CC2500_0E_FREQ1) == 0xC4; // check if reset + return cc2500ReadReg(CC2500_0E_FREQ1) == 0xC4; // check if reset } -#endif diff --git a/src/main/drivers/cc2500.h b/src/main/drivers/cc2500.h index 88530127b1..899ddb7247 100644 --- a/src/main/drivers/cc2500.h +++ b/src/main/drivers/cc2500.h @@ -138,16 +138,16 @@ enum { #define CC2500_LQI_CRC_OK_BM 0x80 #define CC2500_LQI_EST_BM 0x7F -// void FrskyRXspiInit(); -uint8_t cc2500_readFifo(uint8_t *dpbuffer, uint8_t len); -uint8_t cc2500_writeFifo(uint8_t *dpbuffer, uint8_t len); -uint8_t cc2500_ReadRegisterMulti(uint8_t address, uint8_t *data, +uint8_t cc2500ReadFifo(uint8_t *dpbuffer, uint8_t len); +uint8_t cc2500WriteFifo(uint8_t *dpbuffer, uint8_t len); + +uint8_t cc2500ReadRegisterMulti(uint8_t address, uint8_t *data, uint8_t length); -uint8_t CC2500_WriteRegisterMulti(uint8_t address, uint8_t *data, +uint8_t cc2500WriteRegisterMulti(uint8_t address, uint8_t *data, uint8_t length); -uint8_t cc2500_readReg(uint8_t reg); -void cc2500_strobe(uint8_t address); -uint8_t cc2500_writeReg(uint8_t address, uint8_t data); -void CC2500_SetPower(uint8_t power); -uint8_t CC2500_Reset(); +uint8_t cc2500ReadReg(uint8_t reg); +void cc2500Strobe(uint8_t address); +uint8_t cc2500WriteReg(uint8_t address, uint8_t data); +void cc2500SetPower(uint8_t power); +uint8_t cc2500Reset(); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 40e89b8aa1..744c91ef23 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2110,7 +2110,7 @@ static void cliBeeper(char *cmdline) } #endif -#ifdef FRSKY_BIND +#ifdef USE_RX_FRSKY_D void cliFrSkyBind(char *cmdline){ UNUSED(cmdline); frSkyDBind(); @@ -3491,7 +3491,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n" "\t<+|->[name]", cliBeeper), #endif -#ifdef FRSKY_BIND +#ifdef USE_RX_FRSKY_D CLI_COMMAND_DEF("frsky_bind", NULL, NULL, cliFrSkyBind), #endif #ifdef LED_STRIP diff --git a/src/main/rx/frsky_d.c b/src/main/rx/frsky_d.c index 4e1ae76015..7e5cead456 100644 --- a/src/main/rx/frsky_d.c +++ b/src/main/rx/frsky_d.c @@ -25,113 +25,70 @@ #ifdef USE_RX_FRSKY_D #include "build/build_config.h" +#include "build/debug.h" -#include "common/utils.h" #include "common/maths.h" +#include "common/utils.h" #include "drivers/cc2500.h" #include "drivers/io.h" #include "drivers/system.h" #include "drivers/time.h" +#include "fc/config.h" + +#include "config/feature.h" +#include "config/parameter_group_ids.h" + #include "rx/rx.h" #include "rx/rx_spi.h" #include "rx/frsky_d.h" #include "sensors/battery.h" -#include "fc/config.h" - -#include "config/feature.h" -#include "config/parameter_group_ids.h" - #include "telemetry/frsky.h" #define RC_CHANNEL_COUNT 8 #define MAX_MISSING_PKT 100 +#define DEBUG_DATA_ERROR_COUNT 0 + #define SYNC 9000 #define FS_THR 960 -#define FLED_on \ - { \ - IOHi(IOGetByTag(IO_TAG(FRSKY_LED_PIN))); \ - } -#define FLED_off \ - { \ - IOLo(IOGetByTag(IO_TAG(FRSKY_LED_PIN))); \ - } -#define GDO_1 IORead(IOGetByTag(IO_TAG(GDO_0_PIN))) - -#if defined(PA_LNA) -#define TX_EN_on \ - { \ - IOHi(IOGetByTag(IO_TAG(TX_EN_PIN))); \ - } -#define TX_EN_off \ - { \ - IOLo(IOGetByTag(IO_TAG(TX_EN_PIN))); \ - } - -#define RX_EN_on \ - { \ - IOHi(IOGetByTag(IO_TAG(RX_EN_PIN))); \ - } -#define RX_EN_off \ - { \ - IOLo(IOGetByTag(IO_TAG(RX_EN_PIN))); \ - } - -#if defined(DIVERSITY) -#define ANT_SEL_on \ - { \ - IOHi(IOGetByTag(IO_TAG(ANT_SEL_PIN))); \ - } -#define ANT_SEL_off \ - { \ - IOLo(IOGetByTag(IO_TAG(ANT_SEL_PIN))); \ - } -#endif -#endif - -static uint8_t ccLen; -static uint8_t channr; static uint32_t missingPackets; static uint8_t calData[255][3]; static uint32_t time_tune; static uint8_t listLength; static uint8_t bindIdx; static uint8_t cnt; -static uint16_t Servo_data[RC_CHANNEL_COUNT]; -static uint16_t c[8]; -static uint32_t t_out; +static int32_t t_out; static uint8_t Lqi; -static uint32_t packet_timer; -static uint8_t protocol_state; -static int16_t word_temp; +static timeUs_t lastPacketReceivedTime; +static uint8_t protocolState; static uint32_t start_time; static int8_t bindOffset; +static uint16_t dataErrorCount = 0; -static IO_t GdoPin; -static IO_t BindPin = DEFIO_IO(NONE); +static IO_t gdoPin; +static IO_t bindPin = DEFIO_IO(NONE); static bool lastBindPinStatus; -static IO_t FrskyLedPin; -#if defined(PA_LNA) -static IO_t TxEnPin; -static IO_t RxEnPin; -static IO_t AntSelPin; +static IO_t frSkyLedPin; +#if defined(USE_FRSKY_RX_PA_LNA) +static IO_t txEnPin; +static IO_t rxEnPin; +static IO_t antSelPin; static uint8_t pass; #endif bool bindRequested = false; -#ifdef FRSKY_TELEMETRY +#ifdef USE_FRSKY_D_TELEMETRY static uint8_t frame[20]; static int16_t RSSI_dBm; static uint8_t telemetry_id; -static uint8_t telemetryRX; static uint32_t telemetryTime; -#if defined(HUB) +#if defined(TELEMETRY_FRSKY) #define MAX_SERIAL_BYTES 64 static uint8_t hub_index; static uint8_t idxx = 0; @@ -154,8 +111,6 @@ PG_RESET_TEMPLATE(frSkyDConfig_t, frSkyDConfig, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} ); -rx_spi_received_e ret; - enum { STATE_INIT = 0, STATE_BIND, @@ -166,22 +121,22 @@ enum { STATE_STARTING, STATE_UPDATE, STATE_DATA, - STATE_TELEM + STATE_TELEMETRY }; -#if defined(FRSKY_TELEMETRY) +#if defined(USE_FRSKY_D_TELEMETRY) static void compute_RSSIdbm(uint8_t *packet) { - if (packet[ccLen - 2] >= 128) { - RSSI_dBm = ((((uint16_t)packet[ccLen - 2]) * 18) >> 5) - 82; + if (packet[18] >= 128) { + RSSI_dBm = ((((uint16_t)packet[18]) * 18) >> 5) - 82; } else { - RSSI_dBm = ((((uint16_t)packet[ccLen - 2]) * 18) >> 5) + 65; + RSSI_dBm = ((((uint16_t)packet[18]) * 18) >> 5) + 65; } processRssi(constrain((RSSI_dBm << 3) / 10, 0, 100)); } -#if defined(HUB) +#if defined(TELEMETRY_FRSKY) static uint8_t frsky_append_hub_data(uint8_t *buf) { if (telemetry_id == telemetry_expected_id) @@ -228,7 +183,7 @@ static void telemetry_build_frame(uint8_t *packet) frame[3] = (uint8_t)((adcExternal1Sample & 0xff0) >> 4); // A1 frame[4] = (uint8_t)((adcRssiSample & 0xff0) >> 4); // A2 frame[5] = (uint8_t)RSSI_dBm; -#if defined(HUB) +#if defined(TELEMETRY_FRSKY) bytes_used = frsky_append_hub_data(&frame[8]); #endif frame[6] = bytes_used; @@ -237,17 +192,17 @@ static void telemetry_build_frame(uint8_t *packet) #endif -#if defined(PA_LNA) +#if defined(USE_FRSKY_RX_PA_LNA) static void RX_enable() { - TX_EN_off; - RX_EN_on; + IOLo(txEnPin); + IOHi(rxEnPin); } static void TX_enable() { - RX_EN_off; - TX_EN_on; + IOLo(rxEnPin); + IOHi(txEnPin); } #endif @@ -258,82 +213,82 @@ void frSkyDBind() static void initialize() { - CC2500_Reset(); - cc2500_writeReg(CC2500_02_IOCFG0, 0x01); - cc2500_writeReg(CC2500_17_MCSM1, 0x0C); - cc2500_writeReg(CC2500_18_MCSM0, 0x18); - cc2500_writeReg(CC2500_06_PKTLEN, 0x19); - cc2500_writeReg(CC2500_08_PKTCTRL0, 0x05); - cc2500_writeReg(CC2500_3E_PATABLE, 0xFF); - cc2500_writeReg(CC2500_0B_FSCTRL1, 0x08); - cc2500_writeReg(CC2500_0C_FSCTRL0, 0x00); - cc2500_writeReg(CC2500_0D_FREQ2, 0x5C); - cc2500_writeReg(CC2500_0E_FREQ1, 0x76); - cc2500_writeReg(CC2500_0F_FREQ0, 0x27); - cc2500_writeReg(CC2500_10_MDMCFG4, 0xAA); - cc2500_writeReg(CC2500_11_MDMCFG3, 0x39); - cc2500_writeReg(CC2500_12_MDMCFG2, 0x11); - cc2500_writeReg(CC2500_13_MDMCFG1, 0x23); - cc2500_writeReg(CC2500_14_MDMCFG0, 0x7A); - cc2500_writeReg(CC2500_15_DEVIATN, 0x42); - cc2500_writeReg(CC2500_19_FOCCFG, 0x16); - cc2500_writeReg(CC2500_1A_BSCFG, 0x6C); - cc2500_writeReg(CC2500_1B_AGCCTRL2, 0x03); - cc2500_writeReg(CC2500_1C_AGCCTRL1, 0x40); - cc2500_writeReg(CC2500_1D_AGCCTRL0, 0x91); - cc2500_writeReg(CC2500_21_FREND1, 0x56); - cc2500_writeReg(CC2500_22_FREND0, 0x10); - cc2500_writeReg(CC2500_23_FSCAL3, 0xA9); - cc2500_writeReg(CC2500_24_FSCAL2, 0x0A); - cc2500_writeReg(CC2500_25_FSCAL1, 0x00); - cc2500_writeReg(CC2500_26_FSCAL0, 0x11); - cc2500_writeReg(CC2500_29_FSTEST, 0x59); - cc2500_writeReg(CC2500_2C_TEST2, 0x88); - cc2500_writeReg(CC2500_2D_TEST1, 0x31); - cc2500_writeReg(CC2500_2E_TEST0, 0x0B); - cc2500_writeReg(CC2500_03_FIFOTHR, 0x07); - cc2500_writeReg(CC2500_09_ADDR, 0x00); - cc2500_strobe(CC2500_SIDLE); + cc2500Reset(); + cc2500WriteReg(CC2500_02_IOCFG0, 0x01); + cc2500WriteReg(CC2500_17_MCSM1, 0x0C); + cc2500WriteReg(CC2500_18_MCSM0, 0x18); + cc2500WriteReg(CC2500_06_PKTLEN, 0x19); + cc2500WriteReg(CC2500_08_PKTCTRL0, 0x05); + cc2500WriteReg(CC2500_3E_PATABLE, 0xFF); + cc2500WriteReg(CC2500_0B_FSCTRL1, 0x08); + cc2500WriteReg(CC2500_0C_FSCTRL0, 0x00); + cc2500WriteReg(CC2500_0D_FREQ2, 0x5C); + cc2500WriteReg(CC2500_0E_FREQ1, 0x76); + cc2500WriteReg(CC2500_0F_FREQ0, 0x27); + cc2500WriteReg(CC2500_10_MDMCFG4, 0xAA); + cc2500WriteReg(CC2500_11_MDMCFG3, 0x39); + cc2500WriteReg(CC2500_12_MDMCFG2, 0x11); + cc2500WriteReg(CC2500_13_MDMCFG1, 0x23); + cc2500WriteReg(CC2500_14_MDMCFG0, 0x7A); + cc2500WriteReg(CC2500_15_DEVIATN, 0x42); + cc2500WriteReg(CC2500_19_FOCCFG, 0x16); + cc2500WriteReg(CC2500_1A_BSCFG, 0x6C); + cc2500WriteReg(CC2500_1B_AGCCTRL2, 0x03); + cc2500WriteReg(CC2500_1C_AGCCTRL1, 0x40); + cc2500WriteReg(CC2500_1D_AGCCTRL0, 0x91); + cc2500WriteReg(CC2500_21_FREND1, 0x56); + cc2500WriteReg(CC2500_22_FREND0, 0x10); + cc2500WriteReg(CC2500_23_FSCAL3, 0xA9); + cc2500WriteReg(CC2500_24_FSCAL2, 0x0A); + cc2500WriteReg(CC2500_25_FSCAL1, 0x00); + cc2500WriteReg(CC2500_26_FSCAL0, 0x11); + cc2500WriteReg(CC2500_29_FSTEST, 0x59); + cc2500WriteReg(CC2500_2C_TEST2, 0x88); + cc2500WriteReg(CC2500_2D_TEST1, 0x31); + cc2500WriteReg(CC2500_2E_TEST0, 0x0B); + cc2500WriteReg(CC2500_03_FIFOTHR, 0x07); + cc2500WriteReg(CC2500_09_ADDR, 0x00); + cc2500Strobe(CC2500_SIDLE); - cc2500_writeReg(CC2500_07_PKTCTRL1, 0x04); - cc2500_writeReg(CC2500_0C_FSCTRL0, 0); + cc2500WriteReg(CC2500_07_PKTCTRL1, 0x04); + cc2500WriteReg(CC2500_0C_FSCTRL0, 0); for (uint8_t c = 0; c < 0xFF; c++) { - cc2500_strobe(CC2500_SIDLE); - cc2500_writeReg(CC2500_0A_CHANNR, c); - cc2500_strobe(CC2500_SCAL); + cc2500Strobe(CC2500_SIDLE); + cc2500WriteReg(CC2500_0A_CHANNR, c); + cc2500Strobe(CC2500_SCAL); delayMicroseconds(900); - calData[c][0] = cc2500_readReg(CC2500_23_FSCAL3); - calData[c][1] = cc2500_readReg(CC2500_24_FSCAL2); - calData[c][2] = cc2500_readReg(CC2500_25_FSCAL1); + calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3); + calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2); + calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1); } } static void initialize_data(uint8_t adr) { - cc2500_writeReg(CC2500_0C_FSCTRL0, (uint8_t)frSkyDConfig()->bindOffset); - cc2500_writeReg(CC2500_18_MCSM0, 0x8); - cc2500_writeReg(CC2500_09_ADDR, adr ? 0x03 : frSkyDConfig()->bindTxId[0]); - cc2500_writeReg(CC2500_07_PKTCTRL1, 0x0D); - cc2500_writeReg(CC2500_19_FOCCFG, 0x16); + cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)frSkyDConfig()->bindOffset); + cc2500WriteReg(CC2500_18_MCSM0, 0x8); + cc2500WriteReg(CC2500_09_ADDR, adr ? 0x03 : frSkyDConfig()->bindTxId[0]); + cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D); + cc2500WriteReg(CC2500_19_FOCCFG, 0x16); delay(10); } static void initTuneRx(void) { - cc2500_writeReg(CC2500_19_FOCCFG, 0x14); + cc2500WriteReg(CC2500_19_FOCCFG, 0x14); time_tune = millis(); bindOffset = -126; - cc2500_writeReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset); - cc2500_writeReg(CC2500_07_PKTCTRL1, 0x0C); - cc2500_writeReg(CC2500_18_MCSM0, 0x8); + cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset); + cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0C); + cc2500WriteReg(CC2500_18_MCSM0, 0x8); - cc2500_strobe(CC2500_SIDLE); - cc2500_writeReg(CC2500_23_FSCAL3, calData[0][0]); - cc2500_writeReg(CC2500_24_FSCAL2, calData[0][1]); - cc2500_writeReg(CC2500_25_FSCAL1, calData[0][2]); - cc2500_writeReg(CC2500_0A_CHANNR, 0); - cc2500_strobe(CC2500_SFRX); - cc2500_strobe(CC2500_SRX); + cc2500Strobe(CC2500_SIDLE); + cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]); + cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]); + cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]); + cc2500WriteReg(CC2500_0A_CHANNR, 0); + cc2500Strobe(CC2500_SFRX); + cc2500Strobe(CC2500_SRX); } static bool tuneRx(uint8_t *packet) @@ -344,12 +299,12 @@ static bool tuneRx(uint8_t *packet) if ((millis() - time_tune) > 50) { time_tune = millis(); bindOffset += 5; - cc2500_writeReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset); + cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset); } - if (GDO_1) { - ccLen = cc2500_readReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + if (IORead(gdoPin)) { + uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; if (ccLen) { - cc2500_readFifo(packet, ccLen); + cc2500ReadFifo(packet, ccLen); if (packet[ccLen - 1] & 0x80) { if (packet[2] == 0x01) { Lqi = packet[ccLen - 1] & 0x7F; @@ -368,15 +323,15 @@ static bool tuneRx(uint8_t *packet) static void initGetBind(void) { - cc2500_strobe(CC2500_SIDLE); - cc2500_writeReg(CC2500_23_FSCAL3, calData[0][0]); - cc2500_writeReg(CC2500_24_FSCAL2, calData[0][1]); - cc2500_writeReg(CC2500_25_FSCAL1, calData[0][2]); - cc2500_writeReg(CC2500_0A_CHANNR, 0); - cc2500_strobe(CC2500_SFRX); + cc2500Strobe(CC2500_SIDLE); + cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]); + cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]); + cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]); + cc2500WriteReg(CC2500_0A_CHANNR, 0); + cc2500Strobe(CC2500_SFRX); delayMicroseconds(20); // waiting flush FIFO - cc2500_strobe(CC2500_SRX); + cc2500Strobe(CC2500_SRX); listLength = 0; bindIdx = 0x05; } @@ -386,10 +341,10 @@ static bool getBind1(uint8_t *packet) // 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| // Start by getting bind packet 0 and the txid - if (GDO_1) { - ccLen = cc2500_readReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + if (IORead(gdoPin)) { + uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; if (ccLen) { - cc2500_readFifo(packet, ccLen); + cc2500ReadFifo(packet, ccLen); if (packet[ccLen - 1] & 0x80) { if (packet[2] == 0x01) { if (packet[5] == 0x00) { @@ -413,11 +368,10 @@ static bool getBind1(uint8_t *packet) static bool getBind2(uint8_t *packet) { if (bindIdx <= 120) { - if (GDO_1) { - ccLen = - cc2500_readReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + if (IORead(gdoPin)) { + uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; if (ccLen) { - cc2500_readFifo(packet, ccLen); + cc2500ReadFifo(packet, ccLen); if (packet[ccLen - 1] & 0x80) { if (packet[2] == 0x01) { if ((packet[3] == frSkyDConfig()->bindTxId[0]) && @@ -467,27 +421,32 @@ static bool getBind2(uint8_t *packet) static void nextChannel(uint8_t skip) { + static uint8_t channr = 0; + channr += skip; - if (channr >= listLength) + while (channr >= listLength) { channr -= listLength; - cc2500_strobe(CC2500_SIDLE); - cc2500_writeReg(CC2500_23_FSCAL3, + } + cc2500Strobe(CC2500_SIDLE); + cc2500WriteReg(CC2500_23_FSCAL3, calData[frSkyDConfig()->bindHopData[channr]][0]); - cc2500_writeReg(CC2500_24_FSCAL2, + cc2500WriteReg(CC2500_24_FSCAL2, calData[frSkyDConfig()->bindHopData[channr]][1]); - cc2500_writeReg(CC2500_25_FSCAL1, + cc2500WriteReg(CC2500_25_FSCAL1, calData[frSkyDConfig()->bindHopData[channr]][2]); - cc2500_writeReg(CC2500_0A_CHANNR, frSkyDConfig()->bindHopData[channr]); - cc2500_strobe(CC2500_SFRX); + cc2500WriteReg(CC2500_0A_CHANNR, frSkyDConfig()->bindHopData[channr]); + cc2500Strobe(CC2500_SFRX); } static bool checkBindRequested(bool reset) { - bool bindPinStatus = IORead(BindPin); - if (lastBindPinStatus && !bindPinStatus) { - bindRequested = true; + if (bindPin) { + bool bindPinStatus = IORead(bindPin); + if (lastBindPinStatus && !bindPinStatus) { + bindRequested = true; + } + lastBindPinStatus = bindPinStatus; } - lastBindPinStatus = bindPinStatus; if (!bindRequested) { return false; @@ -500,46 +459,63 @@ static bool checkBindRequested(bool reset) } } -void frskyD_Rx_SetRCdata(uint16_t *rcData, const uint8_t *packet) -{ - c[0] = (uint16_t)(((packet[10] & 0x0F) << 8 | packet[6])); - c[1] = (uint16_t)(((packet[10] & 0xF0) << 4 | packet[7])); - c[2] = (uint16_t)(((packet[11] & 0x0F) << 8 | packet[8])); - c[3] = (uint16_t)(((packet[11] & 0xF0) << 4 | packet[9])); - c[4] = (uint16_t)(((packet[16] & 0x0F) << 8 | packet[12])); - c[5] = (uint16_t)(((packet[16] & 0xF0) << 4 | packet[13])); - c[6] = (uint16_t)(((packet[17] & 0x0F) << 8 | packet[14])); - c[7] = (uint16_t)(((packet[17] & 0xF0) << 4 | packet[15])); +#define FRSKY_D_CHANNEL_SCALING (2.0f / 3) - for (uint8_t i = 0; i < 8; i++) { - word_temp = 0.667 * c[i]; - if ((word_temp > 800) && (word_temp < 2200)) - Servo_data[i] = word_temp; - rcData[i] = Servo_data[i]; +static void decodeChannelPair(uint16_t *channels, const uint8_t *packet, const uint8_t highNibbleOffset) { + channels[0] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf) << 8 | packet[0]); + channels[1] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf0) << 4 | packet[1]); +} + +void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet) +{ + uint16_t channels[RC_CHANNEL_COUNT]; + bool dataError = false; + + decodeChannelPair(channels, packet + 6, 4); + decodeChannelPair(channels + 2, packet + 8, 3); + decodeChannelPair(channels + 4, packet + 12, 4); + decodeChannelPair(channels + 6, packet + 14, 3); + + for (int i = 0; i < RC_CHANNEL_COUNT; i++) { + if ((channels[i] < 800) || (channels[i] > 2200)) { + dataError = true; + + break; + } + } + + if (!dataError) { + for (int i = 0; i < RC_CHANNEL_COUNT; i++) { + rcData[i] = channels[i]; + } + } else { + DEBUG_SET(DEBUG_FRSKY_D_RX, DEBUG_DATA_ERROR_COUNT, ++dataErrorCount); } } -rx_spi_received_e frskyD_Rx_DataReceived(uint8_t *packet) +rx_spi_received_e frSkyDDataReceived(uint8_t *packet) { - ret = RX_SPI_RECEIVED_NONE; + const timeUs_t currentPacketReceivedTime = micros(); - switch (protocol_state) { + rx_spi_received_e ret = RX_SPI_RECEIVED_NONE; + + switch (protocolState) { case STATE_INIT: if ((millis() - start_time) > 10) { initialize(); - protocol_state = STATE_BIND; + protocolState = STATE_BIND; } break; case STATE_BIND: if (checkBindRequested(true) || frSkyDConfig()->autoBind) { - FLED_on; + IOHi(frSkyLedPin); initTuneRx(); - protocol_state = STATE_BIND_TUNING; + protocolState = STATE_BIND_TUNING; } else { - protocol_state = STATE_STARTING; + protocolState = STATE_STARTING; } break; @@ -548,21 +524,21 @@ rx_spi_received_e frskyD_Rx_DataReceived(uint8_t *packet) initGetBind(); initialize_data(1); - protocol_state = STATE_BIND_BINDING1; + protocolState = STATE_BIND_BINDING1; } break; case STATE_BIND_BINDING1: if (getBind1(packet)) { - protocol_state = STATE_BIND_BINDING2; + protocolState = STATE_BIND_BINDING2; } break; case STATE_BIND_BINDING2: if (getBind2(packet)) { - cc2500_strobe(CC2500_SIDLE); + cc2500Strobe(CC2500_SIDLE); - protocol_state = STATE_BIND_COMPLETE; + protocolState = STATE_BIND_COMPLETE; } break; @@ -572,86 +548,83 @@ rx_spi_received_e frskyD_Rx_DataReceived(uint8_t *packet) } else { uint8_t ctr = 40; while (ctr--) { - FLED_on; + IOHi(frSkyLedPin); delay(50); - FLED_off; + IOLo(frSkyLedPin); delay(50); } } - protocol_state = STATE_STARTING; + protocolState = STATE_STARTING; break; case STATE_STARTING: listLength = 47; initialize_data(0); - protocol_state = STATE_UPDATE; + protocolState = STATE_UPDATE; nextChannel(1); // - cc2500_strobe(CC2500_SRX); + cc2500Strobe(CC2500_SRX); ret = RX_SPI_RECEIVED_BIND; break; case STATE_UPDATE: - packet_timer = micros(); - protocol_state = STATE_DATA; + lastPacketReceivedTime = currentPacketReceivedTime; + protocolState = STATE_DATA; if (checkBindRequested(false)) { - packet_timer = 0; + lastPacketReceivedTime = 0; t_out = 50; missingPackets = 0; - protocol_state = STATE_INIT; + protocolState = STATE_INIT; break; } // here FS code could be case STATE_DATA: - if (GDO_1) { - ccLen = - cc2500_readReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; - if (ccLen > 20) - ccLen = 20; - if (ccLen == 20) { - cc2500_readFifo(packet, ccLen); - if (packet[ccLen - 1] & 0x80) { + if (IORead(gdoPin)) { + uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + if (ccLen >= 20) { + cc2500ReadFifo(packet, 20); + if (packet[19] & 0x80) { missingPackets = 0; t_out = 1; if (packet[0] == 0x11) { if ((packet[1] == frSkyDConfig()->bindTxId[0]) && (packet[2] == frSkyDConfig()->bindTxId[1])) { - FLED_on; + IOHi(frSkyLedPin); nextChannel(1); -#ifdef FRSKY_TELEMETRY +#ifdef USE_FRSKY_D_TELEMETRY if ((packet[3] % 4) == 2) { - telemetryRX = 1; telemetryTime = micros(); compute_RSSIdbm(packet); telemetry_build_frame(packet); - protocol_state = STATE_TELEM; + protocolState = STATE_TELEMETRY; } else #endif { - cc2500_strobe(CC2500_SRX); - protocol_state = STATE_UPDATE; + cc2500Strobe(CC2500_SRX); + protocolState = STATE_UPDATE; } ret = RX_SPI_RECEIVED_DATA; - packet_timer = micros(); + lastPacketReceivedTime = currentPacketReceivedTime; } } } } } - if ((micros() - packet_timer) > (t_out * SYNC)) { -#ifdef PA_LNA + + if (cmp32(currentPacketReceivedTime, lastPacketReceivedTime) > (t_out * SYNC)) { +#ifdef USE_FRSKY_RX_PA_LNA RX_enable(); #endif if (t_out == 1) { -#ifdef DIVERSITY // SE4311 chip +#ifdef USE_FRSKY_RX_DIVERSITY // SE4311 chip if (missingPackets >= 2) { if (pass & 0x01) { - ANT_SEL_on; + IOHi(antSelPin); } else { - ANT_SEL_off; + IOLo(antSelPin); } pass++; } @@ -664,34 +637,31 @@ rx_spi_received_e frskyD_Rx_DataReceived(uint8_t *packet) nextChannel(1); } else { if (cnt++ & 0x01) { - FLED_off; + IOLo(frSkyLedPin); } else - FLED_on; + IOHi(frSkyLedPin); nextChannel(13); } - cc2500_strobe(CC2500_SRX); - protocol_state = STATE_UPDATE; + cc2500Strobe(CC2500_SRX); + protocolState = STATE_UPDATE; } break; -#ifdef FRSKY_TELEMETRY - case STATE_TELEM: - if (telemetryRX) { - if ((micros() - telemetryTime) >= 1380) { - cc2500_strobe(CC2500_SIDLE); - CC2500_SetPower(6); - cc2500_strobe(CC2500_SFRX); -#if defined(PA_LNA) - TX_enable(); +#ifdef USE_FRSKY_D_TELEMETRY + case STATE_TELEMETRY: + if ((micros() - telemetryTime) >= 1380) { + cc2500Strobe(CC2500_SIDLE); + cc2500SetPower(6); + cc2500Strobe(CC2500_SFRX); +#if defined(USE_FRSKY_RX_PA_LNA) + TX_enable(); #endif - cc2500_strobe(CC2500_SIDLE); - cc2500_writeFifo(frame, frame[0] + 1); - protocol_state = STATE_DATA; - ret = RX_SPI_RECEIVED_DATA; - packet_timer = micros(); - telemetryRX = 0; - } + cc2500Strobe(CC2500_SIDLE); + cc2500WriteFifo(frame, frame[0] + 1); + protocolState = STATE_DATA; + ret = RX_SPI_RECEIVED_DATA; + lastPacketReceivedTime = currentPacketReceivedTime; } break; @@ -701,58 +671,59 @@ rx_spi_received_e frskyD_Rx_DataReceived(uint8_t *packet) return ret; } -void frskyD_Rx_Setup(rx_spi_protocol_e protocol) +static void frskyD_Rx_Setup(rx_spi_protocol_e protocol) { UNUSED(protocol); // gpio init here - GdoPin = IOGetByTag(IO_TAG(GDO_0_PIN)); - BindPin = IOGetByTag(IO_TAG(BINDPLUG_PIN)); - FrskyLedPin = IOGetByTag(IO_TAG(FRSKY_LED_PIN)); -#if defined(PA_LNA) - RxEnPin = IOGetByTag(IO_TAG(RX_EN_PIN)); - TxEnPin = IOGetByTag(IO_TAG(TX_EN_PIN)); - IOInit(RxEnPin, OWNER_RX_BIND, 0); - IOInit(TxEnPin, OWNER_RX_BIND, 0); - IOConfigGPIO(RxEnPin, IOCFG_OUT_PP); - IOConfigGPIO(TxEnPin, IOCFG_OUT_PP); + gdoPin = IOGetByTag(IO_TAG(FRSKY_RX_GDO_0_PIN)); + IOInit(gdoPin, OWNER_RX_BIND, 0); + IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING); + frSkyLedPin = IOGetByTag(IO_TAG(FRSKY_RX_LED_PIN)); + IOInit(frSkyLedPin, OWNER_LED, 0); + IOConfigGPIO(frSkyLedPin, IOCFG_OUT_PP); +#if defined(USE_FRSKY_RX_PA_LNA) + rxEnPin = IOGetByTag(IO_TAG(FRSKY_RX_RX_EN_PIN)); + IOInit(rxEnPin, OWNER_RX_BIND, 0); + IOConfigGPIO(rxEnPin, IOCFG_OUT_PP); + txEnPin = IOGetByTag(IO_TAG(FRSKY_RX_TX_EN_PIN)); + IOInit(txEnPin, OWNER_RX_BIND, 0); + IOConfigGPIO(txEnPin, IOCFG_OUT_PP); #endif -#if defined(DIVERSITY) - AntSelPin = IOGetByTag(IO_TAG(ANT_SEL_PIN)); - IOInit(AntSelPin, OWNER_RX_BIND, 0); - IOConfigGPIO(AntSelPin, IOCFG_OUT_PP); +#if defined(USE_FRSKY_RX_DIVERSITY) + antSelPin = IOGetByTag(IO_TAG(FRSKY_RX_ANT_SEL_PIN)); + IOInit(antSelPin, OWNER_RX_BIND, 0); + IOConfigGPIO(antSelPin, IOCFG_OUT_PP); +#endif +#if defined(BINDPLUG_PIN) + bindPin = IOGetByTag(IO_TAG(BINDPLUG_PIN)); + IOInit(bindPin, OWNER_RX_BIND, 0); + IOConfigGPIO(bindPin, IOCFG_IPU); + + lastBindPinStatus = IORead(bindPin); #endif - // - IOInit(GdoPin, OWNER_RX_BIND, 0); - IOInit(BindPin, OWNER_RX_BIND, 0); - IOInit(FrskyLedPin, OWNER_LED, 0); - // - IOConfigGPIO(GdoPin, IOCFG_IN_FLOATING); - IOConfigGPIO(BindPin, IOCFG_IPU); - IOConfigGPIO(FrskyLedPin, IOCFG_OUT_PP); - lastBindPinStatus = IORead(BindPin); start_time = millis(); - packet_timer = 0; + lastPacketReceivedTime = 0; t_out = 50; missingPackets = 0; - protocol_state = STATE_INIT; -#if defined(DIVERSITY) - ANT_SEL_on; + protocolState = STATE_INIT; +#if defined(USE_FRSKY_RX_DIVERSITY) + IOHi(antSelPin); #endif -#if defined(PA_LNA) +#if defined(USE_FRSKY_RX_PA_LNA) RX_enable(); #endif -#if defined(HUB) +#if defined(TELEMETRY_FRSKY) initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi, &frSkyTelemetryWriteSpi); #endif - // if(!frskySpiDetect())//detect spi working routine + // if(!frSkySpiDetect())//detect spi working routine // return; } -void frskyD_Rx_Init(const rxConfig_t *rxConfig, +void frSkyDInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT; diff --git a/src/main/rx/frsky_d.h b/src/main/rx/frsky_d.h index 61998ab54e..d7c661b130 100644 --- a/src/main/rx/frsky_d.h +++ b/src/main/rx/frsky_d.h @@ -33,7 +33,7 @@ PG_DECLARE(frSkyDConfig_t, frSkyDConfig); struct rxConfig_s; struct rxRuntimeConfig_s; -void frskyD_Rx_Init(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); -void frskyD_Rx_SetRCdata(uint16_t *rcData, const uint8_t *payload); -rx_spi_received_e frskyD_Rx_DataReceived(uint8_t *payload); +void frSkyDInit(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); +void frSkyDSetRcData(uint16_t *rcData, const uint8_t *payload); +rx_spi_received_e frSkyDDataReceived(uint8_t *payload); void frSkyDBind(); diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c index 77fed49b35..a6c141b8bf 100644 --- a/src/main/rx/rx_spi.c +++ b/src/main/rx/rx_spi.c @@ -111,9 +111,9 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol) #endif #ifdef USE_RX_FRSKY_D case RX_SPI_FRSKY_D: - protocolInit = frskyD_Rx_Init; - protocolDataReceived = frskyD_Rx_DataReceived; - protocolSetRcDataFromPayload = frskyD_Rx_SetRCdata; + protocolInit = frSkyDInit; + protocolDataReceived = frSkyDDataReceived; + protocolSetRcDataFromPayload = frSkyDSetRcData; break; #endif } diff --git a/src/main/target/MIDELICF3/target.h b/src/main/target/MIDELICF3/target.h index 01ce9610fa..c372c7b01a 100644 --- a/src/main/target/MIDELICF3/target.h +++ b/src/main/target/MIDELICF3/target.h @@ -95,8 +95,6 @@ #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC -#define USE_RX_CC2500 - #define USE_RX_SPI #define RX_SPI_INSTANCE SPI1 #define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA @@ -105,23 +103,22 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SPI #define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_D -#define FRSKY_BIND +#define USE_FRSKY_D_TELEMETRY -#define FRSKY_TELEMETRY -#define HUB - -#define PA_LNA -#define DIVERSITY +#define USE_FRSKY_RX_PA_LNA +#define USE_FRSKY_RX_DIVERSITY #define RX_NSS_PIN SPI1_NSS_PIN #define RX_SCK_PIN SPI1_SCK_PIN #define RX_MISO_PIN SPI1_MISO_PIN #define RX_MOSI_PIN SPI1_MOSI_PIN -#define GDO_0_PIN PB0 -#define ANT_SEL_PIN PB2 -#define TX_EN_PIN PB1 -#define RX_EN_PIN PB11 -#define FRSKY_LED_PIN PB6 + +#define FRSKY_RX_GDO_0_PIN PB0 +#define FRSKY_RX_ANT_SEL_PIN PB2 +#define FRSKY_RX_TX_EN_PIN PB1 +#define FRSKY_RX_RX_EN_PIN PB11 +#define FRSKY_RX_LED_PIN PB6 + #define BINDPLUG_PIN PC13 #define USE_SERIAL_4WAY_BLHELI_INTERFACE