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 {
 214     const flySky2ARcDataPkt_t *rcPacket = (const flySky2ARcDataPkt_t*) packet;
 215     return (rcPacket->rxId == rxId && rcPacket->txId == txId);
 216 }
 217
 218 static void buildAndWriteTelemetry(uint8_t *packet)
 219 {
 220     if (packet) {
 221         static uint8_t bytesToWrite = FLYSKY_2A_PAYLOAD_SIZE; // first time write full packet to buffer a7105
 222         flySky2ATelemetryPkt_t *telemertyPacket = (flySky2ATelemetryPkt_t*) packet;
 223         uint16_t voltage = getBatteryVoltage();
 224
 225         telemertyPacket->type = FLYSKY_2A_PACKET_TELEMETRY;
 226
 227         telemertyPacket->sens[0].type = SENSOR_INT_V;
 228         telemertyPacket->sens[0].number = 0;
 229         telemertyPacket->sens[0].valueL = voltage & 0xFF;
 230         telemertyPacket->sens[0].valueH = (voltage >> 8) & 0xFF;
 231
 232         telemertyPacket->sens[1].type = SENSOR_RSSI;
 233         telemertyPacket->sens[1].number = 0;
 234         telemertyPacket->sens[1].valueL = rssi_dBm & 0xFF;
 235         telemertyPacket->sens[1].valueH = (rssi_dBm >> 8) & 0xFF;
 236
 237         telemertyPacket->sens[2].type = SENSOR_ERR_RATE;
 238         telemertyPacket->sens[2].number = 0;
 239         telemertyPacket->sens[2].valueL = errorRate & 0xFF;
 240         telemertyPacket->sens[2].valueH = (errorRate >> 8) & 0xFF;
 241
 242         memset (&telemertyPacket->sens[3], 0xFF, 4 * sizeof(flySky2ASens_t));
 243
 244         A7105WriteFIFO(packet, bytesToWrite);
 245
 246         bytesToWrite = 9 + 3 * sizeof(flySky2ASens_t);// next time write only bytes that could change, the others are already set as 0xFF in buffer a7105
 247     }
 248 }
 249
 250 static rx_spi_received_e flySky2AReadAndProcess(uint8_t *payload, const uint32_t timeStamp)
 251 {
 252     rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
 253     uint8_t packet[FLYSKY_2A_PAYLOAD_SIZE];
 254
 255     uint8_t bytesToRead = (bound) ? (9 + 2*FLYSKY_2A_CHANNEL_COUNT) : (11 + FLYSKY_FREQUENCY_COUNT);
 256     A7105ReadFIFO(packet, bytesToRead);
 257
 258     switch (packet[0]) {
 259     case FLYSKY_2A_PACKET_RC_DATA:
 260     case FLYSKY_2A_PACKET_FS_SETTINGS: // failsafe settings
 261     case FLYSKY_2A_PACKET_SETTINGS: // receiver settings
 262         if (isValidPacket(packet)) {
 263             checkRSSI();
 264             resetTimeout(timeStamp);
 265
 266             const flySky2ARcDataPkt_t *rcPacket = (const flySky2ARcDataPkt_t*) packet;
 267
 268             if (rcPacket->type == FLYSKY_2A_PACKET_RC_DATA) {
 269                 if (payload) {
 270                     memcpy(payload, rcPacket->data, 2*FLYSKY_2A_CHANNEL_COUNT);
 271                 }
 272
 273                 if (sendTelemetry) {
 274                     buildAndWriteTelemetry(packet);
 275                     sendTelemetry = false;
 276                     timeTxRequest = timeStamp;
 277                     waitTx = true;
 278                 }
 279
 280                 result = RX_SPI_RECEIVED_DATA;
 281             }
 282
 283             if (!waitTx) {
 284                 A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(1));
 285             }
 286         }
 287         break;
 288
 289     case FLYSKY_2A_PACKET_BIND1:
 290     case FLYSKY_2A_PACKET_BIND2:
 291         if (!bound) {
 292             resetTimeout(timeStamp);
 293
 294             flySky2ABindPkt_t *bindPacket = (flySky2ABindPkt_t*) packet;
 295
 296             if (bindPacket->rfChannelMap[0] != 0xFF) {
 297                 memcpy(rfChannelMap, bindPacket->rfChannelMap, FLYSKY_FREQUENCY_COUNT); // get TX channels
 298             }
 299
 300             txId = bindPacket->txId;
 301             bindPacket->rxId = rxId;
 302             memset(bindPacket->rfChannelMap, 0xFF, 26); // erase channelMap and 10 bytes after it
 303
 304             timeTxRequest = timeLastBind = timeStamp;
 305             waitTx = true;
 306
 307             A7105WriteFIFO(packet, FLYSKY_2A_PAYLOAD_SIZE);
 308         }
 309         break;
 310
 311     default:
 312         break;
 313     }
 314
 315     if (!waitTx) {
 316         A7105Strobe(A7105_RX);
 317     }
 318     return result;
 319 }
 320
 321 static rx_spi_received_e flySkyReadAndProcess(uint8_t *payload, const uint32_t timeStamp)
 322 {
 323     rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
 324     uint8_t packet[FLYSKY_PAYLOAD_SIZE];
 325
 326     uint8_t bytesToRead = (bound) ? (5 + 2*FLYSKY_CHANNEL_COUNT) : (5);
 327     A7105ReadFIFO(packet, bytesToRead);
 328
 329     const flySkyRcDataPkt_t *rcPacket = (const flySkyRcDataPkt_t*) packet;
 330
 331     if (bound && rcPacket->type == FLYSKY_PACKET_RC_DATA && rcPacket->txId == txId) {
 332         checkRSSI();
 333         resetTimeout(timeStamp);
 334
 335         if (payload) {
 336             memcpy(payload, rcPacket->data, 2*FLYSKY_CHANNEL_COUNT);
 337         }
 338
 339         A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(1));
 340
 341         result = RX_SPI_RECEIVED_DATA;
 342     }
 343
 344     if (!bound && rcPacket->type == FLYSKY_PACKET_BIND) {
 345         resetTimeout(timeStamp);
 346
 347         txId = rcPacket->txId;
 348         flySkyCalculateRfChannels();
 349
 350         A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(0));
 351
 352         timeLastBind = timeStamp;
 353     }
 354
 355     A7105Strobe(A7105_RX);
 356     return result;
 357 }
 358
 359 bool flySkyInit(const rxSpiConfig_t *rxSpiConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
 360 {
 361     if (!rxSpiExtiConfigured()) {
 362         return false;
 363     }
 364
 365     protocol = rxSpiConfig->rx_spi_protocol;
 366
 367     rxSpiCommonIOInit(rxSpiConfig);
 368
 369     extiConfig->ioConfig = IOCFG_IPD;
 370     extiConfig->trigger = BETAFLIGHT_EXTI_TRIGGER_RISING;
 371
 372     uint8_t startRxChannel;
 373
 374     if (protocol == RX_SPI_A7105_FLYSKY_2A) {
 375         rxRuntimeState->channelCount = FLYSKY_2A_CHANNEL_COUNT;
 376         timings = &flySky2ATimings;
 377         rxId = U_ID_0 ^ U_ID_1 ^ U_ID_2;
 378         startRxChannel = flySky2ABindChannels[0];
 379         A7105Init(0x5475c52A, IO_NONE);
 380         A7105Config(flySky2ARegs, sizeof(flySky2ARegs));
 381     } else {
 382         rxRuntimeState->channelCount = FLYSKY_CHANNEL_COUNT;
 383         timings = &flySkyTimings;
 384         startRxChannel = 0;
 385         A7105Init(0x5475c52A, IO_NONE);
 386         A7105Config(flySkyRegs, sizeof(flySkyRegs));
 387     }
 388
 389     if (flySkyConfig()->txId == 0) {
 390         bound = false;
 391     } else {
 392         bound = true;
 393         txId = flySkyConfig()->txId; // load TXID
 394         memcpy (rfChannelMap, flySkyConfig()->rfChannelMap, FLYSKY_FREQUENCY_COUNT);// load channel map
 395         startRxChannel = getNextChannel(0);
 396     }
 397
 398     if (rssiSource == RSSI_SOURCE_NONE) {
 399         rssiSource = RSSI_SOURCE_RX_PROTOCOL;
 400     }
 401
 402     A7105WriteReg(A7105_0F_CHANNEL, startRxChannel);
 403     A7105Strobe(A7105_RX); // start listening
 404
 405     resetTimeout(micros());
 406
 407     return true;
 408 }
 409
 410 void flySkySetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
 411 {
 412     if (rcData && payload) {
 413         uint32_t channelCount;
 414         channelCount = (protocol == RX_SPI_A7105_FLYSKY_2A) ? (FLYSKY_2A_CHANNEL_COUNT) : (FLYSKY_CHANNEL_COUNT);
 415
 416         for (unsigned i = 0; i < channelCount; i++) {
 417             rcData[i] = payload[2 * i + 1] << 8 | payload [2 * i + 0];
 418         }
 419     }
 420 }
 421
 422 rx_spi_received_e flySkyDataReceived(uint8_t *payload)
 423 {
 424     rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
 425     uint32_t timeStamp;
 426
 427     if (A7105RxTxFinished(&timeStamp)) {
 428         uint8_t modeReg = A7105ReadReg(A7105_00_MODE);
 429
 430         if (((modeReg & A7105_MODE_TRSR) != 0) && ((modeReg & A7105_MODE_TRER) == 0)) { // TX complete
 431             if (bound) {
 432                 A7105WriteReg(A7105_0F_CHANNEL, getNextChannel(1));
 433             }
 434             A7105Strobe(A7105_RX);
 435         } else if ((modeReg & (A7105_MODE_CRCF|A7105_MODE_TRER)) == 0) { // RX complete, CRC pass
 436             if (protocol == RX_SPI_A7105_FLYSKY_2A) {
 437                 result = flySky2AReadAndProcess(payload, timeStamp);
 438             } else {
 439                 result = flySkyReadAndProcess(payload, timeStamp);
 440             }
 441         } else {
 442             A7105Strobe(A7105_RX);
 443         }
 444     }
 445
 446     if (waitTx && (micros() - timeTxRequest) > TX_DELAY) {
 447         A7105Strobe(A7105_TX);
 448         waitTx = false;
 449     }
 450
 451     if (rxSpiCheckBindRequested(true)) {
 452         bound = false;
 453         txId = 0;
 454         memset(rfChannelMap, 0, FLYSKY_FREQUENCY_COUNT);
 455         uint8_t bindChannel = (protocol == RX_SPI_A7105_FLYSKY_2A) ? flySky2ABindChannels[0] : 0;
 456         A7105WriteReg(A7105_0F_CHANNEL, bindChannel);
 457     }
 458
 459     if (bound) {
 460         checkTimeout();
 461         rxSpiLedBlinkRxLoss(result);
 462     } else {
 463         if ((micros() - timeLastBind) > BIND_TIMEOUT && rfChannelMap[0] != 0 && txId != 0) {
 464             result = RX_SPI_RECEIVED_BIND;
 465             bound = true;
 466             flySkyConfigMutable()->txId = txId; // store TXID
 467             memcpy (flySkyConfigMutable()->rfChannelMap, rfChannelMap, FLYSKY_FREQUENCY_COUNT);// store channel map
 468             writeEEPROM();
 469         }
 470         rxSpiLedBlinkBind();
 471     }
 472
 473     return result;
 474 }
 475
 476 #endif /* USE_RX_FLYSKY */