src/main/rx/a7105_flysky.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 <string.h>
  22
  23 #include "platform.h"
  24
  25 #ifdef USE_RX_FLYSKY
  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/io.h"
  34 #include "drivers/rx/rx_a7105.h"
  35 #include "drivers/rx/rx_spi.h"
  36 #include "drivers/system.h"
  37 #include "drivers/time.h"
  38
  39 #include "config/config.h"
  40
  41 #include "pg/pg.h"
  42 #include "pg/pg_ids.h"
  43 #include "pg/rx_spi.h"
  44
  45 #include "rx/a7105_flysky_defs.h"
  46 #include "rx/rx.h"
  47 #include "rx/rx_spi.h"
  48 #include "rx/rx_spi_common.h"
  49
  50 #include "sensors/battery.h"
  51
  52 #include "a7105_flysky.h"
  53
  54 #if FLYSKY_CHANNEL_COUNT > MAX_FLYSKY_CHANNEL_COUNT
  55 #error "FlySky AFHDS protocol support 8 channel max"
  56 #endif
  57
  58 #if FLYSKY_2A_CHANNEL_COUNT > MAX_FLYSKY_2A_CHANNEL_COUNT
  59 #error "FlySky AFHDS 2A protocol support 14 channel max"
  60 #endif
  61
  62 PG_REGISTER_WITH_RESET_TEMPLATE(flySkyConfig_t, flySkyConfig, PG_FLYSKY_CONFIG, 1);
  63 PG_RESET_TEMPLATE(flySkyConfig_t, flySkyConfig, .txId = 0, .rfChannelMap = {0});
  64
  65 static const uint8_t flySkyRegs[] = {
  66     0xff, 0x42, 0x00, 0x14, 0x00, 0xff, 0xff, 0x00,
  67     0x00, 0x00, 0x00, 0x03, 0x19, 0x05, 0x00, 0x50,
  68     0x9e, 0x4b, 0x00, 0x02, 0x16, 0x2b, 0x12, 0x00,
  69     0x62, 0x80, 0x80, 0x00, 0x0a, 0x32, 0xc3, 0x0f,
  70     0x13, 0xc3, 0x00, 0xff, 0x00, 0x00, 0x3b, 0x00,
  71     0x17, 0x47, 0x80, 0x03, 0x01, 0x45, 0x18, 0x00,
  72     0x01, 0x0f
  73 };
  74
  75 static const uint8_t flySky2ARegs[] = {
  76     0xff, 0x62, 0x00, 0x25, 0x00, 0xff, 0xff, 0x00,
  77     0x00, 0x00, 0x00, 0x03, 0x19, 0x05, 0x00, 0x50,
  78     0x9e, 0x4b, 0x00, 0x02, 0x16, 0x2b, 0x12, 0x4f,
  79     0x62, 0x80, 0xff, 0xff, 0x2a, 0x32, 0xc3, 0x1f,
  80     0x1e, 0xff, 0x00, 0xff, 0x00, 0x00, 0x3b, 0x00,
  81     0x17, 0x47, 0x80, 0x03, 0x01, 0x45, 0x18, 0x00,
  82     0x01, 0x0f
  83 };
  84
  85 static const uint8_t flySky2ABindChannels[] = {
  86     0x0D, 0x8C
  87 };
  88
  89 static const uint8_t flySkyRfChannels[16][16] = {
  90     {   0x0a, 0x5a, 0x14, 0x64, 0x1e, 0x6e, 0x28, 0x78, 0x32, 0x82, 0x3c, 0x8c, 0x46, 0x96, 0x50, 0xa0},
  91     {   0xa0, 0x50, 0x96, 0x46, 0x8c, 0x3c, 0x82, 0x32, 0x78, 0x28, 0x6e, 0x1e, 0x64, 0x14, 0x5a, 0x0a},
  92     {   0x0a, 0x5a, 0x50, 0xa0, 0x14, 0x64, 0x46, 0x96, 0x1e, 0x6e, 0x3c, 0x8c, 0x28, 0x78, 0x32, 0x82},
  93     {   0x82, 0x32, 0x78, 0x28, 0x8c, 0x3c, 0x6e, 0x1e, 0x96, 0x46, 0x64, 0x14, 0xa0, 0x50, 0x5a, 0x0a},
  94     {   0x28, 0x78, 0x0a, 0x5a, 0x50, 0xa0, 0x14, 0x64, 0x1e, 0x6e, 0x3c, 0x8c, 0x32, 0x82, 0x46, 0x96},
  95     {   0x96, 0x46, 0x82, 0x32, 0x8c, 0x3c, 0x6e, 0x1e, 0x64, 0x14, 0xa0, 0x50, 0x5a, 0x0a, 0x78, 0x28},
  96     {   0x50, 0xa0, 0x28, 0x78, 0x0a, 0x5a, 0x1e, 0x6e, 0x3c, 0x8c, 0x32, 0x82, 0x46, 0x96, 0x14, 0x64},
  97     {   0x64, 0x14, 0x96, 0x46, 0x82, 0x32, 0x8c, 0x3c, 0x6e, 0x1e, 0x5a, 0x0a, 0x78, 0x28, 0xa0, 0x50},
  98     {   0x50, 0xa0, 0x46, 0x96, 0x3c, 0x8c, 0x28, 0x78, 0x0a, 0x5a, 0x32, 0x82, 0x1e, 0x6e, 0x14, 0x64},
  99     {   0x64, 0x14, 0x6e, 0x1e, 0x82, 0x32, 0x5a, 0x0a, 0x78, 0x28, 0x8c, 0x3c, 0x96, 0x46, 0xa0, 0x50},
 100     {   0x46, 0x96, 0x3c, 0x8c, 0x50, 0xa0, 0x28, 0x78, 0x0a, 0x5a, 0x1e, 0x6e, 0x32, 0x82, 0x14, 0x64},
 101     {   0x64, 0x14, 0x82, 0x32, 0x6e, 0x1e, 0x5a, 0x0a, 0x78, 0x28, 0xa0, 0x50, 0x8c, 0x3c, 0x96, 0x46},
 102     {   0x46, 0x96, 0x0a, 0x5a, 0x3c, 0x8c, 0x14, 0x64, 0x50, 0xa0, 0x28, 0x78, 0x1e, 0x6e, 0x32, 0x82},
 103     {   0x82, 0x32, 0x6e, 0x1e, 0x78, 0x28, 0xa0, 0x50, 0x64, 0x14, 0x8c, 0x3c, 0x5a, 0x0a, 0x96, 0x46},
 104     {   0x46, 0x96, 0x0a, 0x5a, 0x50, 0xa0, 0x3c, 0x8c, 0x28, 0x78, 0x1e, 0x6e, 0x32, 0x82, 0x14, 0x64},
 105     {   0x64, 0x14, 0x82, 0x32, 0x6e, 0x1e, 0x78, 0x28, 0x8c, 0x3c, 0xa0, 0x50, 0x5a, 0x0a, 0x96, 0x46}
 106 };
 107
 108 const timings_t flySkyTimings = {.packet = 1500, .firstPacket = 1900, .syncPacket = 2250, .telemetry = 0xFFFFFFFF};
 109 const timings_t flySky2ATimings = {.packet = 3850, .firstPacket = 4850, .syncPacket = 5775, .telemetry = 57000};
 110
 111 static rx_spi_protocol_e protocol = RX_SPI_A7105_FLYSKY_2A;
 112 static const timings_t *timings = &flySky2ATimings;
 113 static uint32_t timeout = 0;
 114 static uint32_t timeLastPacket = 0;
 115 static uint32_t timeLastBind = 0;
 116 static uint32_t timeTxRequest = 0;
 117 static uint32_t countTimeout = 0;
 118 static uint32_t countPacket = 0;
 119 static uint32_t txId = 0;
 120 static uint32_t rxId = 0;
 121 static bool bound = false;
 122 static bool sendTelemetry = false;
 123 static bool waitTx = false;
 124 static uint16_t errorRate = 0;
 125 static uint16_t rssi_dBm = 0;
 126 static uint8_t rfChannelMap[FLYSKY_FREQUENCY_COUNT] = {0};
 127
 128 static uint8_t getNextChannel(uint8_t step)
 129 {
 130     static uint8_t channel = 0;
 131     channel = (channel + step) & 0x0F;
 132     return rfChannelMap[channel];
 133 }
 134
 135 static void flySkyCalculateRfChannels(void)
 136 {
 137     uint32_t channelRow = txId & 0x0F;
 138     uint32_t channelOffset = ((txId & 0xF0) >> 4) + 1;
 139
 140     if (channelOffset > 9) {
 141         channelOffset = 9; // from sloped soarer findings, bug in flysky protocol
 142     }
 143
 144     for (unsigned i = 0; i < FLYSKY_FREQUENCY_COUNT; i++) {
 145         rfChannelMap[i] = flySkyRfChannels[channelRow][i] - channelOffset;
 146     }
 147 }
 148
 149 static void resetTimeout(const uint32_t timeStamp)
 150 {
 151     timeLastPacket = timeStamp;
 152     timeout = timings->firstPacket;
 153     countTimeout = 0;
 154     countPacket++;
 155 }
 156
 157 static void checkTimeout(void)
 158 {
 159     static uint32_t timeMeasuareErrRate = 0;
 160     static uint32_t timeLastTelemetry = 0;
 161     uint32_t time = micros();
 162
 163     if ((time - timeMeasuareErrRate) > (101 * timings->packet)) {
 164         errorRate = (countPacket >= 100) ? (0) : (100 - countPacket);
 165         countPacket = 0;
 166         timeMeasuareErrRate = time;
 167     }
 168
 169     if ((time - timeLastTelemetry) > timings->telemetry) {
 170         timeLastTelemetry = time;
 171         sendTelemetry = true;
 172     }
 173
 174     if ((time - timeLastPacket) > timeout) {
 175         uint32_t stepOver = (time - timeLastPacket) / timings->packet;
 176
 177         timeLastPacket = (stepOver > 1) ? (time) : (timeLastPacket + timeout);
 178
 179         A7105Strobe(A7105_STANDBY);
 180         A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(stepOver % FLYSKY_FREQUENCY_COUNT));
 181         A7105Strobe(A7105_RX);
 182
 183         if(countTimeout > 31) {
 184             timeout = timings->syncPacket;
 185             setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
 186         } else {
 187             timeout = timings->packet;
 188             countTimeout++;
 189         }
 190     }
 191 }
 192
 193 static void checkRSSI(void)
 194 {
 195     static uint8_t buf[FLYSKY_RSSI_SAMPLE_COUNT] = {0};
 196     static int16_t sum = 0;
 197     static uint16_t currentIndex = 0;
 198
 199     uint8_t adcValue = A7105ReadReg(A7105_1D_RSSI_THOLD);
 200
 201     sum += adcValue;
 202     sum -= buf[currentIndex];
 203     buf[currentIndex] = adcValue;
 204     currentIndex = (currentIndex + 1) % FLYSKY_RSSI_SAMPLE_COUNT;
 205
 206     rssi_dBm = 50 + sum / (3 * FLYSKY_RSSI_SAMPLE_COUNT); // range about [95...52], -dBm
 207
 208     int16_t tmp = 2280 - 24 * rssi_dBm; // convert to [0...1023]
 209     setRssiDirect(tmp, RSSI_SOURCE_RX_PROTOCOL);
 210 }
 211
 212 static bool isValidPacket(const uint8_t *packet) {
 213     const flySky2ARcDataPkt_t *rcPacket = (const flySky2ARcDataPkt_t*) packet;
 214     return (rcPacket->rxId == rxId && rcPacket->txId == txId);
 215 }
 216
 217 static void buildAndWriteTelemetry(uint8_t *packet)
 218 {
 219     if (packet) {
 220         static uint8_t bytesToWrite = FLYSKY_2A_PAYLOAD_SIZE; // first time write full packet to buffer a7105
 221         flySky2ATelemetryPkt_t *telemertyPacket = (flySky2ATelemetryPkt_t*) packet;
 222         uint16_t voltage = getBatteryVoltage();
 223
 224         telemertyPacket->type = FLYSKY_2A_PACKET_TELEMETRY;
 225
 226         telemertyPacket->sens[0].type = SENSOR_INT_V;
 227         telemertyPacket->sens[0].number = 0;
 228         telemertyPacket->sens[0].valueL = voltage & 0xFF;
 229         telemertyPacket->sens[0].valueH = (voltage >> 8) & 0xFF;
 230
 231         telemertyPacket->sens[1].type = SENSOR_RSSI;
 232         telemertyPacket->sens[1].number = 0;
 233         telemertyPacket->sens[1].valueL = rssi_dBm & 0xFF;
 234         telemertyPacket->sens[1].valueH = (rssi_dBm >> 8) & 0xFF;
 235
 236         telemertyPacket->sens[2].type = SENSOR_ERR_RATE;
 237         telemertyPacket->sens[2].number = 0;
 238         telemertyPacket->sens[2].valueL = errorRate & 0xFF;
 239         telemertyPacket->sens[2].valueH = (errorRate >> 8) & 0xFF;
 240
 241         memset (&telemertyPacket->sens[3], 0xFF, 4 * sizeof(flySky2ASens_t));
 242
 243         A7105WriteFIFO(packet, bytesToWrite);
 244
 245         bytesToWrite = 9 + 3 * sizeof(flySky2ASens_t);// next time write only bytes that could change, the others are already set as 0xFF in buffer a7105
 246     }
 247 }
 248
 249 static rx_spi_received_e flySky2AReadAndProcess(uint8_t *payload, const uint32_t timeStamp)
 250 {
 251     rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
 252     uint8_t packet[FLYSKY_2A_PAYLOAD_SIZE];
 253
 254     uint8_t bytesToRead = (bound) ? (9 + 2*FLYSKY_2A_CHANNEL_COUNT) : (11 + FLYSKY_FREQUENCY_COUNT);
 255     A7105ReadFIFO(packet, bytesToRead);
 256
 257     switch (packet[0]) {
 258     case FLYSKY_2A_PACKET_RC_DATA:
 259     case FLYSKY_2A_PACKET_FS_SETTINGS: // failsafe settings
 260     case FLYSKY_2A_PACKET_SETTINGS: // receiver settings
 261         if (isValidPacket(packet)) {
 262             checkRSSI();
 263             resetTimeout(timeStamp);
 264
 265             const flySky2ARcDataPkt_t *rcPacket = (const flySky2ARcDataPkt_t*) packet;
 266
 267             if (rcPacket->type == FLYSKY_2A_PACKET_RC_DATA) {
 268                 if (payload) {
 269                     memcpy(payload, rcPacket->data, 2*FLYSKY_2A_CHANNEL_COUNT);
 270                 }
 271
 272                 if (sendTelemetry) {
 273                     buildAndWriteTelemetry(packet);
 274                     sendTelemetry = false;
 275                     timeTxRequest = timeStamp;
 276                     waitTx = true;
 277                 }
 278
 279                 result = RX_SPI_RECEIVED_DATA;
 280             }
 281
 282             if (!waitTx) {
 283                 A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(1));
 284             }
 285         }
 286         break;
 287
 288     case FLYSKY_2A_PACKET_BIND1:
 289     case FLYSKY_2A_PACKET_BIND2:
 290         if (!bound) {
 291             resetTimeout(timeStamp);
 292
 293             flySky2ABindPkt_t *bindPacket = (flySky2ABindPkt_t*) packet;
 294
 295             if (bindPacket->rfChannelMap[0] != 0xFF) {
 296                 memcpy(rfChannelMap, bindPacket->rfChannelMap, FLYSKY_FREQUENCY_COUNT); // get TX channels
 297             }
 298
 299             txId = bindPacket->txId;
 300             bindPacket->rxId = rxId;
 301             memset(bindPacket->rfChannelMap, 0xFF, 26); // erase channelMap and 10 bytes after it
 302
 303             timeTxRequest = timeLastBind = timeStamp;
 304             waitTx = true;
 305
 306             A7105WriteFIFO(packet, FLYSKY_2A_PAYLOAD_SIZE);
 307         }
 308         break;
 309
 310     default:
 311         break;
 312     }
 313
 314     if (!waitTx) {
 315         A7105Strobe(A7105_RX);
 316     }
 317     return result;
 318 }
 319
 320 static rx_spi_received_e flySkyReadAndProcess(uint8_t *payload, const uint32_t timeStamp)
 321 {
 322     rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
 323     uint8_t packet[FLYSKY_PAYLOAD_SIZE];
 324
 325     uint8_t bytesToRead = (bound) ? (5 + 2*FLYSKY_CHANNEL_COUNT) : (5);
 326     A7105ReadFIFO(packet, bytesToRead);
 327
 328     const flySkyRcDataPkt_t *rcPacket = (const flySkyRcDataPkt_t*) packet;
 329
 330     if (bound && rcPacket->type == FLYSKY_PACKET_RC_DATA && rcPacket->txId == txId) {
 331         checkRSSI();
 332         resetTimeout(timeStamp);
 333
 334         if (payload) {
 335             memcpy(payload, rcPacket->data, 2*FLYSKY_CHANNEL_COUNT);
 336         }
 337
 338         A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(1));
 339
 340         result = RX_SPI_RECEIVED_DATA;
 341     }
 342
 343     if (!bound && rcPacket->type == FLYSKY_PACKET_BIND) {
 344         resetTimeout(timeStamp);
 345
 346         txId = rcPacket->txId;
 347         flySkyCalculateRfChannels();
 348
 349         A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(0));
 350
 351         timeLastBind = timeStamp;
 352     }
 353
 354     A7105Strobe(A7105_RX);
 355     return result;
 356 }
 357
 358 bool flySkyInit(const rxSpiConfig_t *rxSpiConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
 359 {
 360     if (!rxSpiExtiConfigured()) {
 361         return false;
 362     }
 363
 364     protocol = rxSpiConfig->rx_spi_protocol;
 365
 366     rxSpiCommonIOInit(rxSpiConfig);
 367
 368     extiConfig->ioConfig = IOCFG_IPD;
 369     extiConfig->trigger = BETAFLIGHT_EXTI_TRIGGER_RISING;
 370
 371     uint8_t startRxChannel;
 372
 373     if (protocol == RX_SPI_A7105_FLYSKY_2A) {
 374         rxRuntimeState->channelCount = FLYSKY_2A_CHANNEL_COUNT;
 375         timings = &flySky2ATimings;
 376         rxId = U_ID_0 ^ U_ID_1 ^ U_ID_2;
 377         startRxChannel = flySky2ABindChannels[0];
 378         A7105Init(0x5475c52A, IO_NONE);
 379         A7105Config(flySky2ARegs, sizeof(flySky2ARegs));
 380     } else {
 381         rxRuntimeState->channelCount = FLYSKY_CHANNEL_COUNT;
 382         timings = &flySkyTimings;
 383         startRxChannel = 0;
 384         A7105Init(0x5475c52A, IO_NONE);
 385         A7105Config(flySkyRegs, sizeof(flySkyRegs));
 386     }
 387
 388     if (flySkyConfig()->txId == 0) {
 389         bound = false;
 390     } else {
 391         bound = true;
 392         txId = flySkyConfig()->txId; // load TXID
 393         memcpy (rfChannelMap, flySkyConfig()->rfChannelMap, FLYSKY_FREQUENCY_COUNT);// load channel map
 394         startRxChannel = getNextChannel(0);
 395     }
 396
 397     if (rssiSource == RSSI_SOURCE_NONE) {
 398         rssiSource = RSSI_SOURCE_RX_PROTOCOL;
 399     }
 400
 401     A7105WriteReg(A7105_0F_CHANNEL, startRxChannel);
 402     A7105Strobe(A7105_RX); // start listening
 403
 404     resetTimeout(micros());
 405
 406     return true;
 407 }
 408
 409 void flySkySetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
 410 {
 411     if (rcData && payload) {
 412         uint32_t channelCount;
 413         channelCount = (protocol == RX_SPI_A7105_FLYSKY_2A) ? (FLYSKY_2A_CHANNEL_COUNT) : (FLYSKY_CHANNEL_COUNT);
 414
 415         for (unsigned i = 0; i < channelCount; i++) {
 416             rcData[i] = payload[2 * i + 1] << 8 | payload [2 * i + 0];
 417         }
 418     }
 419 }
 420
 421 rx_spi_received_e flySkyDataReceived(uint8_t *payload)
 422 {
 423     rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
 424     uint32_t timeStamp;
 425
 426     if (A7105RxTxFinished(&timeStamp)) {
 427         uint8_t modeReg = A7105ReadReg(A7105_00_MODE);
 428
 429         if (((modeReg & A7105_MODE_TRSR) != 0) && ((modeReg & A7105_MODE_TRER) == 0)) { // TX complete
 430             if (bound) {
 431                 A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(1));
 432             }
 433             A7105Strobe(A7105_RX);
 434         } else if ((modeReg & (A7105_MODE_CRCF|A7105_MODE_TRER)) == 0) { // RX complete, CRC pass
 435             if (protocol == RX_SPI_A7105_FLYSKY_2A) {
 436                 result = flySky2AReadAndProcess(payload, timeStamp);
 437             } else {
 438                 result = flySkyReadAndProcess(payload, timeStamp);
 439             }
 440         } else {
 441             A7105Strobe(A7105_RX);
 442         }
 443     }
 444
 445     if (waitTx && (micros() - timeTxRequest) > TX_DELAY) {
 446         A7105Strobe(A7105_TX);
 447         waitTx = false;
 448     }
 449
 450     if (rxSpiCheckBindRequested(true)) {
 451         bound = false;
 452         txId = 0;
 453         memset(rfChannelMap, 0, FLYSKY_FREQUENCY_COUNT);
 454         uint8_t bindChannel = (protocol == RX_SPI_A7105_FLYSKY_2A) ? flySky2ABindChannels[0] : 0;
 455         A7105WriteReg(A7105_0F_CHANNEL, bindChannel);
 456     }
 457
 458     if (bound) {
 459         checkTimeout();
 460         rxSpiLedBlinkRxLoss(result);
 461     } else {
 462         if ((micros() - timeLastBind) > BIND_TIMEOUT && rfChannelMap[0] != 0 && txId != 0) {
 463             result = RX_SPI_RECEIVED_BIND;
 464             bound = true;
 465             flySkyConfigMutable()->txId = txId; // store TXID
 466             memcpy (flySkyConfigMutable()->rfChannelMap, rfChannelMap, FLYSKY_FREQUENCY_COUNT);// store channel map
 467             writeEEPROM();
 468         }
 469         rxSpiLedBlinkBind();
 470     }
 471
 472     return result;
 473 }
 474
 475 #endif /* USE_RX_FLYSKY */