src/main/rx/cc2500_frsky_x.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <string.h>
  19 #include <sys/_stdint.h>
  20
  21 #include "platform.h"
  22
  23 #ifdef USE_RX_FRSKY_SPI_X
  24
  25 #include "build/build_config.h"
  26 #include "build/debug.h"
  27
  28 #include "common/maths.h"
  29 #include "common/utils.h"
  30
  31 #include "drivers/adc.h"
  32 #include "drivers/rx/rx_cc2500.h"
  33 #include "drivers/io.h"
  34 #include "drivers/io_def.h"
  35 #include "drivers/io_types.h"
  36 #include "drivers/resource.h"
  37 #include "drivers/system.h"
  38 #include "drivers/time.h"
  39
  40 #include "fc/config.h"
  41
  42 #include "rx/cc2500_frsky_common.h"
  43 #include "rx/cc2500_frsky_shared.h"
  44
  45 #include "sensors/battery.h"
  46
  47 #include "telemetry/smartport.h"
  48
  49 #include "cc2500_frsky_x.h"
  50
  51 const uint16_t crcTable[] = {
  52         0x0000,0x1189,0x2312,0x329b,0x4624,0x57ad,0x6536,0x74bf,
  53         0x8c48,0x9dc1,0xaf5a,0xbed3,0xca6c,0xdbe5,0xe97e,0xf8f7,
  54         0x1081,0x0108,0x3393,0x221a,0x56a5,0x472c,0x75b7,0x643e,
  55         0x9cc9,0x8d40,0xbfdb,0xae52,0xdaed,0xcb64,0xf9ff,0xe876,
  56         0x2102,0x308b,0x0210,0x1399,0x6726,0x76af,0x4434,0x55bd,
  57         0xad4a,0xbcc3,0x8e58,0x9fd1,0xeb6e,0xfae7,0xc87c,0xd9f5,
  58         0x3183,0x200a,0x1291,0x0318,0x77a7,0x662e,0x54b5,0x453c,
  59         0xbdcb,0xac42,0x9ed9,0x8f50,0xfbef,0xea66,0xd8fd,0xc974,
  60         0x4204,0x538d,0x6116,0x709f,0x0420,0x15a9,0x2732,0x36bb,
  61         0xce4c,0xdfc5,0xed5e,0xfcd7,0x8868,0x99e1,0xab7a,0xbaf3,
  62         0x5285,0x430c,0x7197,0x601e,0x14a1,0x0528,0x37b3,0x263a,
  63         0xdecd,0xcf44,0xfddf,0xec56,0x98e9,0x8960,0xbbfb,0xaa72,
  64         0x6306,0x728f,0x4014,0x519d,0x2522,0x34ab,0x0630,0x17b9,
  65         0xef4e,0xfec7,0xcc5c,0xddd5,0xa96a,0xb8e3,0x8a78,0x9bf1,
  66         0x7387,0x620e,0x5095,0x411c,0x35a3,0x242a,0x16b1,0x0738,
  67         0xffcf,0xee46,0xdcdd,0xcd54,0xb9eb,0xa862,0x9af9,0x8b70,
  68         0x8408,0x9581,0xa71a,0xb693,0xc22c,0xd3a5,0xe13e,0xf0b7,
  69         0x0840,0x19c9,0x2b52,0x3adb,0x4e64,0x5fed,0x6d76,0x7cff,
  70         0x9489,0x8500,0xb79b,0xa612,0xd2ad,0xc324,0xf1bf,0xe036,
  71         0x18c1,0x0948,0x3bd3,0x2a5a,0x5ee5,0x4f6c,0x7df7,0x6c7e,
  72         0xa50a,0xb483,0x8618,0x9791,0xe32e,0xf2a7,0xc03c,0xd1b5,
  73         0x2942,0x38cb,0x0a50,0x1bd9,0x6f66,0x7eef,0x4c74,0x5dfd,
  74         0xb58b,0xa402,0x9699,0x8710,0xf3af,0xe226,0xd0bd,0xc134,
  75         0x39c3,0x284a,0x1ad1,0x0b58,0x7fe7,0x6e6e,0x5cf5,0x4d7c,
  76         0xc60c,0xd785,0xe51e,0xf497,0x8028,0x91a1,0xa33a,0xb2b3,
  77         0x4a44,0x5bcd,0x6956,0x78df,0x0c60,0x1de9,0x2f72,0x3efb,
  78         0xd68d,0xc704,0xf59f,0xe416,0x90a9,0x8120,0xb3bb,0xa232,
  79         0x5ac5,0x4b4c,0x79d7,0x685e,0x1ce1,0x0d68,0x3ff3,0x2e7a,
  80         0xe70e,0xf687,0xc41c,0xd595,0xa12a,0xb0a3,0x8238,0x93b1,
  81         0x6b46,0x7acf,0x4854,0x59dd,0x2d62,0x3ceb,0x0e70,0x1ff9,
  82         0xf78f,0xe606,0xd49d,0xc514,0xb1ab,0xa022,0x92b9,0x8330,
  83         0x7bc7,0x6a4e,0x58d5,0x495c,0x3de3,0x2c6a,0x1ef1,0x0f78
  84 };
  85
  86 #define TELEMETRY_OUT_BUFFER_SIZE  64
  87
  88 #define TELEMETRY_SEQUENCE_LENGTH 4
  89
  90 typedef struct telemetrySequenceMarkerData_s {
  91     unsigned int packetSequenceId: 2;
  92     unsigned int unused: 1;
  93     unsigned int initRequest: 1;
  94     unsigned int ackSequenceId: 2;
  95     unsigned int retransmissionRequested: 1;
  96     unsigned int initResponse: 1;
  97 } __attribute__ ((__packed__)) telemetrySequenceMarkerData_t;
  98
  99 typedef union telemetrySequenceMarker_s {
 100     uint8_t raw;
 101     telemetrySequenceMarkerData_t data;
 102 } __attribute__ ((__packed__)) telemetrySequenceMarker_t;
 103
 104 #define SEQUENCE_MARKER_REMOTE_PART 0xf0
 105
 106 #define TELEMETRY_DATA_SIZE 5
 107
 108 typedef struct telemetryData_s {
 109     uint8_t dataLength;
 110     uint8_t data[TELEMETRY_DATA_SIZE];
 111 } __attribute__ ((__packed__)) telemetryData_t;
 112
 113 typedef struct telemetryBuffer_s {
 114     telemetryData_t data;
 115     uint8_t needsProcessing;
 116 } telemetryBuffer_t;
 117
 118 #define TELEMETRY_FRAME_SIZE  sizeof(telemetryData_t)
 119
 120 typedef struct telemetryPayload_s {
 121     uint8_t packetConst;
 122     uint8_t rssiA1;
 123     telemetrySequenceMarker_t sequence;
 124     telemetryData_t data;
 125     uint8_t crc[2];
 126 } __attribute__ ((__packed__)) telemetryPayload_t;
 127
 128 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 129 static telemetryData_t telemetryTxBuffer[TELEMETRY_SEQUENCE_LENGTH];
 130 #endif
 131
 132 static telemetrySequenceMarker_t responseToSend;
 133
 134 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 135 static uint8_t frame[20];
 136 #if defined(USE_TELEMETRY_SMARTPORT)
 137 static uint8_t telemetryOutWriter;
 138
 139 static uint8_t telemetryOutBuffer[TELEMETRY_OUT_BUFFER_SIZE];
 140
 141 static bool telemetryEnabled = false;
 142 #endif
 143 #endif // USE_RX_FRSKY_SPI_TELEMETRY
 144
 145
 146 static uint16_t calculateCrc(uint8_t *data, uint8_t len) {
 147     uint16_t crc = 0;
 148     for(uint8_t i=0; i < len; i++)
 149         crc = (crc<<8) ^ (crcTable[((uint8_t)(crc>>8) ^ *data++) & 0xFF]);
 150     return crc;
 151 }
 152
 153 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 154 #if defined(USE_TELEMETRY_SMARTPORT)
 155 static uint8_t appendSmartPortData(uint8_t *buf)
 156 {
 157     static uint8_t telemetryOutReader = 0;
 158
 159     uint8_t index;
 160     for (index = 0; index < TELEMETRY_DATA_SIZE; index++) { // max 5 bytes in a frame
 161         if (telemetryOutReader == telemetryOutWriter){ // no new data
 162             break;
 163         }
 164         buf[index] = telemetryOutBuffer[telemetryOutReader];
 165         telemetryOutReader = (telemetryOutReader + 1) % TELEMETRY_OUT_BUFFER_SIZE;
 166     }
 167
 168     return index;
 169 }
 170 #endif
 171
 172 static void buildTelemetryFrame(uint8_t *packet)
 173 {
 174     static uint8_t localPacketId;
 175
 176     static bool evenRun = false;
 177
 178     frame[0] = 0x0E;//length
 179     frame[1] = rxFrSkySpiConfig()->bindTxId[0];
 180     frame[2] = rxFrSkySpiConfig()->bindTxId[1];
 181     frame[3] = packet[3];
 182
 183     if (evenRun) {
 184         frame[4]=(uint8_t)rssiDbm|0x80;
 185     } else {
 186         const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1);
 187         frame[4] = (uint8_t)((adcExternal1Sample & 0xfe0) >> 5); // A1;
 188     }
 189     evenRun = !evenRun;
 190
 191     telemetrySequenceMarker_t *inFrameMarker = (telemetrySequenceMarker_t *)&packet[21];
 192     telemetrySequenceMarker_t *outFrameMarker = (telemetrySequenceMarker_t *)&frame[5];
 193     if (inFrameMarker->data.initRequest) { // check syncronization at startup ok if not no sport telemetry
 194         outFrameMarker-> raw = 0;
 195         outFrameMarker->data.initRequest = 1;
 196         outFrameMarker->data.initResponse = 1;
 197
 198         localPacketId = 0;
 199     } else {
 200         if (inFrameMarker->data.retransmissionRequested) {
 201             uint8_t retransmissionFrameId = inFrameMarker->data.ackSequenceId;
 202             outFrameMarker->raw = responseToSend.raw & SEQUENCE_MARKER_REMOTE_PART;
 203             outFrameMarker->data.packetSequenceId = retransmissionFrameId;
 204
 205             memcpy(&frame[6], &telemetryTxBuffer[retransmissionFrameId], TELEMETRY_FRAME_SIZE);
 206         } else {
 207             uint8_t localAckId = inFrameMarker->data.ackSequenceId;
 208             if (localPacketId != (localAckId + 1) % TELEMETRY_SEQUENCE_LENGTH) {
 209                 outFrameMarker->raw = responseToSend.raw & SEQUENCE_MARKER_REMOTE_PART;
 210                 outFrameMarker->data.packetSequenceId = localPacketId;
 211
 212                 frame[6] = appendSmartPortData(&frame[7]);
 213                 memcpy(&telemetryTxBuffer[localPacketId], &frame[6], TELEMETRY_FRAME_SIZE);
 214
 215                 localPacketId = (localPacketId + 1) % TELEMETRY_SEQUENCE_LENGTH;
 216             }
 217         }
 218     }
 219
 220     uint16_t lcrc = calculateCrc(&frame[3], 10);
 221     frame[13]=lcrc>>8;
 222     frame[14]=lcrc;
 223 }
 224
 225 static bool frSkyXCheckQueueEmpty(void)
 226 {
 227     return true;
 228 }
 229
 230 #if defined(USE_TELEMETRY_SMARTPORT)
 231 static void frSkyXTelemetrySendByte(uint8_t c) {
 232     if (c == FSSP_DLE || c == FSSP_START_STOP) {
 233         telemetryOutBuffer[telemetryOutWriter] = FSSP_DLE;
 234         telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE;
 235         telemetryOutBuffer[telemetryOutWriter] = c ^ FSSP_DLE_XOR;
 236         telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE;
 237     } else {
 238         telemetryOutBuffer[telemetryOutWriter] = c;
 239         telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE;
 240     }
 241 }
 242
 243 static void frSkyXTelemetryWriteFrame(const smartPortPayload_t *payload)
 244 {
 245     telemetryOutBuffer[telemetryOutWriter] = FSSP_START_STOP;
 246     telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE;
 247     telemetryOutBuffer[telemetryOutWriter] = FSSP_SENSOR_ID1 & 0x1f;
 248     telemetryOutWriter = (telemetryOutWriter + 1) % TELEMETRY_OUT_BUFFER_SIZE;
 249     uint8_t *data = (uint8_t *)payload;
 250     for (unsigned i = 0; i < sizeof(smartPortPayload_t); i++) {
 251         frSkyXTelemetrySendByte(*data++);
 252     }
 253 }
 254 #endif
 255 #endif // USE_RX_FRSKY_SPI_TELEMETRY
 256
 257
 258 void frSkyXSetRcData(uint16_t *rcData, const uint8_t *packet)
 259 {
 260     uint16_t c[8];
 261
 262     c[0] = (uint16_t)((packet[10] << 8) & 0xF00) | packet[9];
 263     c[1] = (uint16_t)((packet[11] << 4) & 0xFF0) | (packet[10] >> 4);
 264     c[2] = (uint16_t)((packet[13] << 8) & 0xF00) | packet[12];
 265     c[3] = (uint16_t)((packet[14] << 4) & 0xFF0) | (packet[13] >> 4);
 266     c[4] = (uint16_t)((packet[16] << 8) & 0xF00) | packet[15];
 267     c[5] = (uint16_t)((packet[17] << 4) & 0xFF0) | (packet[16] >> 4);
 268     c[6] = (uint16_t)((packet[19] << 8) & 0xF00) | packet[18];
 269     c[7] = (uint16_t)((packet[20] << 4) & 0xFF0) | (packet[19] >> 4);
 270
 271     uint8_t j = 0;
 272     for(uint8_t i = 0; i < 8; i++) {
 273         if(c[i] > 2047) {
 274             j = 8;
 275             c[i] = c[i] - 2048;
 276         } else {
 277             j = 0;
 278         }
 279         int16_t temp = (((c[i] - 64) << 1) / 3 + 860);
 280         if ((temp > 800) && (temp < 2200)) {
 281             rcData[i+j] = temp;
 282         }
 283     }
 284 }
 285
 286 rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const protocolState)
 287 {
 288     static unsigned receiveTelemetryRetryCount = 0;
 289     static timeMs_t pollingTimeMs = 0;
 290     static bool skipChannels = true;
 291     static bool ledIsOn;
 292
 293     static uint8_t remoteProcessedId = 0;
 294     static uint8_t remoteAckId = 0;
 295
 296     static uint8_t remoteToProcessIndex = 0;
 297
 298     static timeUs_t packetTimerUs;
 299
 300     static bool frameReceived;
 301     static timeDelta_t receiveDelayUs;
 302     static uint8_t channelsToSkip = 1;
 303
 304     static telemetryBuffer_t telemetryRxBuffer[TELEMETRY_SEQUENCE_LENGTH];
 305
 306 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 307     static bool telemetryReceived = false;
 308 #endif
 309
 310     rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
 311
 312     switch (*protocolState) {
 313     case STATE_STARTING:
 314         listLength = 47;
 315         initialiseData(0);
 316         *protocolState = STATE_UPDATE;
 317         nextChannel(1);
 318         cc2500Strobe(CC2500_SRX);
 319         break;
 320     case STATE_UPDATE:
 321         packetTimerUs = micros();
 322         *protocolState = STATE_DATA;
 323         frameReceived = false; // again set for receive
 324         receiveDelayUs = 5300;
 325         if (checkBindRequested(false)) {
 326             packetTimerUs = 0;
 327             timeoutUs = 50;
 328             missingPackets = 0;
 329             *protocolState = STATE_INIT;
 330
 331             break;
 332         }
 333
 334         FALLTHROUGH;
 335         // here FS code could be
 336     case STATE_DATA:
 337         if (IORead(gdoPin) && (frameReceived == false)){
 338             uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
 339             ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; // read 2 times to avoid reading errors
 340             if (ccLen > 32) {
 341                 ccLen = 32;
 342             }
 343             if (ccLen) {
 344                 cc2500ReadFifo(packet, ccLen);
 345                 uint16_t lcrc= calculateCrc(&packet[3], (ccLen - 7));
 346                 if((lcrc >> 8) == packet[ccLen-4] && (lcrc&0x00FF) == packet[ccLen - 3]){ // check calculateCrc
 347                     if (packet[0] == 0x1D) {
 348                         if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) &&
 349                                 (packet[2] == rxFrSkySpiConfig()->bindTxId[1]) &&
 350                                 (rxFrSkySpiConfig()->rxNum == 0 || packet[6] == 0 || packet[6] == rxFrSkySpiConfig()->rxNum)) {
 351                             missingPackets = 0;
 352                             timeoutUs = 1;
 353                             receiveDelayUs = 0;
 354                             LedOn();
 355                             if (skipChannels) {
 356                                 channelsToSkip = packet[5] << 2;
 357                                 if (packet[4] >= listLength) {
 358                                     if (packet[4] < (64 + listLength)) {
 359                                         channelsToSkip += 1;
 360                                     } else if (packet[4] < (128 + listLength)) {
 361                                         channelsToSkip += 2;
 362                                     } else if (packet[4] < (192 + listLength)) {
 363                                         channelsToSkip += 3;
 364                                     }
 365                                 }
 366                                 telemetryReceived = true; // now telemetry can be sent
 367                                 skipChannels = false;
 368                             }
 369 #ifdef USE_RX_FRSKY_SPI_TELEMETRY
 370                             setRssiDbm(packet[ccLen - 2]);
 371 #endif
 372
 373                             telemetrySequenceMarker_t *inFrameMarker = (telemetrySequenceMarker_t *)&packet[21];
 374
 375                             uint8_t remoteNewPacketId = inFrameMarker->data.packetSequenceId;
 376                             memcpy(&telemetryRxBuffer[remoteNewPacketId].data, &packet[22], TELEMETRY_FRAME_SIZE);
 377                             telemetryRxBuffer[remoteNewPacketId].needsProcessing = true;
 378
 379                             responseToSend.raw = 0;
 380                             uint8_t remoteToAckId = (remoteAckId + 1) % TELEMETRY_SEQUENCE_LENGTH;
 381                             if (remoteNewPacketId != remoteToAckId) {
 382                                 while (remoteToAckId != remoteNewPacketId) {
 383                                     if (!telemetryRxBuffer[remoteToAckId].needsProcessing) {
 384                                         responseToSend.data.ackSequenceId = remoteToAckId;
 385                                         responseToSend.data.retransmissionRequested = 1;
 386
 387                                         receiveTelemetryRetryCount++;
 388
 389                                         break;
 390                                     }
 391
 392                                     remoteToAckId = (remoteToAckId + 1) % TELEMETRY_SEQUENCE_LENGTH;
 393                                 }
 394                             }
 395
 396                             if (!responseToSend.data.retransmissionRequested) {
 397                                 receiveTelemetryRetryCount = 0;
 398
 399                                 remoteToAckId = (remoteAckId + 1) % TELEMETRY_SEQUENCE_LENGTH;
 400                                 uint8_t remoteNextAckId;
 401                                 while (telemetryRxBuffer[remoteToAckId].needsProcessing && remoteToAckId != remoteAckId) {
 402                                     remoteNextAckId = remoteToAckId;
 403                                     remoteToAckId = (remoteToAckId + 1) % TELEMETRY_SEQUENCE_LENGTH;
 404                                 }
 405                                 remoteAckId = remoteNextAckId;
 406                                 responseToSend.data.ackSequenceId = remoteAckId;
 407                             }
 408
 409                             if (receiveTelemetryRetryCount >= 5) {
 410                                  remoteProcessedId =  TELEMETRY_SEQUENCE_LENGTH - 1;
 411                                  remoteAckId =  TELEMETRY_SEQUENCE_LENGTH - 1;
 412                                  for (unsigned i = 0; i < TELEMETRY_SEQUENCE_LENGTH; i++) {
 413                                      telemetryRxBuffer[i].needsProcessing = false;
 414                                  }
 415
 416                                  receiveTelemetryRetryCount = 0;
 417                              }
 418
 419                             packetTimerUs = micros();
 420                             frameReceived = true; // no need to process frame again.
 421                         }
 422                     }
 423                 }
 424             }
 425         }
 426         if (telemetryReceived) {
 427             if(cmpTimeUs(micros(), packetTimerUs) > receiveDelayUs) { // if received or not received in this time sent telemetry data
 428                 *protocolState = STATE_TELEMETRY;
 429                 buildTelemetryFrame(packet);
 430             }
 431         }
 432         if (cmpTimeUs(micros(), packetTimerUs) > timeoutUs * SYNC_DELAY_MAX) {
 433             if (ledIsOn) {
 434                 LedOff();
 435             } else {
 436                 LedOn();
 437             }
 438             ledIsOn = !ledIsOn;
 439
 440 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 441             setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
 442 #endif
 443             nextChannel(1);
 444             cc2500Strobe(CC2500_SRX);
 445             *protocolState = STATE_UPDATE;
 446         }
 447         break;
 448 #ifdef USE_RX_FRSKY_SPI_TELEMETRY
 449     case STATE_TELEMETRY:
 450         if(cmpTimeUs(micros(), packetTimerUs) >= receiveDelayUs + 400) { // if received or not received in this time sent telemetry data
 451             cc2500Strobe(CC2500_SIDLE);
 452             cc2500SetPower(6);
 453             cc2500Strobe(CC2500_SFRX);
 454             delayMicroseconds(30);
 455 #if defined(USE_RX_FRSKY_SPI_PA_LNA)
 456             TxEnable();
 457 #endif
 458             cc2500Strobe(CC2500_SIDLE);
 459             cc2500WriteFifo(frame, frame[0] + 1);
 460
 461 #if defined(USE_TELEMETRY_SMARTPORT)
 462             if (telemetryEnabled) {
 463                 bool clearToSend = false;
 464                 timeMs_t now = millis();
 465                 smartPortPayload_t *payload = NULL;
 466                 if ((now - pollingTimeMs) > 24) {
 467                     pollingTimeMs = now;
 468
 469                     clearToSend = true;
 470                 } else {
 471                     uint8_t remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH;
 472                     while (telemetryRxBuffer[remoteToProcessId].needsProcessing && !payload) {
 473                         while (remoteToProcessIndex < telemetryRxBuffer[remoteToProcessId].data.dataLength && !payload) {
 474                             payload = smartPortDataReceive(telemetryRxBuffer[remoteToProcessId].data.data[remoteToProcessIndex], &clearToSend, frSkyXCheckQueueEmpty, false);
 475                             remoteToProcessIndex = remoteToProcessIndex + 1;
 476                         }
 477
 478                         if (remoteToProcessIndex == telemetryRxBuffer[remoteToProcessId].data.dataLength) {
 479                             remoteToProcessIndex = 0;
 480                             telemetryRxBuffer[remoteToProcessId].needsProcessing = false;
 481                             remoteProcessedId = remoteToProcessId;
 482                             remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH;
 483                         }
 484                     }
 485                 }
 486                 processSmartPortTelemetry(payload, &clearToSend, NULL);
 487             }
 488 #endif
 489             *protocolState = STATE_RESUME;
 490             ret = RX_SPI_RECEIVED_DATA;
 491         }
 492
 493         break;
 494 #endif // USE_RX_FRSKY_SPI_TELEMETRY
 495     case STATE_RESUME:
 496         if (cmpTimeUs(micros(), packetTimerUs) > receiveDelayUs + 3700) {
 497             packetTimerUs = micros();
 498             receiveDelayUs = 5300;
 499             frameReceived = false; // again set for receive
 500             nextChannel(channelsToSkip);
 501             cc2500Strobe(CC2500_SRX);
 502 #ifdef USE_RX_FRSKY_SPI_PA_LNA
 503             TxDisable();
 504 #if defined(USE_RX_FRSKY_SPI_DIVERSITY)
 505             if (missingPackets >= 2) {
 506                 switchAntennae();
 507             }
 508 #endif
 509 #endif // USE_RX_FRSKY_SPI_PA_LNA
 510             if (missingPackets > MAX_MISSING_PKT) {
 511                 timeoutUs = 50;
 512                 skipChannels = true;
 513                 telemetryReceived = false;
 514                 *protocolState = STATE_UPDATE;
 515                 break;
 516             }
 517             missingPackets++;
 518             *protocolState = STATE_DATA;
 519         }
 520         break;
 521     }
 522
 523     return ret;
 524 }
 525
 526 void frSkyXInit(void)
 527 {
 528 #if defined(USE_TELEMETRY_SMARTPORT)
 529      telemetryEnabled = initSmartPortTelemetryExternal(frSkyXTelemetryWriteFrame);
 530 #endif
 531 }
 532
 533 #endif