diff --git a/src/main/rx/cc2500_frsky_d.c b/src/main/rx/cc2500_frsky_d.c index 70be7eaf4e..ed8eaf1575 100644 --- a/src/main/rx/cc2500_frsky_d.c +++ b/src/main/rx/cc2500_frsky_d.c @@ -59,7 +59,6 @@ #define FS_THR 960 static uint32_t missingPackets; -static uint8_t calData[255][3]; static uint8_t cnt; static int32_t t_out; static timeUs_t lastPacketReceivedTime; @@ -143,59 +142,6 @@ static void telemetry_build_frame(uint8_t *packet) #endif // USE_RX_FRSKY_SPI_TELEMETRY -static void initialize(void) -{ - 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); - - cc2500WriteReg(CC2500_07_PKTCTRL1, 0x04); - cc2500WriteReg(CC2500_0C_FSCTRL0, 0); - for (uint8_t c = 0; c < 0xFF; c++) { - cc2500Strobe(CC2500_SIDLE); - cc2500WriteReg(CC2500_0A_CHANNR, c); - cc2500Strobe(CC2500_SCAL); - delayMicroseconds(900); - calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3); - calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2); - calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1); - } -} - - #define FRSKY_D_CHANNEL_SCALING (2.0f / 3) static void decodeChannelPair(uint16_t *channels, const uint8_t *packet, const uint8_t highNibbleOffset) { @@ -250,14 +196,14 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet) case STATE_BIND_BINDING1: case STATE_BIND_BINDING2: case STATE_BIND_COMPLETE: - handleBinding(protocolState, packet); + protocolState = handleBinding(protocolState, packet); break; case STATE_STARTING: listLength = 47; initialiseData(0); protocolState = STATE_UPDATE; - nextChannel(1, true); // + nextChannel(1); cc2500Strobe(CC2500_SRX); ret = RX_SPI_RECEIVED_BIND; @@ -288,7 +234,7 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet) if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) && (packet[2] == rxFrSkySpiConfig()->bindTxId[1])) { IOHi(frSkyLedPin); - nextChannel(1, true); + nextChannel(1); #if defined(USE_RX_FRSKY_SPI_TELEMETRY) if ((packet[3] % 4) == 2) { telemetryTime = micros(); @@ -334,7 +280,7 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet) } missingPackets++; - nextChannel(1, true); + nextChannel(1); } else { if (cnt++ & 0x01) { IOLo(frSkyLedPin); @@ -345,7 +291,7 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet) #if defined(USE_RX_FRSKY_SPI_TELEMETRY) setRssiUnfiltered(0, RSSI_SOURCE_RX_PROTOCOL); #endif - nextChannel(13, true); + nextChannel(13); } cc2500Strobe(CC2500_SRX); @@ -381,16 +327,18 @@ void frSkyDInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT; - frskySpiRxSetup(); - + protocolState = STATE_INIT; lastPacketReceivedTime = 0; + missingPackets = 0; + t_out = 50; #if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_FRSKY) initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi, &frSkyTelemetryWriteSpi); #endif - if (rssiSource == RSSI_SOURCE_NONE) { - rssiSource = RSSI_SOURCE_RX_PROTOCOL; - } + + frskySpiRxSetup(rxConfig->rx_spi_protocol); + + start_time = millis(); } #endif diff --git a/src/main/rx/cc2500_frsky_shared.c b/src/main/rx/cc2500_frsky_shared.c index 30ac91f090..4ebe61a98b 100644 --- a/src/main/rx/cc2500_frsky_shared.c +++ b/src/main/rx/cc2500_frsky_shared.c @@ -38,14 +38,12 @@ #include "cc2500_frsky_shared.h" -static uint32_t missingPackets; +static rx_spi_protocol_e spiProtocol; + static uint8_t calData[255][3]; static timeMs_t timeTunedMs; uint8_t listLength; static uint8_t bindIdx; -static uint8_t Lqi; -static uint8_t protocolState; -static timeMs_t timeStartedMs; static int8_t bindOffset; static bool lastBindPinStatus; bool bindRequested = false; @@ -107,6 +105,67 @@ void frSkyBind(void) bindRequested = true; } +void initialize() { + cc2500Reset(); + cc2500WriteReg(CC2500_02_IOCFG0, 0x01); + cc2500WriteReg(CC2500_17_MCSM1, 0x0C); + cc2500WriteReg(CC2500_18_MCSM0, 0x18); + cc2500WriteReg(CC2500_07_PKTCTRL1, 0x04); + cc2500WriteReg(CC2500_3E_PATABLE, 0xFF); + cc2500WriteReg(CC2500_0C_FSCTRL0, 0x00); + cc2500WriteReg(CC2500_0D_FREQ2, 0x5C); + cc2500WriteReg(CC2500_0E_FREQ1, 0x76); + cc2500WriteReg(CC2500_0F_FREQ0, 0x27); + cc2500WriteReg(CC2500_13_MDMCFG1, 0x23); + cc2500WriteReg(CC2500_14_MDMCFG0, 0x7A); + 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); + + if (spiProtocol == RX_SPI_FRSKY_D) { + cc2500WriteReg(CC2500_06_PKTLEN, 0x19); + cc2500WriteReg(CC2500_08_PKTCTRL0, 0x05); + cc2500WriteReg(CC2500_0B_FSCTRL1, 0x08); + cc2500WriteReg(CC2500_10_MDMCFG4, 0xAA); + cc2500WriteReg(CC2500_11_MDMCFG3, 0x39); + cc2500WriteReg(CC2500_12_MDMCFG2, 0x11); + cc2500WriteReg(CC2500_15_DEVIATN, 0x42); + } else { + cc2500WriteReg(CC2500_06_PKTLEN, 0x1E); + cc2500WriteReg(CC2500_08_PKTCTRL0, 0x01); + cc2500WriteReg(CC2500_0B_FSCTRL1, 0x0A); + cc2500WriteReg(CC2500_10_MDMCFG4, 0x7B); + cc2500WriteReg(CC2500_11_MDMCFG3, 0x61); + cc2500WriteReg(CC2500_12_MDMCFG2, 0x13); + cc2500WriteReg(CC2500_15_DEVIATN, 0x51); + } + + for(uint8_t c=0;c<0xFF;c++) + {//calibrate all channels + cc2500Strobe(CC2500_SIDLE); + cc2500WriteReg(CC2500_0A_CHANNR, c); + cc2500Strobe(CC2500_SCAL); + delayMicroseconds(900); // + calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3); + calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2); + calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1); + } +} + void initialiseData(uint8_t adr) { cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)rxFrSkySpiConfig()->bindOffset); @@ -151,7 +210,7 @@ static bool tuneRx(uint8_t *packet) cc2500ReadFifo(packet, ccLen); if (packet[ccLen - 1] & 0x80) { if (packet[2] == 0x01) { - Lqi = packet[ccLen - 1] & 0x7F; + uint8_t Lqi = packet[ccLen - 1] & 0x7F; if (Lqi < 50) { rxFrSkySpiConfigMutable()->bindOffset = bindOffset; @@ -199,6 +258,8 @@ static bool getBind1(uint8_t *packet) packet[6 + n]; } + rxFrSkySpiConfigMutable()->rxNum = packet[12]; + return true; } } @@ -231,6 +292,7 @@ static bool getBind2(uint8_t *packet) return true; } #endif + for (uint8_t n = 0; n < 5; n++) { if (packet[6 + n] == packet[ccLen - 3] || (packet[6 + n] == 0)) { if (bindIdx >= 0x2D) { @@ -280,7 +342,7 @@ bool checkBindRequested(bool reset) } } -void handleBinding(uint8_t protocolState, uint8_t *packet) +uint8_t handleBinding(uint8_t protocolState, uint8_t *packet) { switch (protocolState) { case STATE_BIND: @@ -295,7 +357,7 @@ void handleBinding(uint8_t protocolState, uint8_t *packet) break; case STATE_BIND_TUNING: - if (tuneRx(packet)) { + if (tuneRx(packet)) { initGetBind(); initialiseData(1); @@ -334,9 +396,11 @@ void handleBinding(uint8_t protocolState, uint8_t *packet) break; } + + return protocolState; } -void nextChannel(uint8_t skip, bool sendStrobe) +void nextChannel(uint8_t skip) { static uint8_t channr = 0; @@ -352,13 +416,21 @@ void nextChannel(uint8_t skip, bool sendStrobe) cc2500WriteReg(CC2500_25_FSCAL1, calData[rxFrSkySpiConfig()->bindHopData[channr]][2]); cc2500WriteReg(CC2500_0A_CHANNR, rxFrSkySpiConfig()->bindHopData[channr]); - if (sendStrobe) { + if (spiProtocol == RX_SPI_FRSKY_D) { cc2500Strobe(CC2500_SFRX); } } -void frskySpiRxSetup() +void frskySpiRxSetup(rx_spi_protocol_e protocol) { + spiProtocol = protocol; + +#if defined(USE_RX_FRSKY_SPI_TELEMETRY) + if (rssiSource == RSSI_SOURCE_NONE) { + rssiSource = RSSI_SOURCE_RX_PROTOCOL; + } +#endif + // gpio init here gdoPin = IOGetByTag(IO_TAG(RX_FRSKY_SPI_GDO_0_PIN)); IOInit(gdoPin, OWNER_RX_BIND, 0); @@ -388,9 +460,6 @@ void frskySpiRxSetup() lastBindPinStatus = IORead(bindPin); #endif - timeStartedMs = millis(); - missingPackets = 0; - protocolState = STATE_INIT; #if defined(USE_RX_FRSKY_SPI_PA_LNA) #if defined(USE_RX_FRSKY_SPI_DIVERSITY) IOHi(antSelPin); @@ -398,10 +467,6 @@ void frskySpiRxSetup() RxEnable(); #endif // USE_RX_FRSKY_SPI_PA_LNA - if (rssiSource == RSSI_SOURCE_NONE) { - rssiSource = RSSI_SOURCE_RX_PROTOCOL; - } - // if(!frSkySpiDetect())//detect spi working routine // return; } diff --git a/src/main/rx/cc2500_frsky_shared.h b/src/main/rx/cc2500_frsky_shared.h index 3325132171..c4877e9143 100644 --- a/src/main/rx/cc2500_frsky_shared.h +++ b/src/main/rx/cc2500_frsky_shared.h @@ -17,6 +17,8 @@ #pragma once +#include "rx/rx_spi.h" + #define MAX_MISSING_PKT 100 #define DEBUG_DATA_ERROR_COUNT 0 @@ -47,15 +49,16 @@ extern IO_t antSelPin; void setRssiDbm(uint8_t value); -void frskySpiRxSetup(); +void frskySpiRxSetup(rx_spi_protocol_e protocol); void RxEnable(void); void TxEnable(void); +void initialize(); void initialiseData(uint8_t adr); bool checkBindRequested(bool reset); -void handleBinding(uint8_t protocolState, uint8_t *packet); +uint8_t handleBinding(uint8_t protocolState, uint8_t *packet); -void nextChannel(uint8_t skip, bool sendStrobe); +void nextChannel(uint8_t skip); diff --git a/src/main/rx/cc2500_frsky_x.c b/src/main/rx/cc2500_frsky_x.c index 5298368e13..e228b5cd24 100644 --- a/src/main/rx/cc2500_frsky_x.c +++ b/src/main/rx/cc2500_frsky_x.c @@ -146,7 +146,6 @@ static telemetrySequenceMarker_t responseToSend; static uint8_t ccLen; static uint32_t missingPackets; -static uint8_t calData[255][3]; static uint8_t cnt; static timeDelta_t t_out; static timeUs_t packet_timer; @@ -295,59 +294,6 @@ static void frSkyXTelemetryWriteFrame(const smartPortPayload_t *payload) #endif // USE_RX_FRSKY_SPI_TELEMETRY -static void initialize() { - cc2500Reset(); - cc2500WriteReg(CC2500_02_IOCFG0, 0x01); - cc2500WriteReg(CC2500_17_MCSM1, 0x0C); - cc2500WriteReg(CC2500_18_MCSM0, 0x18); - cc2500WriteReg(CC2500_06_PKTLEN, 0x1E); - cc2500WriteReg(CC2500_07_PKTCTRL1, 0x04); - cc2500WriteReg(CC2500_08_PKTCTRL0, 0x01); - cc2500WriteReg(CC2500_3E_PATABLE, 0xFF); - cc2500WriteReg(CC2500_0B_FSCTRL1, 0x0A); - cc2500WriteReg(CC2500_0C_FSCTRL0, 0x00); - cc2500WriteReg(CC2500_0D_FREQ2, 0x5C); - cc2500WriteReg(CC2500_0E_FREQ1, 0x76); - cc2500WriteReg(CC2500_0F_FREQ0, 0x27); - cc2500WriteReg(CC2500_10_MDMCFG4, 0x7B); - cc2500WriteReg(CC2500_11_MDMCFG3, 0x61); - cc2500WriteReg(CC2500_12_MDMCFG2, 0x13); - cc2500WriteReg(CC2500_13_MDMCFG1, 0x23); - cc2500WriteReg(CC2500_14_MDMCFG0, 0x7A); - cc2500WriteReg(CC2500_15_DEVIATN, 0x51); - 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); // Go to idle... - - for(uint8_t c=0;c<0xFF;c++) - {//calibrate all channels - cc2500Strobe(CC2500_SIDLE); - cc2500WriteReg(CC2500_0A_CHANNR, c); - cc2500Strobe(CC2500_SCAL); - delayMicroseconds(900); // - calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3); - calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2); - calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1); - } - //#######END INIT######## -} - - void frSkyXSetRcData(uint16_t *rcData, const uint8_t *packet) { uint16_t c[8]; @@ -396,14 +342,14 @@ rx_spi_received_e frSkyXDataReceived(uint8_t *packet) case STATE_BIND_BINDING1: case STATE_BIND_BINDING2: case STATE_BIND_COMPLETE: - handleBinding(protocolState, packet); + protocolState = handleBinding(protocolState, packet); break; case STATE_STARTING: listLength = 47; initialiseData(0); protocolState = STATE_UPDATE; - nextChannel(1, false); // + nextChannel(1); cc2500Strobe(CC2500_SRX); ret = RX_SPI_RECEIVED_BIND; break; @@ -433,7 +379,7 @@ rx_spi_received_e frSkyXDataReceived(uint8_t *packet) if (packet[0] == 0x1D) { if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) && (packet[2] == rxFrSkySpiConfig()->bindTxId[1]) && - (packet[6]==rxFrSkySpiConfig()->rxNum)) { + (rxFrSkySpiConfig()->rxNum == 0 || packet[6] == 0 || packet[6] == rxFrSkySpiConfig()->rxNum)) { missingPackets = 0; t_out = 1; t_received = 0; @@ -523,7 +469,7 @@ rx_spi_received_e frSkyXDataReceived(uint8_t *packet) #if defined(USE_RX_FRSKY_SPI_TELEMETRY) setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL); #endif - nextChannel(1, false); + nextChannel(1); cc2500Strobe(CC2500_SRX); protocolState = STATE_UPDATE; } @@ -580,7 +526,7 @@ rx_spi_received_e frSkyXDataReceived(uint8_t *packet) packet_timer = micros(); t_received=5300; frame_received=false;//again set for receive - nextChannel(chanskip, false); + nextChannel(chanskip); cc2500Strobe(CC2500_SRX); #ifdef USE_RX_FRSKY_SPI_PA_LNA RxEnable(); @@ -620,11 +566,15 @@ void frSkyXInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT; - frskySpiRxSetup(); + protocolState = STATE_INIT; + missingPackets = 0; + t_out = 50; #if defined(USE_TELEMETRY_SMARTPORT) telemetryEnabled = initSmartPortTelemetryExternal(frSkyXTelemetryWriteFrame); #endif + + frskySpiRxSetup(rxConfig->rx_spi_protocol); } #endif