diff --git a/src/main/rx/cc2500_frsky_x.c b/src/main/rx/cc2500_frsky_x.c index 3f008adf84..154d9fe7bd 100644 --- a/src/main/rx/cc2500_frsky_x.c +++ b/src/main/rx/cc2500_frsky_x.c @@ -254,7 +254,7 @@ static void buildTelemetryFrame(uint8_t *packet) frame[14]=lcrc; } -static bool frSkyXCheckQueueEmpty(void) +static bool frSkyXReadyToSend(void) { return true; } @@ -553,7 +553,7 @@ rx_spi_received_e frSkyXProcessFrame(uint8_t * const packet) } while (remoteToProcessIndex < telemetryRxBuffer[remoteToProcessId].data.dataLength && !payload) { - payload = smartPortDataReceive(telemetryRxBuffer[remoteToProcessId].data.data[remoteToProcessIndex], &clearToSend, frSkyXCheckQueueEmpty, false); + payload = smartPortDataReceive(telemetryRxBuffer[remoteToProcessId].data.data[remoteToProcessIndex], &clearToSend, frSkyXReadyToSend, false); remoteToProcessIndex = remoteToProcessIndex + 1; } } diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index e5741e042e..774bd98db5 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -207,7 +207,7 @@ static smartPortWriteFrameFn *smartPortWriteFrame; static bool smartPortMspReplyPending = false; #endif -smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool useChecksum) +smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortReadyToSendFn *readyToSend, bool useChecksum) { static uint8_t rxBuffer[sizeof(smartPortPayload_t)]; static uint8_t smartPortRxBytes = 0; @@ -229,9 +229,10 @@ smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPor if (awaitingSensorId) { awaitingSensorId = false; - if ((c == FSSP_SENSOR_ID1) && checkQueueEmpty()) { - // our slot is starting, no need to decode more + if ((c == FSSP_SENSOR_ID1) && readyToSend()) { + // our slot is starting, start sending *clearToSend = true; + // no need to decode more skipUntilStart = true; } else if (c == FSSP_SENSOR_ID2) { checksum = 0; @@ -865,7 +866,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear } } -static bool serialCheckQueueEmpty(void) +static bool serialReadyToSend(void) { return (serialRxBytesWaiting(smartPortSerialPort) == 0); } @@ -879,7 +880,7 @@ void handleSmartPortTelemetry(void) bool clearToSend = false; while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) { uint8_t c = serialRead(smartPortSerialPort); - payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true); + payload = smartPortDataReceive(c, &clearToSend, serialReadyToSend, true); } processSmartPortTelemetry(payload, &clearToSend, &requestTimeout); diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index 2398e3d3f1..929119cf3b 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -61,7 +61,7 @@ typedef struct smartPortPayload_s { } __attribute__((packed)) smartPortPayload_t; typedef void smartPortWriteFrameFn(const smartPortPayload_t *payload); -typedef bool smartPortCheckQueueEmptyFn(void); +typedef bool smartPortReadyToSendFn(void); bool initSmartPortTelemetry(void); void checkSmartPortTelemetryState(void); @@ -70,7 +70,7 @@ bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameEx void handleSmartPortTelemetry(void); void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *hasRequest, const uint32_t *requestTimeout); -smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool withChecksum); +smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortReadyToSendFn *checkQueueEmpty, bool withChecksum); struct serialPort_s; void smartPortWriteFrameSerial(const smartPortPayload_t *payload, struct serialPort_s *port, uint16_t checksum);