src/main/rx/expresslrs_common.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 /*
  22  * Based on https://github.com/ExpressLRS/ExpressLRS
  23  * Thanks to AlessandroAU, original creator of the ExpressLRS project.
  24  */
  25
  26 #include <string.h>
  27 #include "platform.h"
  28
  29 #ifdef USE_RX_EXPRESSLRS
  30
  31 #include "build/build_config.h"
  32 #include "common/utils.h"
  33 #include "common/maths.h"
  34 #include "rx/crsf.h"
  35 #include "rx/expresslrs_common.h"
  36 #include "drivers/rx/rx_sx127x.h"
  37 #include "drivers/rx/rx_sx1280.h"
  38
  39 STATIC_UNIT_TESTED uint16_t crc14tab[ELRS_CRC_LEN] = {0};
  40
  41 static uint8_t volatile fhssIndex = 0;
  42 STATIC_UNIT_TESTED uint8_t fhssSequence[ELRS_NR_SEQUENCE_ENTRIES] = {0};
  43 static uint16_t seqCount = 0;
  44 static uint8_t syncChannel = 0;
  45 static uint32_t freqSpread = 0;
  46
  47 #define MS_TO_US(ms) (ms * 1000)
  48
  49 // Regarding failsafe timeout values:
  50 // @CapnBry - Higher rates shorter timeout. Usually it runs 1-1.5 seconds with complete sync 500Hz.
  51 //            250Hz is 2-5s. 150Hz 2.5s. 50Hz stays in sync all 5 seconds of my test.
  52 // The failsafe timeout values come from the ELRS project's ExpressLRS_AirRateConfig definitions.
  53 elrsModSettings_t airRateConfig[][ELRS_RATE_MAX] = {
  54 #ifdef USE_RX_SX127X
  55     {
  56         {0, RATE_LORA_200HZ, SX127x_BW_500_00_KHZ, SX127x_SF_6, SX127x_CR_4_7, 5000, TLM_RATIO_1_64, 4, 8},
  57         {1, RATE_LORA_100HZ, SX127x_BW_500_00_KHZ, SX127x_SF_7, SX127x_CR_4_7, 10000, TLM_RATIO_1_64, 4, 8},
  58         {2, RATE_LORA_50HZ, SX127x_BW_500_00_KHZ, SX127x_SF_8, SX127x_CR_4_7, 20000, TLM_RATIO_1_16, 4, 10},
  59         {3, RATE_LORA_25HZ, SX127x_BW_500_00_KHZ, SX127x_SF_9, SX127x_CR_4_7, 40000, TLM_RATIO_1_8, 2, 10}
  60     },
  61 #endif
  62 #ifdef USE_RX_SX1280
  63     {
  64         {0, RATE_LORA_500HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF5, SX1280_LORA_CR_LI_4_6, 2000, TLM_RATIO_1_128, 4, 12},
  65         {1, RATE_LORA_250HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF6, SX1280_LORA_CR_LI_4_7, 4000, TLM_RATIO_1_64, 4, 14},
  66         {2, RATE_LORA_150HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF7, SX1280_LORA_CR_LI_4_7, 6666, TLM_RATIO_1_32, 4, 12},
  67         {3, RATE_LORA_50HZ, SX1280_LORA_BW_0800, SX1280_LORA_SF8, SX1280_LORA_CR_LI_4_7, 20000, TLM_RATIO_1_16, 2, 12}
  68     },
  69 #endif
  70 #if !defined(USE_RX_SX127X) && !defined(USE_RX_SX1280)
  71     {{0}},
  72 #endif
  73 };
  74
  75 elrsRfPerfParams_t rfPerfConfig[][ELRS_RATE_MAX] = {
  76 #ifdef USE_RX_SX127X
  77     {
  78         {0, RATE_LORA_200HZ, -112, 4380, 3000, 2500, 600, 5000},
  79         {1, RATE_LORA_100HZ, -117, 8770, 3500, 2500, 600, 5000},
  80         {2, RATE_LORA_50HZ, -120, 17540, 4000, 2500, 600, 5000},
  81         {3, RATE_LORA_25HZ, -123, 17540, 6000, 4000, 0, 5000}
  82     },
  83 #endif
  84 #ifdef USE_RX_SX1280
  85     {
  86         {0, RATE_LORA_500HZ, -105, 1665, 2500, 2500, 3, 5000},
  87         {1, RATE_LORA_250HZ, -108, 3300, 3000, 2500, 6, 5000},
  88         {2, RATE_LORA_150HZ, -112, 5871, 3500, 2500, 10, 5000},
  89         {3, RATE_LORA_50HZ, -115, 10798, 4000, 2500, 0, 5000}
  90     },
  91 #endif
  92 #if !defined(USE_RX_SX127X) && !defined(USE_RX_SX1280)
  93     {{0}},
  94 #endif
  95 };
  96
  97 const elrsFhssConfig_t fhssConfigs[] = {
  98 #ifdef USE_RX_SX127X
  99     {AU433,  FREQ_HZ_TO_REG_VAL_900(433420000), FREQ_HZ_TO_REG_VAL_900(434420000), 3},
 100     {AU915,  FREQ_HZ_TO_REG_VAL_900(915500000), FREQ_HZ_TO_REG_VAL_900(926900000), 20},
 101     {EU433,  FREQ_HZ_TO_REG_VAL_900(433100000), FREQ_HZ_TO_REG_VAL_900(434450000), 3},
 102     {EU868,  FREQ_HZ_TO_REG_VAL_900(865275000), FREQ_HZ_TO_REG_VAL_900(869575000), 13},
 103     {IN866,  FREQ_HZ_TO_REG_VAL_900(865375000), FREQ_HZ_TO_REG_VAL_900(866950000), 4},
 104     {FCC915, FREQ_HZ_TO_REG_VAL_900(903500000), FREQ_HZ_TO_REG_VAL_900(926900000), 40},
 105 #endif
 106 #ifdef USE_RX_SX1280
 107     {ISM2400, FREQ_HZ_TO_REG_VAL_24(2400400000), FREQ_HZ_TO_REG_VAL_24(2479400000), 80},
 108     {CE2400,  FREQ_HZ_TO_REG_VAL_24(2400400000), FREQ_HZ_TO_REG_VAL_24(2479400000), 80},
 109 #endif
 110 #if !defined(USE_RX_SX127X) && !defined(USE_RX_SX1280)
 111     {0},
 112 #endif
 113 };
 114
 115 const elrsFhssConfig_t *fhssConfig;
 116
 117 void generateCrc14Table(void)
 118 {
 119     uint16_t crc;
 120     for (uint16_t i = 0; i < ELRS_CRC_LEN; i++) {
 121         crc = i << (14 - 8);
 122         for (uint8_t j = 0; j < 8; j++) {
 123             crc = (crc << 1) ^ ((crc & 0x2000) ? ELRS_CRC14_POLY : 0);
 124         }
 125         crc14tab[i] = crc;
 126     }
 127 }
 128
 129 uint16_t calcCrc14(uint8_t *data, uint8_t len, uint16_t crc)
 130 {
 131     while (len--) {
 132         crc = (crc << 8) ^ crc14tab[((crc >> 6) ^ (uint16_t) *data++) & 0x00FF];
 133     }
 134     return crc & 0x3FFF;
 135 }
 136
 137 uint32_t fhssGetInitialFreq(const int32_t freqCorrection)
 138 {
 139     return fhssConfig->freqStart + (syncChannel * freqSpread / ELRS_FREQ_SPREAD_SCALE) - freqCorrection;
 140 }
 141
 142 uint8_t fhssGetNumEntries(void)
 143 {
 144     return fhssConfig->freqCount;
 145 }
 146
 147 uint8_t fhssGetCurrIndex(void)
 148 {
 149     return fhssIndex;
 150 }
 151
 152 void fhssSetCurrIndex(const uint8_t value)
 153 {
 154     fhssIndex = value % seqCount;
 155 }
 156
 157 uint32_t fhssGetNextFreq(const int32_t freqCorrection)
 158 {
 159     fhssIndex = (fhssIndex + 1) % seqCount;
 160     uint32_t freq = fhssConfig->freqStart + (freqSpread * fhssSequence[fhssIndex] / ELRS_FREQ_SPREAD_SCALE) - freqCorrection;
 161     return freq;
 162 }
 163
 164 static uint32_t seed = 0;
 165
 166 // returns 0 <= x < max where max < 256
 167 static uint8_t rngN(const uint8_t max)
 168 {
 169     const uint32_t m = 2147483648;
 170     const uint32_t a = 214013;
 171     const uint32_t c = 2531011;
 172     seed = (a * seed + c) % m;
 173     return (seed >> 16) % max;
 174 }
 175
 176 /**
 177 Requirements:
 178 1. 0 every n hops
 179 2. No two repeated channels
 180 3. Equal occurance of each (or as even as possible) of each channel
 181 4. Pseudorandom
 182
 183 Approach:
 184   Fill the sequence array with the sync channel every FHSS_FREQ_CNT
 185   Iterate through the array, and for each block, swap each entry in it with
 186   another random entry, excluding the sync channel.
 187
 188 */
 189 void fhssGenSequence(const uint8_t UID[], const elrsFreqDomain_e dom)
 190 {
 191     seed = (((long)UID[2] << 24) + ((long)UID[3] << 16) + ((long)UID[4] << 8) + UID[5]) ^ ELRS_OTA_VERSION_ID;
 192     fhssConfig = &fhssConfigs[dom];
 193     seqCount = (256 / MAX(fhssConfig->freqCount, 1)) * fhssConfig->freqCount;
 194     syncChannel = (fhssConfig->freqCount / 2) + 1;
 195     freqSpread = (fhssConfig->freqStop - fhssConfig->freqStart) * ELRS_FREQ_SPREAD_SCALE / MAX((fhssConfig->freqCount - 1), 1);
 196
 197     // initialize the sequence array
 198     for (uint16_t i = 0; i < seqCount; i++) {
 199         if (i % fhssConfig->freqCount == 0) {
 200             fhssSequence[i] = syncChannel;
 201         } else if (i % fhssConfig->freqCount == syncChannel) {
 202             fhssSequence[i] = 0;
 203         } else {
 204             fhssSequence[i] = i % fhssConfig->freqCount;
 205         }
 206     }
 207
 208     for (uint16_t i = 0; i < seqCount; i++) {
 209         // if it's not the sync channel
 210         if (i % fhssConfig->freqCount != 0) {
 211             uint8_t offset = (i / fhssConfig->freqCount) * fhssConfig->freqCount; // offset to start of current block
 212             uint8_t rand = rngN(fhssConfig->freqCount - 1) + 1; // random number between 1 and numFreqs
 213
 214             // switch this entry and another random entry in the same block
 215             uint8_t temp = fhssSequence[i];
 216             fhssSequence[i] = fhssSequence[offset + rand];
 217             fhssSequence[offset + rand] = temp;
 218         }
 219     }
 220 }
 221
 222 uint8_t tlmRatioEnumToValue(const elrsTlmRatio_e enumval)
 223 {
 224     // !! TLM_RATIO_STD/TLM_RATIO_DISARMED should be converted by the caller !!
 225     if (enumval == TLM_RATIO_NO_TLM) {
 226         return 1;
 227     }
 228
 229     // 1 << (8 - (enumval - TLM_RATIO_NO_TLM))
 230     // 1_128 = 128, 1_64 = 64, 1_32 = 32, etc
 231     return 1 << (8 + TLM_RATIO_NO_TLM - enumval);
 232 }
 233
 234 uint16_t rateEnumToHz(const elrsRfRate_e eRate)
 235 {
 236     switch (eRate) {
 237     case RATE_LORA_500HZ: return 500;
 238     case RATE_LORA_250HZ: return 250;
 239     case RATE_LORA_200HZ: return 200;
 240     case RATE_LORA_150HZ: return 150;
 241     case RATE_LORA_100HZ: return 100;
 242     case RATE_LORA_50HZ: return 50;
 243     case RATE_LORA_25HZ: return 25;
 244     case RATE_LORA_4HZ: return 4;
 245     default: return 1;
 246     }
 247 }
 248
 249 uint16_t txPowerIndexToValue(const uint8_t index)
 250 {
 251     switch (index) {
 252     case 0: return 0;
 253     case 1: return 10;
 254     case 2: return 25;
 255     case 3: return 100;
 256     case 4: return 500;
 257     case 5: return 1000;
 258     case 6: return 2000;
 259     case 7: return 250;
 260     case 8: return 50;
 261     default: return 0;
 262     }
 263 }
 264
 265 #define ELRS_LQ_DEPTH 4 //100 % 32
 266
 267 typedef struct linkQuality_s {
 268     uint32_t array[ELRS_LQ_DEPTH];
 269     uint8_t value;
 270     uint8_t index;
 271     uint32_t mask;
 272 } linkQuality_t;
 273
 274 static linkQuality_t lq;
 275
 276 void lqIncrease(void)
 277 {
 278     if (!lqPeriodIsSet()) {
 279         lq.array[lq.index] |= lq.mask;
 280         lq.value += 1;
 281     }
 282 }
 283
 284 void lqNewPeriod(void)
 285 {
 286     lq.mask <<= 1;
 287     if (lq.mask == 0) {
 288         lq.mask = (1 << 0);
 289         lq.index += 1;
 290     }
 291
 292     // At idx N / 32 and bit N % 32, wrap back to idx=0, bit=0
 293     if ((lq.index == 3) && (lq.mask & (1 << ELRS_LQ_DEPTH))) {
 294         lq.index = 0;
 295         lq.mask = (1 << 0);
 296     }
 297
 298     if ((lq.array[lq.index] & lq.mask) != 0) {
 299         lq.array[lq.index] &= ~lq.mask;
 300         lq.value -= 1;
 301     }
 302 }
 303
 304 uint8_t lqGet(void)
 305 {
 306     return lq.value;
 307 }
 308
 309 bool lqPeriodIsSet(void)
 310 {
 311     return lq.array[lq.index] & lq.mask;
 312 }
 313
 314 void lqReset(void)
 315 {
 316     memset(&lq, 0, sizeof(lq));
 317     lq.mask = (1 << 0);
 318 }
 319
 320 uint16_t convertSwitch1b(const uint16_t val)
 321 {
 322     return val ? 2000 : 1000;
 323 }
 324
 325 // 3b to decode 7 pos switches
 326 uint16_t convertSwitch3b(const uint16_t val)
 327 {
 328     switch (val) {
 329     case 0: return 1000;
 330     case 1: return 1275;
 331     case 2: return 1425;
 332     case 3: return 1575;
 333     case 4: return 1725;
 334     case 5: return 2000;
 335     default: return 1500;
 336     }
 337 }
 338
 339 uint16_t convertSwitchNb(const uint16_t val, const uint16_t max)
 340 {
 341     return (val > max) ? 1500 : val * 1000 / max + 1000;
 342 }
 343
 344 uint8_t hybridWideNonceToSwitchIndex(const uint8_t nonce)
 345 {
 346     // Returns the sequence (0 to 7, then 0 to 7 rotated left by 1):
 347     // 0, 1, 2, 3, 4, 5, 6, 7,
 348     // 1, 2, 3, 4, 5, 6, 7, 0
 349     // Because telemetry can occur on every 2, 4, 8, 16, 32, 64, 128th packet
 350     // this makes sure each of the 8 values is sent at least once every 16 packets
 351     // regardless of the TLM ratio
 352     // Index 7 also can never fall on a telemetry slot
 353     return ((nonce & 0x07) + ((nonce >> 3) & 0x01)) % 8;
 354 }
 355
 356 uint8_t airRateIndexToIndex900(uint8_t airRate, uint8_t currentIndex)
 357 {
 358     switch (airRate) {
 359     case 0:
 360         return 0;
 361     case 1:
 362         return currentIndex;
 363     case 2:
 364         return 1;
 365     case 3:
 366         return 2;
 367     case 4:
 368         return 3;
 369     default:
 370         return currentIndex;
 371     }
 372 }
 373
 374 uint8_t airRateIndexToIndex24(uint8_t airRate, uint8_t currentIndex)
 375 {
 376     switch (airRate) {
 377     case 0:
 378         return currentIndex;
 379     case 1:
 380         return currentIndex;
 381     case 2:
 382         return currentIndex;
 383     case 3:
 384         return currentIndex;
 385     case 4:
 386         return 0;
 387     case 5:
 388         return currentIndex;
 389     case 6:
 390         return 1;
 391     case 7:
 392         return 2;
 393     case 8:
 394         return currentIndex;
 395     case 9:
 396         return 3;
 397     default:
 398         return currentIndex;
 399     }
 400 }
 401
 402 #endif /* USE_RX_EXPRESSLRS */