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