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(void)
 158 {
 159     rxSpiStartupSpeed();
 160
 161     cc2500Reset();
 162
 163     cc2500ApplyRegisterConfig(cc2500FrskyBaseConfig, sizeof(cc2500FrskyBaseConfig));
 164
 165     switch (spiProtocol) {
 166     case RX_SPI_FRSKY_D:
 167         cc2500ApplyRegisterConfig(cc2500FrskyDConfig, sizeof(cc2500FrskyDConfig));
 168
 169         break;
 170     case RX_SPI_FRSKY_X:
 171         cc2500ApplyRegisterConfig(cc2500FrskyXConfig, sizeof(cc2500FrskyXConfig));
 172
 173         break;
 174     case RX_SPI_FRSKY_X_LBT:
 175         cc2500ApplyRegisterConfig(cc2500FrskyXLbtConfig, sizeof(cc2500FrskyXLbtConfig));
 176
 177         break;
 178     case RX_SPI_FRSKY_X_V2:
 179         cc2500ApplyRegisterConfig(cc2500FrskyXConfig, sizeof(cc2500FrskyXConfig));
 180         cc2500ApplyRegisterConfig(cc2500FrskyXV2Config, sizeof(cc2500FrskyXV2Config));
 181
 182         break;
 183     case RX_SPI_FRSKY_X_LBT_V2:
 184         cc2500ApplyRegisterConfig(cc2500FrskyXLbtConfig, sizeof(cc2500FrskyXLbtConfig));
 185         cc2500ApplyRegisterConfig(cc2500FrskyXLbtV2Config, sizeof(cc2500FrskyXLbtV2Config));
 186
 187         break;
 188     default:
 189
 190         break;
 191     }
 192
 193     for(unsigned c = 0;c < 0xFF; c++)
 194     { //calibrate all channels
 195         cc2500Strobe(CC2500_SIDLE);
 196         cc2500WriteReg(CC2500_0A_CHANNR, c);
 197         cc2500Strobe(CC2500_SCAL);
 198         delayMicroseconds(900); //
 199         calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3);
 200         calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2);
 201         calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1);
 202     }
 203
 204     rxSpiNormalSpeed();
 205 }
 206
 207 void initialiseData(bool inBindState)
 208 {
 209     cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)rxCc2500SpiConfig()->bindOffset);
 210     cc2500WriteReg(CC2500_18_MCSM0, 0x8);
 211     cc2500WriteReg(CC2500_09_ADDR, inBindState ? 0x03 : rxCc2500SpiConfig()->bindTxId[0]);
 212     cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D);
 213     cc2500WriteReg(CC2500_19_FOCCFG, 0x16);
 214     if (!inBindState) {
 215         cc2500WriteReg(CC2500_03_FIFOTHR,  0x0E);
 216     }
 217     delay(10);
 218 }
 219
 220 static void initTuneRx(void)
 221 {
 222     cc2500WriteReg(CC2500_19_FOCCFG, 0x14);
 223     timeTunedMs = millis();
 224     bindOffset = -126;
 225     cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
 226     cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0C);
 227     cc2500WriteReg(CC2500_18_MCSM0, 0x8);
 228
 229     cc2500Strobe(CC2500_SIDLE);
 230     cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]);
 231     cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]);
 232     cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]);
 233     cc2500WriteReg(CC2500_0A_CHANNR, 0);
 234     cc2500Strobe(CC2500_SFRX);
 235     cc2500Strobe(CC2500_SRX);
 236 }
 237
 238 static bool isValidBindPacket(uint8_t *packet)
 239 {
 240     if (spiProtocol == RX_SPI_FRSKY_D || spiProtocol == RX_SPI_FRSKY_X_V2 || spiProtocol == RX_SPI_FRSKY_X_LBT_V2) {
 241         if (!(packet[packetLength - 1] & 0x80)) {
 242             return false;
 243         }
 244     }
 245     if ((packet[0] == packetLength - 3) && (packet[1] == 0x03) && (packet[2] == 0x01)) {
 246         return true;
 247     }
 248
 249     return false;
 250 }
 251
 252 static bool tuneRx(uint8_t *packet, int8_t inc)
 253 {
 254     if ((millis() - timeTunedMs) > 50 || bindOffset == 126 || bindOffset == -126) {
 255         timeTunedMs = millis();
 256         bindOffset += inc;
 257         cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
 258         cc2500Strobe(CC2500_SRX);
 259     }
 260     if (rxSpiGetExtiState()) {
 261         uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
 262         if (ccLen >= packetLength) {
 263             cc2500ReadFifo(packet, packetLength);
 264             if (isValidBindPacket(packet)) {
 265                 return true;
 266             }
 267         }
 268     }
 269
 270     return false;
 271 }
 272
 273 static void initGetBind(void)
 274 {
 275     cc2500Strobe(CC2500_SIDLE);
 276     cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]);
 277     cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]);
 278     cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]);
 279     cc2500WriteReg(CC2500_0A_CHANNR, 0);
 280     cc2500Strobe(CC2500_SFRX);
 281     delayMicroseconds(20); // waiting flush FIFO
 282
 283     cc2500Strobe(CC2500_SRX);
 284     listLength = 0;
 285     bindState = 0;
 286 }
 287
 288 static void generateV2HopData(uint16_t id)
 289 {
 290     uint8_t inc = (id % 46) + 1;                                // Increment
 291     if (inc == 12 || inc == 35) inc++;                          // Exception list from dumps
 292     uint8_t offset = id % 5;                                    // Start offset
 293
 294     uint8_t channel;
 295     for (uint8_t i = 0; i < 47; i++) {
 296         channel = 5 * ((uint16_t)(inc * i) % 47) + offset;      // Exception list from dumps
 297         if (spiProtocol == RX_SPI_FRSKY_X_LBT_V2) {             // LBT or FCC
 298             // LBT
 299             if (channel <=1 || channel == 43 || channel == 44 || channel == 87 || channel == 88 || channel == 129 || channel == 130 || channel == 173 || channel == 174) {
 300                 channel += 2;
 301             }
 302             else if (channel == 216 || channel == 217 || channel == 218) {
 303                 channel += 3;
 304             }
 305         } else {
 306             // FCC
 307             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) {
 308                 channel += 2;
 309             }
 310         }
 311         rxCc2500SpiConfigMutable()->bindHopData[i] = channel;   // Store
 312     }
 313     rxCc2500SpiConfigMutable()->bindHopData[47] = 0;            //Bind freq
 314     rxCc2500SpiConfigMutable()->bindHopData[48] = 0;
 315     rxCc2500SpiConfigMutable()->bindHopData[49] = 0;
 316 }
 317
 318 static bool getBind(uint8_t *packet)
 319 {
 320     // len|bind |tx
 321     // 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|
 322     if (rxSpiGetExtiState()) {
 323         uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
 324         if (ccLen >= packetLength) {
 325             cc2500ReadFifo(packet, packetLength);
 326             if (isValidBindPacket(packet)) {
 327                 if (spiProtocol == RX_SPI_FRSKY_X_V2 || spiProtocol == RX_SPI_FRSKY_X_LBT_V2) {
 328                     for (uint8_t i = 3; i < packetLength - 4; i++) {
 329                         packet[i] ^= 0xA7;
 330                     }
 331                     rxCc2500SpiConfigMutable()->bindTxId[0] = packet[3];
 332                     rxCc2500SpiConfigMutable()->bindTxId[1] = packet[4];
 333                     rxCc2500SpiConfigMutable()->bindTxId[2] = packet[5];
 334                     rxCc2500SpiConfigMutable()->rxNum = packet[6];
 335                     generateV2HopData((packet[4] << 8) + packet[3]);
 336                     listLength = 47;
 337
 338                     return true;
 339                 } else {
 340                     if (packet[5] == 0x00) {
 341                         rxCc2500SpiConfigMutable()->bindTxId[0] = packet[3];
 342                         rxCc2500SpiConfigMutable()->bindTxId[1] = packet[4];
 343                         if (spiProtocol == RX_SPI_FRSKY_D) {
 344                             rxCc2500SpiConfigMutable()->bindTxId[2] = packet[17];
 345                             rxCc2500SpiConfigMutable()->rxNum = 0;
 346                         } else {
 347                             rxCc2500SpiConfigMutable()->bindTxId[2] = packet[11];
 348                             rxCc2500SpiConfigMutable()->rxNum = packet[12];
 349                         }
 350                     }
 351                     for (uint8_t n = 0; n < 5; n++) {
 352                          rxCc2500SpiConfigMutable()->bindHopData[packet[5] + n] = (packet[5] + n) >= 47 ? 0 : packet[6 + n];
 353                     }
 354                     bindState |= 1 << (packet[5] / 5);
 355                     if (bindState == 0x3FF) {
 356                         listLength = 47;
 357
 358                         return true;
 359                     }
 360                 }
 361             }
 362         }
 363     }
 364
 365     return false;
 366 }
 367
 368 rx_spi_received_e frSkySpiDataReceived(uint8_t *packet)
 369 {
 370     rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
 371
 372     switch (protocolState) {
 373     case STATE_INIT:
 374         if ((millis() - start_time) > 10) {
 375             initialise();
 376
 377             protocolState = STATE_BIND;
 378         }
 379
 380         break;
 381     case STATE_BIND:
 382         if (rxSpiCheckBindRequested(true) || rxCc2500SpiConfig()->autoBind) {
 383             rxSpiLedOn();
 384             initTuneRx();
 385
 386             protocolState = STATE_BIND_TUNING_LOW;
 387         } else {
 388             protocolState = STATE_STARTING;
 389         }
 390
 391         break;
 392     case STATE_BIND_TUNING_LOW:
 393        if (tuneRx(packet, 2)) {
 394             bindOffset_min = bindOffset;
 395             bindOffset = 126;
 396
 397             protocolState = STATE_BIND_TUNING_HIGH;
 398         }
 399
 400         break;
 401     case STATE_BIND_TUNING_HIGH:
 402         if (tuneRx(packet, -2)) {
 403             bindOffset_max = bindOffset;
 404             bindOffset = ((int16_t)bindOffset_max + (int16_t)bindOffset_min) / 2;
 405             rxCc2500SpiConfigMutable()->bindOffset = bindOffset;
 406             initGetBind();
 407             initialiseData(true);
 408
 409             if(bindOffset_min < bindOffset_max)
 410                 protocolState = STATE_BIND_BINDING;
 411             else
 412                 protocolState = STATE_BIND;
 413         }
 414
 415         break;
 416     case STATE_BIND_BINDING:
 417         if (getBind(packet)) {
 418             cc2500Strobe(CC2500_SIDLE);
 419
 420             protocolState = STATE_BIND_COMPLETE;
 421         }
 422
 423         break;
 424     case STATE_BIND_COMPLETE:
 425         if (!rxCc2500SpiConfig()->autoBind) {
 426             writeEEPROM();
 427         } else {
 428             uint8_t ctr = 80;
 429             while (ctr--) {
 430                 rxSpiLedToggle();
 431                 delay(50);
 432             }
 433         }
 434
 435         ret = RX_SPI_RECEIVED_BIND;
 436         protocolState = STATE_STARTING;
 437
 438         break;
 439
 440     case STATE_STARTING:
 441     default:
 442         ret = handlePacket(packet, &protocolState);
 443
 444         break;
 445     }
 446
 447     return ret;
 448 }
 449
 450 rx_spi_received_e frSkySpiProcessFrame(uint8_t *packet)
 451 {
 452     if (processFrame) {
 453         return processFrame(packet);
 454     }
 455
 456     return RX_SPI_RECEIVED_NONE;
 457 }
 458
 459 void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload)
 460 {
 461     setRcData(rcData, payload);
 462 }
 463
 464 void nextChannel(uint8_t skip)
 465 {
 466     static uint8_t channr = 0;
 467
 468     channr += skip;
 469     while (channr >= listLength) {
 470         channr -= listLength;
 471     }
 472     cc2500Strobe(CC2500_SIDLE);
 473     cc2500WriteReg(CC2500_23_FSCAL3,
 474                     calData[rxCc2500SpiConfig()->bindHopData[channr]][0]);
 475     cc2500WriteReg(CC2500_24_FSCAL2,
 476                     calData[rxCc2500SpiConfig()->bindHopData[channr]][1]);
 477     cc2500WriteReg(CC2500_25_FSCAL1,
 478                     calData[rxCc2500SpiConfig()->bindHopData[channr]][2]);
 479     cc2500WriteReg(CC2500_0A_CHANNR, rxCc2500SpiConfig()->bindHopData[channr]);
 480     if (spiProtocol == RX_SPI_FRSKY_D) {
 481         cc2500Strobe(CC2500_SFRX);
 482     }
 483 }
 484
 485 bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
 486 {
 487     UNUSED(extiConfig);
 488
 489     rxSpiCommonIOInit(rxSpiConfig);
 490     if (!cc2500SpiInit()) {
 491         return false;
 492     }
 493
 494     spiProtocol = rxSpiConfig->rx_spi_protocol;
 495
 496     switch (spiProtocol) {
 497     case RX_SPI_FRSKY_D:
 498         rxRuntimeState->channelCount = RC_CHANNEL_COUNT_FRSKY_D;
 499
 500         handlePacket = frSkyDHandlePacket;
 501         setRcData = frSkyDSetRcData;
 502         packetLength = FRSKY_RX_D8_LENGTH;
 503         frSkyDInit();
 504
 505         break;
 506     case RX_SPI_FRSKY_X:
 507     case RX_SPI_FRSKY_X_LBT:
 508     case RX_SPI_FRSKY_X_V2:
 509     case RX_SPI_FRSKY_X_LBT_V2:
 510         rxRuntimeState->channelCount = RC_CHANNEL_COUNT_FRSKY_X;
 511
 512         handlePacket = frSkyXHandlePacket;
 513 #if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
 514         processFrame = frSkyXProcessFrame;
 515 #endif
 516         setRcData = frSkyXSetRcData;
 517         packetLength = frSkyXInit();
 518
 519         break;
 520     default:
 521
 522         break;
 523     }
 524
 525 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 526     if (rssiSource == RSSI_SOURCE_NONE) {
 527         rssiSource = RSSI_SOURCE_RX_PROTOCOL;
 528     }
 529 #endif
 530
 531     missingPackets = 0;
 532     timeoutUs = 50;
 533
 534     start_time = millis();
 535     protocolState = STATE_INIT;
 536
 537     return true;
 538 }
 539 #endif