diff --git a/src/main/rx/cc2500_frsky_d.c b/src/main/rx/cc2500_frsky_d.c index 32a9a169ba..5be0848b86 100644 --- a/src/main/rx/cc2500_frsky_d.c +++ b/src/main/rx/cc2500_frsky_d.c @@ -194,7 +194,7 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro switch (*protocolState) { case STATE_STARTING: listLength = 47; - initialiseData(0); + initialiseData(false); *protocolState = STATE_UPDATE; nextChannel(1); cc2500Strobe(CC2500_SRX); @@ -218,9 +218,11 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro case STATE_DATA: if (cc2500getGdo()) { uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; + bool packetOk = false; if (ccLen >= 20) { cc2500ReadFifo(packet, 20); if (packet[19] & 0x80) { + packetOk = true; missingPackets = 0; timeoutUs = 1; if (packet[0] == 0x11) { @@ -246,6 +248,9 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro } } } + if (!packetOk) { + cc2500Strobe(CC2500_SRX); + } } if (cmpTimeUs(currentPacketReceivedTime, lastPacketReceivedTime) > (timeoutUs * SYNC_DELAY_MAX)) { diff --git a/src/main/rx/cc2500_frsky_shared.c b/src/main/rx/cc2500_frsky_shared.c index f1de441680..50d9547546 100644 --- a/src/main/rx/cc2500_frsky_shared.c +++ b/src/main/rx/cc2500_frsky_shared.c @@ -152,13 +152,16 @@ static void initialise() { } } -void initialiseData(uint8_t adr) +void initialiseData(bool inBindState) { cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)rxCc2500SpiConfig()->bindOffset); cc2500WriteReg(CC2500_18_MCSM0, 0x8); - cc2500WriteReg(CC2500_09_ADDR, adr ? 0x03 : rxCc2500SpiConfig()->bindTxId[0]); + cc2500WriteReg(CC2500_09_ADDR, inBindState ? 0x03 : rxCc2500SpiConfig()->bindTxId[0]); cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D); cc2500WriteReg(CC2500_19_FOCCFG, 0x16); + if (!inBindState) { + cc2500WriteReg(CC2500_03_FIFOTHR, 0x14); + } delay(10); } @@ -334,7 +337,7 @@ rx_spi_received_e frSkySpiDataReceived(uint8_t *packet) case STATE_BIND_TUNING: if (tuneRx(packet)) { initGetBind(); - initialiseData(1); + initialiseData(true); protocolState = STATE_BIND_BINDING1; } diff --git a/src/main/rx/cc2500_frsky_shared.h b/src/main/rx/cc2500_frsky_shared.h index e8ed074592..16754ac72a 100644 --- a/src/main/rx/cc2500_frsky_shared.h +++ b/src/main/rx/cc2500_frsky_shared.h @@ -51,6 +51,6 @@ extern uint8_t listLength; extern uint32_t missingPackets; extern timeDelta_t timeoutUs; -void initialiseData(uint8_t adr); +void initialiseData(bool inBindState); void nextChannel(uint8_t skip); diff --git a/src/main/rx/cc2500_frsky_x.c b/src/main/rx/cc2500_frsky_x.c index 45232aa246..c0e579d5fc 100644 --- a/src/main/rx/cc2500_frsky_x.c +++ b/src/main/rx/cc2500_frsky_x.c @@ -346,7 +346,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro switch (*protocolState) { case STATE_STARTING: listLength = 47; - initialiseData(0); + initialiseData(false); *protocolState = STATE_UPDATE; nextChannel(1); cc2500Strobe(CC2500_SRX); @@ -369,10 +369,12 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro // here FS code could be case STATE_DATA: if (cc2500getGdo() && (frameReceived == false)){ + bool packetOk = false; uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; if (ccLen >= packetLength) { cc2500ReadFifo(packet, packetLength); if (isValidPacket(packet)) { + packetOk = true; missingPackets = 0; timeoutUs = 1; receiveDelayUs = 0; @@ -447,6 +449,9 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro DEBUG_SET(DEBUG_RX_FRSKY_SPI, DEBUG_DATA_BAD_FRAME, packetErrors); } } + if (!packetOk) { + cc2500Strobe(CC2500_SRX); + } } if (telemetryReceived) { if (cmpTimeUs(micros(), packetTimerUs) > receiveDelayUs) { // if received or not received in this time sent telemetry data