src/main/rx/cc2500_frsky_shared.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22
  23 #include "platform.h"
  24
  25 #ifdef USE_RX_FRSKY_SPI
  26
  27 #include "common/maths.h"
  28
  29 #include "drivers/io.h"
  30 #include "drivers/rx/rx_cc2500.h"
  31 #include "drivers/rx/rx_spi.h"
  32 #include "drivers/time.h"
  33
  34 #include "config/config.h"
  35
  36 #include "pg/rx.h"
  37 #include "pg/rx_spi.h"
  38 #include "pg/rx_spi_cc2500.h"
  39
  40 #include "rx/rx.h"
  41 #include "rx/rx_spi.h"
  42 #include "rx/rx_spi_common.h"
  43
  44 #include "rx/cc2500_common.h"
  45 #include "rx/cc2500_frsky_common.h"
  46 #include "rx/cc2500_frsky_d.h"
  47 #include "rx/cc2500_frsky_x.h"
  48
  49 #include "cc2500_frsky_shared.h"
  50
  51 rx_spi_protocol_e spiProtocol;
  52
  53 static timeMs_t start_time;
  54 static uint8_t packetLength;
  55 static uint8_t protocolState;
  56
  57 uint32_t missingPackets;
  58 timeDelta_t timeoutUs;
  59
  60 static uint8_t calData[255][3];
  61 static timeMs_t timeTunedMs;
  62 uint8_t listLength;
  63 static uint16_t bindState;
  64 static int8_t bindOffset, bindOffset_min, bindOffset_max;
  65
  66 typedef uint8_t handlePacketFn(uint8_t * const packet, uint8_t * const protocolState);
  67 typedef rx_spi_received_e processFrameFn(uint8_t * const packet);
  68 typedef void setRcDataFn(uint16_t *rcData, const uint8_t *payload);
  69
  70 static handlePacketFn *handlePacket;
  71 static processFrameFn *processFrame;
  72 static setRcDataFn *setRcData;
  73
  74 const cc2500RegisterConfigElement_t cc2500FrskyBaseConfig[] =
  75 {
  76     { CC2500_02_IOCFG0,   0x01 },
  77     { CC2500_18_MCSM0,    0x18 },
  78     { CC2500_07_PKTCTRL1, 0x05 },
  79     { CC2500_3E_PATABLE,  0xFF },
  80     { CC2500_0C_FSCTRL0,  0x00 },
  81     { CC2500_0D_FREQ2,    0x5C },
  82     { CC2500_13_MDMCFG1,  0x23 },
  83     { CC2500_14_MDMCFG0,  0x7A },
  84     { CC2500_19_FOCCFG,   0x16 },
  85     { CC2500_1A_BSCFG,    0x6C },
  86     { CC2500_1B_AGCCTRL2, 0x03 },
  87     { CC2500_1C_AGCCTRL1, 0x40 },
  88     { CC2500_1D_AGCCTRL0, 0x91 },
  89     { CC2500_21_FREND1,   0x56 },
  90     { CC2500_22_FREND0,   0x10 },
  91     { CC2500_23_FSCAL3,   0xA9 },
  92     { CC2500_24_FSCAL2,   0x0A },
  93     { CC2500_25_FSCAL1,   0x00 },
  94     { CC2500_26_FSCAL0,   0x11 },
  95     { CC2500_29_FSTEST,   0x59 },
  96     { CC2500_2C_TEST2,    0x88 },
  97     { CC2500_2D_TEST1,    0x31 },
  98     { CC2500_2E_TEST0,    0x0B },
  99     { CC2500_03_FIFOTHR,  0x07 },
 100     { CC2500_09_ADDR,     0x00 }
 101 };
 102
 103 const cc2500RegisterConfigElement_t cc2500FrskyDConfig[] =
 104 {
 105     { CC2500_17_MCSM1,    0x0C },
 106     { CC2500_0E_FREQ1,    0x76 },
 107     { CC2500_0F_FREQ0,    0x27 },
 108     { CC2500_06_PKTLEN,   0x19 },
 109     { CC2500_08_PKTCTRL0, 0x05 },
 110     { CC2500_0B_FSCTRL1,  0x08 },
 111     { CC2500_10_MDMCFG4,  0xAA },
 112     { CC2500_11_MDMCFG3,  0x39 },
 113     { CC2500_12_MDMCFG2,  0x11 },
 114     { CC2500_15_DEVIATN,  0x42 }
 115 };
 116
 117 const cc2500RegisterConfigElement_t cc2500FrskyXConfig[] =
 118 {
 119     { CC2500_17_MCSM1,    0x0C },
 120     { CC2500_0E_FREQ1,    0x76 },
 121     { CC2500_0F_FREQ0,    0x27 },
 122     { CC2500_06_PKTLEN,   0x1E },
 123     { CC2500_08_PKTCTRL0, 0x01 },
 124     { CC2500_0B_FSCTRL1,  0x0A },
 125     { CC2500_10_MDMCFG4,  0x7B },
 126     { CC2500_11_MDMCFG3,  0x61 },
 127     { CC2500_12_MDMCFG2,  0x13 },
 128     { CC2500_15_DEVIATN,  0x51 }
 129 };
 130
 131 const cc2500RegisterConfigElement_t cc2500FrskyXLbtConfig[] =
 132 {
 133     { CC2500_17_MCSM1,    0x0E },
 134     { CC2500_0E_FREQ1,    0x80 },
 135     { CC2500_0F_FREQ0,    0x00 },
 136     { CC2500_06_PKTLEN,   0x23 },
 137     { CC2500_08_PKTCTRL0, 0x01 },
 138     { CC2500_0B_FSCTRL1,  0x08 },
 139     { CC2500_10_MDMCFG4,  0x7B },
 140     { CC2500_11_MDMCFG3,  0xF8 },
 141     { CC2500_12_MDMCFG2,  0x03 },
 142     { CC2500_15_DEVIATN,  0x53 }
 143 };
 144
 145 const cc2500RegisterConfigElement_t cc2500FrskyXV2Config[] =
 146 {
 147     { CC2500_17_MCSM1,    0x0E },
 148     { CC2500_08_PKTCTRL0, 0x05 },
 149     { CC2500_11_MDMCFG3,  0x84 },
 150 };
 151
 152 const cc2500RegisterConfigElement_t cc2500FrskyXLbtV2Config[] =
 153 {
 154     { CC2500_08_PKTCTRL0, 0x05 },
 155 };
 156
 157 static void initialise() {
 158     rxSpiStartupSpeed();
 159
 160     cc2500Reset();
 161
 162     cc2500ApplyRegisterConfig(cc2500FrskyBaseConfig, sizeof(cc2500FrskyBaseConfig));
 163
 164     switch (spiProtocol) {
 165     case RX_SPI_FRSKY_D:
 166         cc2500ApplyRegisterConfig(cc2500FrskyDConfig, sizeof(cc2500FrskyDConfig));
 167
 168         break;
 169     case RX_SPI_FRSKY_X:
 170         cc2500ApplyRegisterConfig(cc2500FrskyXConfig, sizeof(cc2500FrskyXConfig));
 171
 172         break;
 173     case RX_SPI_FRSKY_X_LBT:
 174         cc2500ApplyRegisterConfig(cc2500FrskyXLbtConfig, sizeof(cc2500FrskyXLbtConfig));
 175
 176         break;
 177     case RX_SPI_FRSKY_X_V2:
 178         cc2500ApplyRegisterConfig(cc2500FrskyXConfig, sizeof(cc2500FrskyXConfig));
 179         cc2500ApplyRegisterConfig(cc2500FrskyXV2Config, sizeof(cc2500FrskyXV2Config));
 180
 181         break;
 182     case RX_SPI_FRSKY_X_LBT_V2:
 183         cc2500ApplyRegisterConfig(cc2500FrskyXLbtConfig, sizeof(cc2500FrskyXLbtConfig));
 184         cc2500ApplyRegisterConfig(cc2500FrskyXLbtV2Config, sizeof(cc2500FrskyXLbtV2Config));
 185
 186         break;
 187     default:
 188
 189         break;
 190     }
 191
 192     for(unsigned c = 0;c < 0xFF; c++)
 193     { //calibrate all channels
 194         cc2500Strobe(CC2500_SIDLE);
 195         cc2500WriteReg(CC2500_0A_CHANNR, c);
 196         cc2500Strobe(CC2500_SCAL);
 197         delayMicroseconds(900); //
 198         calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3);
 199         calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2);
 200         calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1);
 201     }
 202
 203     rxSpiNormalSpeed();
 204 }
 205
 206 void initialiseData(bool inBindState)
 207 {
 208     cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)rxCc2500SpiConfig()->bindOffset);
 209     cc2500WriteReg(CC2500_18_MCSM0, 0x8);
 210     cc2500WriteReg(CC2500_09_ADDR, inBindState ? 0x03 : rxCc2500SpiConfig()->bindTxId[0]);
 211     cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D);
 212     cc2500WriteReg(CC2500_19_FOCCFG, 0x16);
 213     if (!inBindState) {
 214         cc2500WriteReg(CC2500_03_FIFOTHR,  0x0E);
 215     }
 216     delay(10);
 217 }
 218
 219 static void initTuneRx(void)
 220 {
 221     cc2500WriteReg(CC2500_19_FOCCFG, 0x14);
 222     timeTunedMs = millis();
 223     bindOffset = -126;
 224     cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
 225     cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0C);
 226     cc2500WriteReg(CC2500_18_MCSM0, 0x8);
 227
 228     cc2500Strobe(CC2500_SIDLE);
 229     cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]);
 230     cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]);
 231     cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]);
 232     cc2500WriteReg(CC2500_0A_CHANNR, 0);
 233     cc2500Strobe(CC2500_SFRX);
 234     cc2500Strobe(CC2500_SRX);
 235 }
 236
 237 static bool isValidBindPacket(uint8_t *packet)
 238 {
 239     if (spiProtocol == RX_SPI_FRSKY_D || spiProtocol == RX_SPI_FRSKY_X_V2 || spiProtocol == RX_SPI_FRSKY_X_LBT_V2) {
 240         if (!(packet[packetLength - 1] & 0x80)) {
 241             return false;
 242         }
 243     }
 244     if ((packet[0] == packetLength - 3) && (packet[1] == 0x03) && (packet[2] == 0x01)) {
 245         return true;
 246     }
 247
 248     return false;
 249 }
 250
 251 static bool tuneRx(uint8_t *packet, int8_t inc)
 252 {
 253     if ((millis() - timeTunedMs) > 50 || bindOffset == 126 || bindOffset == -126) {
 254         timeTunedMs = millis();
 255         bindOffset += inc;
 256         cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
 257         cc2500Strobe(CC2500_SRX);
 258     }
 259     if (rxSpiGetExtiState()) {
 260         uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
 261         if (ccLen >= packetLength) {
 262             cc2500ReadFifo(packet, packetLength);
 263             if (isValidBindPacket(packet)) {
 264                 return true;
 265             }
 266         }
 267     }
 268
 269     return false;
 270 }
 271
 272 static void initGetBind(void)
 273 {
 274     cc2500Strobe(CC2500_SIDLE);
 275     cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]);
 276     cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]);
 277     cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]);
 278     cc2500WriteReg(CC2500_0A_CHANNR, 0);
 279     cc2500Strobe(CC2500_SFRX);
 280     delayMicroseconds(20); // waiting flush FIFO
 281
 282     cc2500Strobe(CC2500_SRX);
 283     listLength = 0;
 284     bindState = 0;
 285 }
 286
 287 static void generateV2HopData(uint16_t id)
 288 {
 289     uint8_t inc = (id % 46) + 1;                                // Increment
 290     if (inc == 12 || inc == 35) inc++;                          // Exception list from dumps
 291     uint8_t offset = id % 5;                                    // Start offset
 292
 293     uint8_t channel;
 294     for (uint8_t i = 0; i < 47; i++) {
 295         channel = 5 * ((uint16_t)(inc * i) % 47) + offset;      // Exception list from dumps
 296         if (spiProtocol == RX_SPI_FRSKY_X_LBT_V2) {             // LBT or FCC
 297             // LBT
 298             if (channel <=1 || channel == 43 || channel == 44 || channel == 87 || channel == 88 || channel == 129 || channel == 130 || channel == 173 || channel == 174) {
 299                 channel += 2;
 300             }
 301             else if (channel == 216 || channel == 217 || channel == 218) {
 302                 channel += 3;
 303             }
 304         } else {
 305             // FCC
 306             if (channel == 3 || channel == 4 || channel == 46 || channel == 47 || channel == 90 || channel == 91  || channel == 133 || channel == 134 || channel == 176 || channel == 177 || channel == 220 || channel == 221) {
 307                 channel += 2;
 308             }
 309         }
 310         rxCc2500SpiConfigMutable()->bindHopData[i] = channel;   // Store
 311     }
 312     rxCc2500SpiConfigMutable()->bindHopData[47] = 0;            //Bind freq
 313     rxCc2500SpiConfigMutable()->bindHopData[48] = 0;
 314     rxCc2500SpiConfigMutable()->bindHopData[49] = 0;
 315 }
 316
 317 static bool getBind(uint8_t *packet)
 318 {
 319     // len|bind |tx
 320     // 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|
 321     if (rxSpiGetExtiState()) {
 322         uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
 323         if (ccLen >= packetLength) {
 324             cc2500ReadFifo(packet, packetLength);
 325             if (isValidBindPacket(packet)) {
 326                 if (spiProtocol == RX_SPI_FRSKY_X_V2 || spiProtocol == RX_SPI_FRSKY_X_LBT_V2) {
 327                     for (uint8_t i = 3; i < packetLength - 4; i++) {
 328                         packet[i] ^= 0xA7;
 329                     }
 330                     rxCc2500SpiConfigMutable()->bindTxId[0] = packet[3];
 331                     rxCc2500SpiConfigMutable()->bindTxId[1] = packet[4];
 332                     rxCc2500SpiConfigMutable()->bindTxId[2] = packet[5];
 333                     rxCc2500SpiConfigMutable()->rxNum = packet[6];
 334                     generateV2HopData((packet[4] << 8) + packet[3]);
 335                     listLength = 47;
 336
 337                     return true;
 338                 } else {
 339                     if (packet[5] == 0x00) {
 340                         rxCc2500SpiConfigMutable()->bindTxId[0] = packet[3];
 341                         rxCc2500SpiConfigMutable()->bindTxId[1] = packet[4];
 342                         if (spiProtocol == RX_SPI_FRSKY_D) {
 343                             rxCc2500SpiConfigMutable()->bindTxId[2] = packet[17];
 344                             rxCc2500SpiConfigMutable()->rxNum = 0;
 345                         } else {
 346                             rxCc2500SpiConfigMutable()->bindTxId[2] = packet[11];
 347                             rxCc2500SpiConfigMutable()->rxNum = packet[12];
 348                         }
 349                     }
 350                     for (uint8_t n = 0; n < 5; n++) {
 351                          rxCc2500SpiConfigMutable()->bindHopData[packet[5] + n] = (packet[5] + n) >= 47 ? 0 : packet[6 + n];
 352                     }
 353                     bindState |= 1 << (packet[5] / 5);
 354                     if (bindState == 0x3FF) {
 355                         listLength = 47;
 356
 357                         return true;
 358                     }
 359                 }
 360             }
 361         }
 362     }
 363
 364     return false;
 365 }
 366
 367 rx_spi_received_e frSkySpiDataReceived(uint8_t *packet)
 368 {
 369     rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
 370
 371     switch (protocolState) {
 372     case STATE_INIT:
 373         if ((millis() - start_time) > 10) {
 374             initialise();
 375
 376             protocolState = STATE_BIND;
 377         }
 378
 379         break;
 380     case STATE_BIND:
 381         if (rxSpiCheckBindRequested(true) || rxCc2500SpiConfig()->autoBind) {
 382             rxSpiLedOn();
 383             initTuneRx();
 384
 385             protocolState = STATE_BIND_TUNING_LOW;
 386         } else {
 387             protocolState = STATE_STARTING;
 388         }
 389
 390         break;
 391     case STATE_BIND_TUNING_LOW:
 392        if (tuneRx(packet, 2)) {
 393             bindOffset_min = bindOffset;
 394             bindOffset = 126;
 395
 396             protocolState = STATE_BIND_TUNING_HIGH;
 397         }
 398
 399         break;
 400     case STATE_BIND_TUNING_HIGH:
 401         if (tuneRx(packet, -2)) {
 402             bindOffset_max = bindOffset;
 403             bindOffset = ((int16_t)bindOffset_max + (int16_t)bindOffset_min) / 2;
 404             rxCc2500SpiConfigMutable()->bindOffset = bindOffset;
 405             initGetBind();
 406             initialiseData(true);
 407
 408             if(bindOffset_min < bindOffset_max)
 409                 protocolState = STATE_BIND_BINDING;
 410             else
 411                 protocolState = STATE_BIND;
 412         }
 413
 414         break;
 415     case STATE_BIND_BINDING:
 416         if (getBind(packet)) {
 417             cc2500Strobe(CC2500_SIDLE);
 418
 419             protocolState = STATE_BIND_COMPLETE;
 420         }
 421
 422         break;
 423     case STATE_BIND_COMPLETE:
 424         if (!rxCc2500SpiConfig()->autoBind) {
 425             writeEEPROM();
 426         } else {
 427             uint8_t ctr = 80;
 428             while (ctr--) {
 429                 rxSpiLedToggle();
 430                 delay(50);
 431             }
 432         }
 433
 434         ret = RX_SPI_RECEIVED_BIND;
 435         protocolState = STATE_STARTING;
 436
 437         break;
 438     default:
 439         ret = handlePacket(packet, &protocolState);
 440
 441         break;
 442     }
 443
 444     return ret;
 445 }
 446
 447 rx_spi_received_e frSkySpiProcessFrame(uint8_t *packet)
 448 {
 449     if (processFrame) {
 450         return processFrame(packet);
 451     }
 452
 453     return RX_SPI_RECEIVED_NONE;
 454 }
 455
 456 void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload)
 457 {
 458     setRcData(rcData, payload);
 459 }
 460
 461 void nextChannel(uint8_t skip)
 462 {
 463     static uint8_t channr = 0;
 464
 465     channr += skip;
 466     while (channr >= listLength) {
 467         channr -= listLength;
 468     }
 469     cc2500Strobe(CC2500_SIDLE);
 470     cc2500WriteReg(CC2500_23_FSCAL3,
 471                     calData[rxCc2500SpiConfig()->bindHopData[channr]][0]);
 472     cc2500WriteReg(CC2500_24_FSCAL2,
 473                     calData[rxCc2500SpiConfig()->bindHopData[channr]][1]);
 474     cc2500WriteReg(CC2500_25_FSCAL1,
 475                     calData[rxCc2500SpiConfig()->bindHopData[channr]][2]);
 476     cc2500WriteReg(CC2500_0A_CHANNR, rxCc2500SpiConfig()->bindHopData[channr]);
 477     if (spiProtocol == RX_SPI_FRSKY_D) {
 478         cc2500Strobe(CC2500_SFRX);
 479     }
 480 }
 481
 482 bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
 483 {
 484     UNUSED(extiConfig);
 485
 486     rxSpiCommonIOInit(rxSpiConfig);
 487     if (!cc2500SpiInit()) {
 488         return false;
 489     }
 490
 491     spiProtocol = rxSpiConfig->rx_spi_protocol;
 492
 493     switch (spiProtocol) {
 494     case RX_SPI_FRSKY_D:
 495         rxRuntimeState->channelCount = RC_CHANNEL_COUNT_FRSKY_D;
 496
 497         handlePacket = frSkyDHandlePacket;
 498         setRcData = frSkyDSetRcData;
 499         packetLength = FRSKY_RX_D8_LENGTH;
 500         frSkyDInit();
 501
 502         break;
 503     case RX_SPI_FRSKY_X:
 504     case RX_SPI_FRSKY_X_LBT:
 505     case RX_SPI_FRSKY_X_V2:
 506     case RX_SPI_FRSKY_X_LBT_V2:
 507         rxRuntimeState->channelCount = RC_CHANNEL_COUNT_FRSKY_X;
 508
 509         handlePacket = frSkyXHandlePacket;
 510 #if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
 511         processFrame = frSkyXProcessFrame;
 512 #endif
 513         setRcData = frSkyXSetRcData;
 514         packetLength = frSkyXInit();
 515
 516         break;
 517     default:
 518
 519         break;
 520     }
 521
 522 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 523     if (rssiSource == RSSI_SOURCE_NONE) {
 524         rssiSource = RSSI_SOURCE_RX_PROTOCOL;
 525     }
 526 #endif
 527
 528     missingPackets = 0;
 529     timeoutUs = 50;
 530
 531     start_time = millis();
 532     protocolState = STATE_INIT;
 533
 534     return true;
 535 }
 536 #endif