Fixed telemetry for FrSky X SPI.
This commit is contained in:
parent
0634b1d079
commit
bbf09b6027
|
@ -295,7 +295,6 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro
|
||||||
cc2500Strobe(CC2500_SIDLE);
|
cc2500Strobe(CC2500_SIDLE);
|
||||||
cc2500WriteFifo(frame, frame[0] + 1);
|
cc2500WriteFifo(frame, frame[0] + 1);
|
||||||
*protocolState = STATE_DATA;
|
*protocolState = STATE_DATA;
|
||||||
ret = RX_SPI_RECEIVED_DATA;
|
|
||||||
lastPacketReceivedTime = currentPacketReceivedTime;
|
lastPacketReceivedTime = currentPacketReceivedTime;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -155,7 +155,8 @@ static uint8_t telemetryOutBuffer[TELEMETRY_OUT_BUFFER_SIZE];
|
||||||
|
|
||||||
static bool telemetryEnabled = false;
|
static bool telemetryEnabled = false;
|
||||||
|
|
||||||
static uint8_t remoteProcessedId = 0;
|
static uint8_t remoteToProcessId = 0;
|
||||||
|
static uint8_t remoteToProcessIndex = 0;
|
||||||
#endif
|
#endif
|
||||||
#endif // USE_RX_FRSKY_SPI_TELEMETRY
|
#endif // USE_RX_FRSKY_SPI_TELEMETRY
|
||||||
|
|
||||||
|
@ -432,7 +433,8 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
|
||||||
|
|
||||||
if (receiveTelemetryRetryCount >= 5) {
|
if (receiveTelemetryRetryCount >= 5) {
|
||||||
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
|
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
|
||||||
remoteProcessedId = TELEMETRY_SEQUENCE_LENGTH - 1;
|
remoteToProcessId = 0;
|
||||||
|
remoteToProcessIndex = 0;
|
||||||
#endif
|
#endif
|
||||||
remoteAckId = TELEMETRY_SEQUENCE_LENGTH - 1;
|
remoteAckId = TELEMETRY_SEQUENCE_LENGTH - 1;
|
||||||
for (unsigned i = 0; i < TELEMETRY_SEQUENCE_LENGTH; i++) {
|
for (unsigned i = 0; i < TELEMETRY_SEQUENCE_LENGTH; i++) {
|
||||||
|
@ -468,6 +470,10 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
|
||||||
cc2500Strobe(CC2500_SRX);
|
cc2500Strobe(CC2500_SRX);
|
||||||
*protocolState = STATE_UPDATE;
|
*protocolState = STATE_UPDATE;
|
||||||
}
|
}
|
||||||
|
if (frameReceived) {
|
||||||
|
ret |= RX_SPI_RECEIVED_DATA;
|
||||||
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
#ifdef USE_RX_FRSKY_SPI_TELEMETRY
|
#ifdef USE_RX_FRSKY_SPI_TELEMETRY
|
||||||
case STATE_TELEMETRY:
|
case STATE_TELEMETRY:
|
||||||
|
@ -488,9 +494,6 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
*protocolState = STATE_RESUME;
|
*protocolState = STATE_RESUME;
|
||||||
if (frameReceived) {
|
|
||||||
ret |= RX_SPI_RECEIVED_DATA;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
@ -531,7 +534,6 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
|
||||||
rx_spi_received_e frSkyXProcessFrame(uint8_t * const packet)
|
rx_spi_received_e frSkyXProcessFrame(uint8_t * const packet)
|
||||||
{
|
{
|
||||||
static timeMs_t pollingTimeMs = 0;
|
static timeMs_t pollingTimeMs = 0;
|
||||||
static uint8_t remoteToProcessIndex = 0;
|
|
||||||
|
|
||||||
UNUSED(packet);
|
UNUSED(packet);
|
||||||
|
|
||||||
|
@ -543,19 +545,21 @@ rx_spi_received_e frSkyXProcessFrame(uint8_t * const packet)
|
||||||
|
|
||||||
clearToSend = true;
|
clearToSend = true;
|
||||||
} else {
|
} else {
|
||||||
uint8_t remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH;
|
|
||||||
while (telemetryRxBuffer[remoteToProcessId].needsProcessing && !payload) {
|
while (telemetryRxBuffer[remoteToProcessId].needsProcessing && !payload) {
|
||||||
|
if (remoteToProcessIndex >= telemetryRxBuffer[remoteToProcessId].data.dataLength) {
|
||||||
|
remoteToProcessIndex = 0;
|
||||||
|
telemetryRxBuffer[remoteToProcessId].needsProcessing = false;
|
||||||
|
remoteToProcessId = (remoteToProcessId + 1) % TELEMETRY_SEQUENCE_LENGTH;
|
||||||
|
|
||||||
|
if (!telemetryRxBuffer[remoteToProcessId].needsProcessing) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
while (remoteToProcessIndex < telemetryRxBuffer[remoteToProcessId].data.dataLength && !payload) {
|
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, frSkyXCheckQueueEmpty, false);
|
||||||
remoteToProcessIndex = remoteToProcessIndex + 1;
|
remoteToProcessIndex = remoteToProcessIndex + 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (remoteToProcessIndex == telemetryRxBuffer[remoteToProcessId].data.dataLength) {
|
|
||||||
remoteToProcessIndex = 0;
|
|
||||||
telemetryRxBuffer[remoteToProcessId].needsProcessing = false;
|
|
||||||
remoteProcessedId = remoteToProcessId;
|
|
||||||
remoteToProcessId = (remoteToProcessId + 1) % TELEMETRY_SEQUENCE_LENGTH;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue