diff --git a/src/main/fc/core.c b/src/main/fc/core.c index c7b7bafd1c..1e5eaeff4b 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -1113,7 +1113,6 @@ static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs) } processRcCommand(); - } // Function for loop trigger diff --git a/src/main/rx/cc2500_frsky_common.h b/src/main/rx/cc2500_frsky_common.h index c67e63608c..569ded430e 100644 --- a/src/main/rx/cc2500_frsky_common.h +++ b/src/main/rx/cc2500_frsky_common.h @@ -24,4 +24,5 @@ bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeConfig); rx_spi_received_e frSkySpiDataReceived(uint8_t *packet); +rx_spi_received_e frSkySpiProcessFrame(uint8_t *packet); void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload); 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 f24b39b03d..6d6a2dad1e 100644 --- a/src/main/rx/cc2500_frsky_shared.c +++ b/src/main/rx/cc2500_frsky_shared.c @@ -62,9 +62,11 @@ static uint8_t bindIdx; static int8_t bindOffset; typedef uint8_t handlePacketFn(uint8_t * const packet, uint8_t * const protocolState); +typedef rx_spi_received_e processFrameFn(uint8_t * const packet); typedef void setRcDataFn(uint16_t *rcData, const uint8_t *payload); static handlePacketFn *handlePacket; +static processFrameFn *processFrame; static setRcDataFn *setRcData; static void initialise() { @@ -152,13 +154,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 +339,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; } @@ -378,6 +383,15 @@ rx_spi_received_e frSkySpiDataReceived(uint8_t *packet) return ret; } +rx_spi_received_e frSkySpiProcessFrame(uint8_t *packet) +{ + if (processFrame) { + return processFrame(packet); + } + + return RX_SPI_RECEIVED_NONE; +} + void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload) { setRcData(rcData, payload); @@ -425,6 +439,9 @@ bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntime rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT_FRSKY_X; handlePacket = frSkyXHandlePacket; +#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT) + processFrame = frSkyXProcessFrame; +#endif setRcData = frSkyXSetRcData; frSkyXInit(spiProtocol); 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..a447a628e0 100644 --- a/src/main/rx/cc2500_frsky_x.c +++ b/src/main/rx/cc2500_frsky_x.c @@ -141,16 +141,21 @@ typedef struct telemetryPayload_s { static telemetryData_t telemetryTxBuffer[TELEMETRY_SEQUENCE_LENGTH]; #endif +static telemetryBuffer_t telemetryRxBuffer[TELEMETRY_SEQUENCE_LENGTH]; + static telemetrySequenceMarker_t responseToSend; #if defined(USE_RX_FRSKY_SPI_TELEMETRY) static uint8_t frame[20]; + #if defined(USE_TELEMETRY_SMARTPORT) static uint8_t telemetryOutWriter; static uint8_t telemetryOutBuffer[TELEMETRY_OUT_BUFFER_SIZE]; static bool telemetryEnabled = false; + +static uint8_t remoteProcessedId = 0; #endif #endif // USE_RX_FRSKY_SPI_TELEMETRY @@ -320,14 +325,10 @@ bool isValidPacket(const uint8_t *packet) rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const protocolState) { static unsigned receiveTelemetryRetryCount = 0; - static timeMs_t pollingTimeMs = 0; static bool skipChannels = true; - static uint8_t remoteProcessedId = 0; static uint8_t remoteAckId = 0; - static uint8_t remoteToProcessIndex = 0; - static timeUs_t packetTimerUs; static bool frameReceived; @@ -335,8 +336,6 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro static uint8_t channelsToSkip = 1; static uint32_t packetErrors = 0; - static telemetryBuffer_t telemetryRxBuffer[TELEMETRY_SEQUENCE_LENGTH]; - #if defined(USE_RX_FRSKY_SPI_TELEMETRY) static bool telemetryReceived = false; #endif @@ -346,7 +345,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 +368,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; @@ -430,7 +431,9 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro } if (receiveTelemetryRetryCount >= 5) { +#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT) remoteProcessedId = TELEMETRY_SEQUENCE_LENGTH - 1; +#endif remoteAckId = TELEMETRY_SEQUENCE_LENGTH - 1; for (unsigned i = 0; i < TELEMETRY_SEQUENCE_LENGTH; i++) { telemetryRxBuffer[i].needsProcessing = false; @@ -447,6 +450,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 @@ -478,35 +484,12 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro #if defined(USE_TELEMETRY_SMARTPORT) if (telemetryEnabled) { - bool clearToSend = false; - timeMs_t now = millis(); - smartPortPayload_t *payload = NULL; - if ((now - pollingTimeMs) > 24) { - pollingTimeMs = now; - - clearToSend = true; - } else { - uint8_t remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH; - while (telemetryRxBuffer[remoteToProcessId].needsProcessing && !payload) { - while (remoteToProcessIndex < telemetryRxBuffer[remoteToProcessId].data.dataLength && !payload) { - payload = smartPortDataReceive(telemetryRxBuffer[remoteToProcessId].data.data[remoteToProcessIndex], &clearToSend, frSkyXCheckQueueEmpty, false); - remoteToProcessIndex = remoteToProcessIndex + 1; - } - - if (remoteToProcessIndex == telemetryRxBuffer[remoteToProcessId].data.dataLength) { - remoteToProcessIndex = 0; - telemetryRxBuffer[remoteToProcessId].needsProcessing = false; - remoteProcessedId = remoteToProcessId; - remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH; - } - } - } - processSmartPortTelemetry(payload, &clearToSend, NULL); + ret |= RX_SPI_ROCESSING_REQUIRED; } #endif *protocolState = STATE_RESUME; if (frameReceived) { - ret = RX_SPI_RECEIVED_DATA; + ret |= RX_SPI_RECEIVED_DATA; } } @@ -544,6 +527,44 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro return ret; } +#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT) +rx_spi_received_e frSkyXProcessFrame(uint8_t * const packet) +{ + static timeMs_t pollingTimeMs = 0; + static uint8_t remoteToProcessIndex = 0; + + UNUSED(packet); + + bool clearToSend = false; + timeMs_t now = millis(); + smartPortPayload_t *payload = NULL; + if ((now - pollingTimeMs) > 24) { + pollingTimeMs = now; + + clearToSend = true; + } else { + uint8_t remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH; + while (telemetryRxBuffer[remoteToProcessId].needsProcessing && !payload) { + while (remoteToProcessIndex < telemetryRxBuffer[remoteToProcessId].data.dataLength && !payload) { + payload = smartPortDataReceive(telemetryRxBuffer[remoteToProcessId].data.data[remoteToProcessIndex], &clearToSend, frSkyXCheckQueueEmpty, false); + remoteToProcessIndex = remoteToProcessIndex + 1; + } + + if (remoteToProcessIndex == telemetryRxBuffer[remoteToProcessId].data.dataLength) { + remoteToProcessIndex = 0; + telemetryRxBuffer[remoteToProcessId].needsProcessing = false; + remoteProcessedId = remoteToProcessId; + remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH; + } + } + } + + processSmartPortTelemetry(payload, &clearToSend, NULL); + + return RX_SPI_RECEIVED_NONE; +} +#endif + void frSkyXInit(const rx_spi_protocol_e spiProtocol) { switch(spiProtocol) { diff --git a/src/main/rx/cc2500_frsky_x.h b/src/main/rx/cc2500_frsky_x.h index 37792ac7b1..5a464d0b54 100644 --- a/src/main/rx/cc2500_frsky_x.h +++ b/src/main/rx/cc2500_frsky_x.h @@ -29,3 +29,4 @@ void frSkyXSetRcData(uint16_t *rcData, const uint8_t *payload); void frSkyXInit(const rx_spi_protocol_e spiProtocol); rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const protocolState); +rx_spi_received_e frSkyXProcessFrame(uint8_t * const packet); diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c index 2da8a57150..311b574110 100644 --- a/src/main/rx/rx_spi.c +++ b/src/main/rx/rx_spi.c @@ -57,10 +57,12 @@ STATIC_UNIT_TESTED uint8_t rxSpiNewPacketAvailable; // set true when a new packe typedef bool (*protocolInitFnPtr)(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeConfig); typedef rx_spi_received_e (*protocolDataReceivedFnPtr)(uint8_t *payload); +typedef rx_spi_received_e (*protocolProcessFrameFnPtr)(uint8_t *payload); typedef void (*protocolSetRcDataFromPayloadFnPtr)(uint16_t *rcData, const uint8_t *payload); static protocolInitFnPtr protocolInit; static protocolDataReceivedFnPtr protocolDataReceived; +static protocolProcessFrameFnPtr protocolProcessFrame; static protocolSetRcDataFromPayloadFnPtr protocolSetRcDataFromPayload; STATIC_UNIT_TESTED uint16_t rxSpiReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) @@ -137,6 +139,7 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol) protocolInit = frSkySpiInit; protocolDataReceived = frSkySpiDataReceived; protocolSetRcDataFromPayload = frSkySpiSetRcData; + protocolProcessFrame = frSkySpiProcessFrame; break; #endif // USE_RX_FRSKY_SPI @@ -175,11 +178,39 @@ static uint8_t rxSpiFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxRuntimeConfig); - if (protocolDataReceived(rxSpiPayload) == RX_SPI_RECEIVED_DATA) { + uint8_t status = RX_FRAME_PENDING; + + rx_spi_received_e result = protocolDataReceived(rxSpiPayload); + + if (result & RX_SPI_RECEIVED_DATA) { rxSpiNewPacketAvailable = true; - return RX_FRAME_COMPLETE; + status = RX_FRAME_COMPLETE; } - return RX_FRAME_PENDING; + + if (result & RX_SPI_ROCESSING_REQUIRED) { + status |= RX_FRAME_PROCESSING_REQUIRED; + } + + return status; +} + +static bool rxSpiProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) +{ + UNUSED(rxRuntimeConfig); + + if (protocolProcessFrame) { + rx_spi_received_e result = protocolProcessFrame(rxSpiPayload); + + if (result & RX_SPI_RECEIVED_DATA) { + rxSpiNewPacketAvailable = true; + } + + if (result & RX_SPI_ROCESSING_REQUIRED) { + return false; + } + } + + return true; } /* @@ -201,6 +232,7 @@ bool rxSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeCon rxRuntimeConfig->rcReadRawFn = rxSpiReadRawRC; rxRuntimeConfig->rcFrameStatusFn = rxSpiFrameStatus; + rxRuntimeConfig->rcProcessFrameFn = rxSpiProcessFrame; return ret; } diff --git a/src/main/rx/rx_spi.h b/src/main/rx/rx_spi.h index 5e63dd2c48..5faaaf3551 100644 --- a/src/main/rx/rx_spi.h +++ b/src/main/rx/rx_spi.h @@ -47,8 +47,9 @@ typedef enum { typedef enum { RX_SPI_RECEIVED_NONE = 0, - RX_SPI_RECEIVED_BIND, - RX_SPI_RECEIVED_DATA + RX_SPI_RECEIVED_BIND = (1 << 0), + RX_SPI_RECEIVED_DATA = (1 << 1), + RX_SPI_ROCESSING_REQUIRED = (1 << 2), } rx_spi_received_e; // RC channels in AETR order