src/main/rx/frsky_d.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 <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdlib.h>
  21 #include <string.h>
  22
  23 #include "platform.h"
  24
  25 #ifdef USE_RX_FRSKY_D
  26
  27 #include "build/build_config.h"
  28 #include "build/debug.h"
  29
  30 #include "common/maths.h"
  31 #include "common/utils.h"
  32
  33 #include "drivers/cc2500.h"
  34 #include "drivers/io.h"
  35 #include "drivers/system.h"
  36 #include "drivers/time.h"
  37
  38 #include "fc/config.h"
  39
  40 #include "config/feature.h"
  41 #include "config/parameter_group_ids.h"
  42
  43 #include "rx/rx.h"
  44 #include "rx/rx_spi.h"
  45 #include "rx/frsky_d.h"
  46
  47 #include "sensors/battery.h"
  48
  49 #include "telemetry/frsky.h"
  50
  51 #define RC_CHANNEL_COUNT 8
  52 #define MAX_MISSING_PKT 100
  53
  54 #define DEBUG_DATA_ERROR_COUNT 0
  55
  56 #define SYNC 9000
  57 #define FS_THR 960
  58
  59 static uint32_t missingPackets;
  60 static uint8_t calData[255][3];
  61 static uint32_t time_tune;
  62 static uint8_t listLength;
  63 static uint8_t bindIdx;
  64 static uint8_t cnt;
  65 static int32_t t_out;
  66 static uint8_t Lqi;
  67 static timeUs_t lastPacketReceivedTime;
  68 static uint8_t protocolState;
  69 static uint32_t start_time;
  70 static int8_t bindOffset;
  71 static uint16_t dataErrorCount = 0;
  72
  73 static IO_t gdoPin;
  74 static IO_t bindPin = DEFIO_IO(NONE);
  75 static bool lastBindPinStatus;
  76 static IO_t frSkyLedPin;
  77 #if defined(USE_FRSKY_RX_PA_LNA)
  78 static IO_t txEnPin;
  79 static IO_t rxEnPin;
  80 static IO_t antSelPin;
  81 static uint8_t pass;
  82 #endif
  83 bool bindRequested = false;
  84
  85 #ifdef USE_FRSKY_D_TELEMETRY
  86 static uint8_t frame[20];
  87 static int16_t RSSI_dBm;
  88 static uint8_t telemetry_id;
  89 static uint32_t telemetryTime;
  90
  91 #if defined(USE_TELEMETRY_FRSKY)
  92 #define MAX_SERIAL_BYTES 64
  93 static uint8_t hub_index;
  94 static uint8_t idxx = 0;
  95 static uint8_t idx_ok = 0;
  96 static uint8_t telemetry_expected_id = 0;
  97 static uint8_t srx_data[MAX_SERIAL_BYTES]; // buffer for telemetry serial data
  98 #endif
  99 #endif
 100
 101 PG_REGISTER_WITH_RESET_TEMPLATE(frSkyDConfig_t, frSkyDConfig, PG_FRSKY_D_CONFIG,
 102                                 0);
 103
 104 PG_RESET_TEMPLATE(frSkyDConfig_t, frSkyDConfig,
 105     .autoBind = false,
 106     .bindTxId = {0, 0},
 107     .bindOffset = 0,
 108     .bindHopData = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 109         0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 110         0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 111         0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
 112 );
 113
 114 enum {
 115     STATE_INIT = 0,
 116     STATE_BIND,
 117     STATE_BIND_TUNING,
 118     STATE_BIND_BINDING1,
 119     STATE_BIND_BINDING2,
 120     STATE_BIND_COMPLETE,
 121     STATE_STARTING,
 122     STATE_UPDATE,
 123     STATE_DATA,
 124     STATE_TELEMETRY
 125 };
 126
 127 #if defined(USE_FRSKY_D_TELEMETRY)
 128 static void compute_RSSIdbm(uint8_t *packet)
 129 {
 130     if (packet[18] >= 128) {
 131         RSSI_dBm = ((((uint16_t)packet[18]) * 18) >> 5) - 82;
 132     } else {
 133         RSSI_dBm = ((((uint16_t)packet[18]) * 18) >> 5) + 65;
 134     }
 135
 136     setRssiUnfiltered(constrain(RSSI_dBm << 3, 0, 1024), RSSI_SOURCE_RX_PROTOCOL);
 137 }
 138
 139 #if defined(USE_TELEMETRY_FRSKY)
 140 static uint8_t frsky_append_hub_data(uint8_t *buf)
 141 {
 142     if (telemetry_id == telemetry_expected_id)
 143         idx_ok = idxx;
 144     else // rx re-requests last packet
 145         idxx = idx_ok;
 146
 147     telemetry_expected_id = (telemetry_id + 1) & 0x1F;
 148     uint8_t index = 0;
 149     for (uint8_t i = 0; i < 10; i++) {
 150         if (idxx == hub_index) {
 151             break;
 152         }
 153         buf[i] = srx_data[idxx];
 154         idxx = (idxx + 1) & (MAX_SERIAL_BYTES - 1);
 155         index++;
 156     }
 157     return index;
 158 }
 159
 160 static void frSkyTelemetryInitFrameSpi(void)
 161 {
 162     hub_index = 0;
 163     idxx = 0;
 164 }
 165
 166 static void frSkyTelemetryWriteSpi(uint8_t ch)
 167 {
 168     if (hub_index < MAX_SERIAL_BYTES) {
 169         srx_data[hub_index++] = ch;
 170     }
 171 }
 172 #endif
 173
 174 static void telemetry_build_frame(uint8_t *packet)
 175 {
 176     const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1);
 177     const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
 178     uint8_t bytes_used = 0;
 179     telemetry_id = packet[4];
 180     frame[0] = 0x11; // length
 181     frame[1] = frSkyDConfig()->bindTxId[0];
 182     frame[2] = frSkyDConfig()->bindTxId[1];
 183     frame[3] = (uint8_t)((adcExternal1Sample & 0xff0) >> 4); // A1
 184     frame[4] = (uint8_t)((adcRssiSample & 0xff0) >> 4);      // A2
 185     frame[5] = (uint8_t)RSSI_dBm;
 186 #if defined(USE_TELEMETRY_FRSKY)
 187     bytes_used = frsky_append_hub_data(&frame[8]);
 188 #endif
 189     frame[6] = bytes_used;
 190     frame[7] = telemetry_id;
 191 }
 192
 193 #endif
 194
 195 #if defined(USE_FRSKY_RX_PA_LNA)
 196 static void RX_enable(void)
 197 {
 198     IOLo(txEnPin);
 199     IOHi(rxEnPin);
 200 }
 201
 202 static void TX_enable(void)
 203 {
 204     IOLo(rxEnPin);
 205     IOHi(txEnPin);
 206 }
 207 #endif
 208
 209 void frSkyDBind(void)
 210 {
 211     bindRequested = true;
 212 }
 213
 214 static void initialize(void)
 215 {
 216     cc2500Reset();
 217     cc2500WriteReg(CC2500_02_IOCFG0, 0x01);
 218     cc2500WriteReg(CC2500_17_MCSM1, 0x0C);
 219     cc2500WriteReg(CC2500_18_MCSM0, 0x18);
 220     cc2500WriteReg(CC2500_06_PKTLEN, 0x19);
 221     cc2500WriteReg(CC2500_08_PKTCTRL0, 0x05);
 222     cc2500WriteReg(CC2500_3E_PATABLE, 0xFF);
 223     cc2500WriteReg(CC2500_0B_FSCTRL1, 0x08);
 224     cc2500WriteReg(CC2500_0C_FSCTRL0, 0x00);
 225     cc2500WriteReg(CC2500_0D_FREQ2, 0x5C);
 226     cc2500WriteReg(CC2500_0E_FREQ1, 0x76);
 227     cc2500WriteReg(CC2500_0F_FREQ0, 0x27);
 228     cc2500WriteReg(CC2500_10_MDMCFG4, 0xAA);
 229     cc2500WriteReg(CC2500_11_MDMCFG3, 0x39);
 230     cc2500WriteReg(CC2500_12_MDMCFG2, 0x11);
 231     cc2500WriteReg(CC2500_13_MDMCFG1, 0x23);
 232     cc2500WriteReg(CC2500_14_MDMCFG0, 0x7A);
 233     cc2500WriteReg(CC2500_15_DEVIATN, 0x42);
 234     cc2500WriteReg(CC2500_19_FOCCFG, 0x16);
 235     cc2500WriteReg(CC2500_1A_BSCFG, 0x6C);
 236     cc2500WriteReg(CC2500_1B_AGCCTRL2, 0x03);
 237     cc2500WriteReg(CC2500_1C_AGCCTRL1, 0x40);
 238     cc2500WriteReg(CC2500_1D_AGCCTRL0, 0x91);
 239     cc2500WriteReg(CC2500_21_FREND1, 0x56);
 240     cc2500WriteReg(CC2500_22_FREND0, 0x10);
 241     cc2500WriteReg(CC2500_23_FSCAL3, 0xA9);
 242     cc2500WriteReg(CC2500_24_FSCAL2, 0x0A);
 243     cc2500WriteReg(CC2500_25_FSCAL1, 0x00);
 244     cc2500WriteReg(CC2500_26_FSCAL0, 0x11);
 245     cc2500WriteReg(CC2500_29_FSTEST, 0x59);
 246     cc2500WriteReg(CC2500_2C_TEST2, 0x88);
 247     cc2500WriteReg(CC2500_2D_TEST1, 0x31);
 248     cc2500WriteReg(CC2500_2E_TEST0, 0x0B);
 249     cc2500WriteReg(CC2500_03_FIFOTHR, 0x07);
 250     cc2500WriteReg(CC2500_09_ADDR, 0x00);
 251     cc2500Strobe(CC2500_SIDLE);
 252
 253     cc2500WriteReg(CC2500_07_PKTCTRL1, 0x04);
 254     cc2500WriteReg(CC2500_0C_FSCTRL0, 0);
 255     for (uint8_t c = 0; c < 0xFF; c++) {
 256         cc2500Strobe(CC2500_SIDLE);
 257         cc2500WriteReg(CC2500_0A_CHANNR, c);
 258         cc2500Strobe(CC2500_SCAL);
 259         delayMicroseconds(900);
 260         calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3);
 261         calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2);
 262         calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1);
 263     }
 264 }
 265
 266 static void initialize_data(uint8_t adr)
 267 {
 268     cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)frSkyDConfig()->bindOffset);
 269     cc2500WriteReg(CC2500_18_MCSM0, 0x8);
 270     cc2500WriteReg(CC2500_09_ADDR, adr ? 0x03 : frSkyDConfig()->bindTxId[0]);
 271     cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D);
 272     cc2500WriteReg(CC2500_19_FOCCFG, 0x16);
 273     delay(10);
 274 }
 275
 276 static void initTuneRx(void)
 277 {
 278     cc2500WriteReg(CC2500_19_FOCCFG, 0x14);
 279     time_tune = millis();
 280     bindOffset = -126;
 281     cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
 282     cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0C);
 283     cc2500WriteReg(CC2500_18_MCSM0, 0x8);
 284
 285     cc2500Strobe(CC2500_SIDLE);
 286     cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]);
 287     cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]);
 288     cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]);
 289     cc2500WriteReg(CC2500_0A_CHANNR, 0);
 290     cc2500Strobe(CC2500_SFRX);
 291     cc2500Strobe(CC2500_SRX);
 292 }
 293
 294 static bool tuneRx(uint8_t *packet)
 295 {
 296     if (bindOffset >= 126) {
 297         bindOffset = -126;
 298     }
 299     if ((millis() - time_tune) > 50) {
 300         time_tune = millis();
 301         bindOffset += 5;
 302         cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
 303     }
 304     if (IORead(gdoPin)) {
 305         uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
 306         if (ccLen) {
 307             cc2500ReadFifo(packet, ccLen);
 308             if (packet[ccLen - 1] & 0x80) {
 309                 if (packet[2] == 0x01) {
 310                     Lqi = packet[ccLen - 1] & 0x7F;
 311                     if (Lqi < 50) {
 312                         frSkyDConfigMutable()->bindOffset = bindOffset;
 313
 314                         return true;
 315                     }
 316                 }
 317             }
 318         }
 319     }
 320
 321     return false;
 322 }
 323
 324 static void initGetBind(void)
 325 {
 326     cc2500Strobe(CC2500_SIDLE);
 327     cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]);
 328     cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]);
 329     cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]);
 330     cc2500WriteReg(CC2500_0A_CHANNR, 0);
 331     cc2500Strobe(CC2500_SFRX);
 332     delayMicroseconds(20); // waiting flush FIFO
 333
 334     cc2500Strobe(CC2500_SRX);
 335     listLength = 0;
 336     bindIdx = 0x05;
 337 }
 338
 339 static bool getBind1(uint8_t *packet)
 340 {
 341     // len|bind |tx
 342     // id|03|01|idx|h0|h1|h2|h3|h4|00|00|00|00|00|00|00|00|00|00|00|00|00|00|00|CHK1|CHK2|RSSI|LQI/CRC|
 343     // Start by getting bind packet 0 and the txid
 344     if (IORead(gdoPin)) {
 345         uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
 346         if (ccLen) {
 347             cc2500ReadFifo(packet, ccLen);
 348             if (packet[ccLen - 1] & 0x80) {
 349                 if (packet[2] == 0x01) {
 350                     if (packet[5] == 0x00) {
 351                         frSkyDConfigMutable()->bindTxId[0] = packet[3];
 352                         frSkyDConfigMutable()->bindTxId[1] = packet[4];
 353                         for (uint8_t n = 0; n < 5; n++) {
 354                             frSkyDConfigMutable()->bindHopData[packet[5] + n] =
 355                                 packet[6 + n];
 356                         }
 357
 358                         return true;
 359                     }
 360                 }
 361             }
 362         }
 363     }
 364
 365     return false;
 366 }
 367
 368 static bool getBind2(uint8_t *packet)
 369 {
 370     if (bindIdx <= 120) {
 371         if (IORead(gdoPin)) {
 372             uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
 373             if (ccLen) {
 374                 cc2500ReadFifo(packet, ccLen);
 375                 if (packet[ccLen - 1] & 0x80) {
 376                     if (packet[2] == 0x01) {
 377                         if ((packet[3] == frSkyDConfig()->bindTxId[0]) &&
 378                             (packet[4] == frSkyDConfig()->bindTxId[1])) {
 379                             if (packet[5] == bindIdx) {
 380 #if defined(DJTS)
 381                                 if (packet[5] == 0x2D) {
 382                                     for (uint8_t i = 0; i < 2; i++) {
 383                                         frSkyDConfigMutable()
 384                                             ->bindHopData[packet[5] + i] =
 385                                             packet[6 + i];
 386                                     }
 387                                     listLength = 47;
 388
 389                                     return true;
 390                                 }
 391 #endif
 392                                 for (uint8_t n = 0; n < 5; n++) {
 393                                     if (packet[6 + n] == packet[ccLen - 3] ||
 394                                         (packet[6 + n] == 0)) {
 395                                         if (bindIdx >= 0x2D) {
 396                                             listLength = packet[5] + n;
 397
 398                                             return true;
 399                                         }
 400                                     }
 401
 402                                     frSkyDConfigMutable()->bindHopData[packet[5] + n] = packet[6 + n];
 403                                 }
 404
 405                                 bindIdx = bindIdx + 5;
 406
 407                                 return false;
 408                             }
 409                         }
 410                     }
 411                 }
 412             }
 413         }
 414
 415         return false;
 416     } else {
 417         return true;
 418     }
 419
 420 }
 421
 422 static void nextChannel(uint8_t skip)
 423 {
 424     static uint8_t channr = 0;
 425
 426     channr += skip;
 427     while (channr >= listLength) {
 428         channr -= listLength;
 429     }
 430     cc2500Strobe(CC2500_SIDLE);
 431     cc2500WriteReg(CC2500_23_FSCAL3,
 432                     calData[frSkyDConfig()->bindHopData[channr]][0]);
 433     cc2500WriteReg(CC2500_24_FSCAL2,
 434                     calData[frSkyDConfig()->bindHopData[channr]][1]);
 435     cc2500WriteReg(CC2500_25_FSCAL1,
 436                     calData[frSkyDConfig()->bindHopData[channr]][2]);
 437     cc2500WriteReg(CC2500_0A_CHANNR, frSkyDConfig()->bindHopData[channr]);
 438     cc2500Strobe(CC2500_SFRX);
 439 }
 440
 441 static bool checkBindRequested(bool reset)
 442 {
 443     if (bindPin) {
 444         bool bindPinStatus = IORead(bindPin);
 445         if (lastBindPinStatus && !bindPinStatus) {
 446             bindRequested = true;
 447         }
 448         lastBindPinStatus = bindPinStatus;
 449     }
 450
 451     if (!bindRequested) {
 452         return false;
 453     } else {
 454         if (reset) {
 455             bindRequested = false;
 456         }
 457
 458         return true;
 459     }
 460 }
 461
 462 #define FRSKY_D_CHANNEL_SCALING (2.0f / 3)
 463
 464 static void decodeChannelPair(uint16_t *channels, const uint8_t *packet, const uint8_t highNibbleOffset) {
 465     channels[0] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf) << 8 | packet[0]);
 466     channels[1] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf0) << 4 | packet[1]);
 467 }
 468
 469 void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet)
 470 {
 471     uint16_t channels[RC_CHANNEL_COUNT];
 472     bool dataError = false;
 473
 474     decodeChannelPair(channels, packet + 6, 4);
 475     decodeChannelPair(channels + 2, packet + 8, 3);
 476     decodeChannelPair(channels + 4, packet + 12, 4);
 477     decodeChannelPair(channels + 6, packet + 14, 3);
 478
 479     for (int i = 0; i < RC_CHANNEL_COUNT; i++) {
 480         if ((channels[i] < 800) || (channels[i] > 2200)) {
 481             dataError = true;
 482
 483             break;
 484         }
 485     }
 486
 487     if (!dataError) {
 488         for (int i = 0; i < RC_CHANNEL_COUNT; i++) {
 489             rcData[i] = channels[i];
 490         }
 491     } else {
 492         DEBUG_SET(DEBUG_FRSKY_D_RX, DEBUG_DATA_ERROR_COUNT, ++dataErrorCount);
 493     }
 494 }
 495
 496 rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
 497 {
 498     const timeUs_t currentPacketReceivedTime = micros();
 499
 500     rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
 501
 502     switch (protocolState) {
 503     case STATE_INIT:
 504         if ((millis() - start_time) > 10) {
 505             initialize();
 506
 507             protocolState = STATE_BIND;
 508         }
 509
 510         break;
 511     case STATE_BIND:
 512         if (checkBindRequested(true) || frSkyDConfig()->autoBind) {
 513             IOHi(frSkyLedPin);
 514             initTuneRx();
 515
 516             protocolState = STATE_BIND_TUNING;
 517         } else {
 518             protocolState = STATE_STARTING;
 519         }
 520
 521         break;
 522     case STATE_BIND_TUNING:
 523         if (tuneRx(packet)) {
 524             initGetBind();
 525             initialize_data(1);
 526
 527             protocolState = STATE_BIND_BINDING1;
 528         }
 529
 530         break;
 531     case STATE_BIND_BINDING1:
 532         if (getBind1(packet)) {
 533             protocolState = STATE_BIND_BINDING2;
 534         }
 535
 536         break;
 537     case STATE_BIND_BINDING2:
 538         if (getBind2(packet)) {
 539             cc2500Strobe(CC2500_SIDLE);
 540
 541             protocolState = STATE_BIND_COMPLETE;
 542         }
 543
 544         break;
 545     case STATE_BIND_COMPLETE:
 546         if (!frSkyDConfig()->autoBind) {
 547             writeEEPROM();
 548         } else {
 549             uint8_t ctr = 40;
 550             while (ctr--) {
 551                 IOHi(frSkyLedPin);
 552                 delay(50);
 553                 IOLo(frSkyLedPin);
 554                 delay(50);
 555             }
 556         }
 557
 558         protocolState = STATE_STARTING;
 559
 560         break;
 561     case STATE_STARTING:
 562         listLength = 47;
 563         initialize_data(0);
 564         protocolState = STATE_UPDATE;
 565         nextChannel(1); //
 566         cc2500Strobe(CC2500_SRX);
 567         ret = RX_SPI_RECEIVED_BIND;
 568
 569         break;
 570     case STATE_UPDATE:
 571         lastPacketReceivedTime = currentPacketReceivedTime;
 572         protocolState = STATE_DATA;
 573
 574         if (checkBindRequested(false)) {
 575             lastPacketReceivedTime = 0;
 576             t_out = 50;
 577             missingPackets = 0;
 578
 579             protocolState = STATE_INIT;
 580
 581             break;
 582         }
 583     // here FS code could be
 584     case STATE_DATA:
 585         if (IORead(gdoPin)) {
 586             uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
 587             if (ccLen >= 20) {
 588                 cc2500ReadFifo(packet, 20);
 589                 if (packet[19] & 0x80) {
 590                     missingPackets = 0;
 591                     t_out = 1;
 592                     if (packet[0] == 0x11) {
 593                         if ((packet[1] == frSkyDConfig()->bindTxId[0]) &&
 594                             (packet[2] == frSkyDConfig()->bindTxId[1])) {
 595                             IOHi(frSkyLedPin);
 596                             nextChannel(1);
 597 #ifdef USE_FRSKY_D_TELEMETRY
 598                             if ((packet[3] % 4) == 2) {
 599                                 telemetryTime = micros();
 600                                 compute_RSSIdbm(packet);
 601                                 telemetry_build_frame(packet);
 602                                 protocolState = STATE_TELEMETRY;
 603                             } else
 604 #endif
 605                             {
 606                                 cc2500Strobe(CC2500_SRX);
 607                                 protocolState = STATE_UPDATE;
 608                             }
 609                             ret = RX_SPI_RECEIVED_DATA;
 610                             lastPacketReceivedTime = currentPacketReceivedTime;
 611                         }
 612                     }
 613                 }
 614             }
 615         }
 616
 617         if (cmpTimeUs(currentPacketReceivedTime, lastPacketReceivedTime) > (t_out * SYNC)) {
 618 #ifdef USE_FRSKY_RX_PA_LNA
 619             RX_enable();
 620 #endif
 621             if (t_out == 1) {
 622 #ifdef USE_FRSKY_RX_DIVERSITY // SE4311 chip
 623                 if (missingPackets >= 2) {
 624                     if (pass & 0x01) {
 625                         IOHi(antSelPin);
 626                     } else {
 627                         IOLo(antSelPin);
 628                     }
 629                     pass++;
 630                 }
 631 #endif
 632
 633                 if (missingPackets > MAX_MISSING_PKT)
 634                     t_out = 50;
 635
 636                 missingPackets++;
 637                 nextChannel(1);
 638             } else {
 639                 if (cnt++ & 0x01) {
 640                     IOLo(frSkyLedPin);
 641                 } else
 642                     IOHi(frSkyLedPin);
 643
 644                 nextChannel(13);
 645             }
 646
 647             cc2500Strobe(CC2500_SRX);
 648             protocolState = STATE_UPDATE;
 649         }
 650         break;
 651 #ifdef USE_FRSKY_D_TELEMETRY
 652     case STATE_TELEMETRY:
 653         if ((micros() - telemetryTime) >= 1380) {
 654             cc2500Strobe(CC2500_SIDLE);
 655             cc2500SetPower(6);
 656             cc2500Strobe(CC2500_SFRX);
 657 #if defined(USE_FRSKY_RX_PA_LNA)
 658             TX_enable();
 659 #endif
 660             cc2500Strobe(CC2500_SIDLE);
 661             cc2500WriteFifo(frame, frame[0] + 1);
 662             protocolState = STATE_DATA;
 663             ret = RX_SPI_RECEIVED_DATA;
 664             lastPacketReceivedTime = currentPacketReceivedTime;
 665         }
 666
 667         break;
 668
 669 #endif
 670     }
 671     return ret;
 672 }
 673
 674 static void frskyD_Rx_Setup(rx_spi_protocol_e protocol)
 675 {
 676     UNUSED(protocol);
 677     // gpio init here
 678     gdoPin = IOGetByTag(IO_TAG(FRSKY_RX_GDO_0_PIN));
 679     IOInit(gdoPin, OWNER_RX_BIND, 0);
 680     IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING);
 681     frSkyLedPin = IOGetByTag(IO_TAG(FRSKY_RX_LED_PIN));
 682     IOInit(frSkyLedPin, OWNER_LED, 0);
 683     IOConfigGPIO(frSkyLedPin, IOCFG_OUT_PP);
 684 #if defined(USE_FRSKY_RX_PA_LNA)
 685     rxEnPin = IOGetByTag(IO_TAG(FRSKY_RX_RX_EN_PIN));
 686     IOInit(rxEnPin, OWNER_RX_BIND, 0);
 687     IOConfigGPIO(rxEnPin, IOCFG_OUT_PP);
 688     txEnPin = IOGetByTag(IO_TAG(FRSKY_RX_TX_EN_PIN));
 689     IOInit(txEnPin, OWNER_RX_BIND, 0);
 690     IOConfigGPIO(txEnPin, IOCFG_OUT_PP);
 691 #endif
 692 #if defined(USE_FRSKY_RX_DIVERSITY)
 693     antSelPin = IOGetByTag(IO_TAG(FRSKY_RX_ANT_SEL_PIN));
 694     IOInit(antSelPin, OWNER_RX_BIND, 0);
 695     IOConfigGPIO(antSelPin, IOCFG_OUT_PP);
 696 #endif
 697 #if defined(BINDPLUG_PIN)
 698     bindPin = IOGetByTag(IO_TAG(BINDPLUG_PIN));
 699     IOInit(bindPin, OWNER_RX_BIND, 0);
 700     IOConfigGPIO(bindPin, IOCFG_IPU);
 701
 702     lastBindPinStatus = IORead(bindPin);
 703 #endif
 704
 705     start_time = millis();
 706     lastPacketReceivedTime = 0;
 707     t_out = 50;
 708     missingPackets = 0;
 709     protocolState = STATE_INIT;
 710 #if defined(USE_FRSKY_RX_DIVERSITY)
 711     IOHi(antSelPin);
 712 #endif
 713 #if defined(USE_FRSKY_RX_PA_LNA)
 714     RX_enable();
 715 #endif
 716
 717 #if defined(USE_FRSKY_D_TELEMETRY)
 718 #if defined(USE_TELEMETRY_FRSKY)
 719     initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi,
 720                                &frSkyTelemetryWriteSpi);
 721 #endif
 722
 723     if (rssiSource == RSSI_SOURCE_NONE) {
 724         rssiSource = RSSI_SOURCE_RX_PROTOCOL;
 725     }
 726 #endif
 727
 728     // if(!frSkySpiDetect())//detect spi working routine
 729     // return;
 730 }
 731
 732 void frSkyDInit(const rxConfig_t *rxConfig,
 733                     rxRuntimeConfig_t *rxRuntimeConfig)
 734 {
 735     rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT;
 736     frskyD_Rx_Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol);
 737 }
 738 #endif