src/main/rx/rx.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 <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdlib.h>
  21
  22 #include <string.h>
  23
  24 #include "platform.h"
  25
  26 #include "build/build_config.h"
  27 #include "build/debug.h"
  28
  29 #include "common/maths.h"
  30 #include "common/utils.h"
  31
  32 #include "programming/logic_condition.h"
  33
  34 #include "config/feature.h"
  35 #include "config/parameter_group.h"
  36 #include "config/parameter_group_ids.h"
  37
  38
  39 #include "drivers/adc.h"
  40 #include "drivers/serial.h"
  41 #include "drivers/time.h"
  42
  43 #include "fc/config.h"
  44 #include "fc/rc_controls.h"
  45 #include "fc/rc_modes.h"
  46 #include "fc/settings.h"
  47
  48 #include "flight/failsafe.h"
  49
  50 #include "io/serial.h"
  51
  52 #include "rx/rx.h"
  53 #include "rx/crsf.h"
  54 #include "rx/ibus.h"
  55 #include "rx/jetiexbus.h"
  56 #include "rx/fport.h"
  57 #include "rx/fport2.h"
  58 #include "rx/msp.h"
  59 #include "rx/msp_override.h"
  60 #include "rx/sbus.h"
  61 #include "rx/spektrum.h"
  62 #include "rx/srxl2.h"
  63 #include "rx/sumd.h"
  64 #include "rx/ghst.h"
  65 #include "rx/mavlink.h"
  66
  67 const char rcChannelLetters[] = "AERT";
  68
  69 static uint16_t rssi = 0;                  // range: [0;1023]
  70 static timeUs_t lastMspRssiUpdateUs = 0;
  71
  72 #define MSP_RSSI_TIMEOUT_US     1500000   // 1.5 sec
  73 #define RX_LQ_INTERVAL_MS       200
  74 #define RX_LQ_TIMEOUT_MS        1000
  75
  76 static rxLinkQualityTracker_e rxLQTracker;
  77 static rssiSource_e activeRssiSource;
  78
  79 static bool rxDataProcessingRequired = false;
  80 static bool auxiliaryProcessingRequired = false;
  81
  82 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
  83 static bool mspOverrideDataProcessingRequired = false;
  84 #endif
  85
  86 static bool rxSignalReceived = false;
  87 static bool rxFlightChannelsValid = false;
  88 static uint8_t rxChannelCount;
  89
  90 static timeUs_t rxNextUpdateAtUs = 0;
  91 static timeUs_t needRxSignalBefore = 0;
  92 static timeUs_t suspendRxSignalUntil = 0;
  93 static uint8_t skipRxSamples = 0;
  94
  95 static rcChannel_t rcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
  96
  97 #define SKIP_RC_ON_SUSPEND_PERIOD 1500000           // 1.5 second period in usec (call frequency independent)
  98 #define SKIP_RC_SAMPLES_ON_RESUME  2                // flush 2 samples to drop wrong measurements (timing independent)
  99
 100 rxLinkStatistics_t rxLinkStatistics;
 101 rxRuntimeConfig_t rxRuntimeConfig;
 102 static uint8_t rcSampleIndex = 0;
 103
 104 PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 12);
 105
 106 #ifndef SERIALRX_PROVIDER
 107 #define SERIALRX_PROVIDER 0
 108 #endif
 109
 110 #ifndef DEFAULT_RX_TYPE
 111 #define DEFAULT_RX_TYPE   RX_TYPE_NONE
 112 #endif
 113
 114 #define RX_MIN_USEX 885
 115 PG_RESET_TEMPLATE(rxConfig_t, rxConfig,
 116     .receiverType = DEFAULT_RX_TYPE,
 117     .rcmap = {0, 1, 3, 2},      // Default to AETR map
 118     .halfDuplex = SETTING_SERIALRX_HALFDUPLEX_DEFAULT,
 119     .serialrx_provider = SERIALRX_PROVIDER,
 120 #ifdef USE_SPEKTRUM_BIND
 121     .spektrum_sat_bind = SETTING_SPEKTRUM_SAT_BIND_DEFAULT,
 122 #endif
 123     .serialrx_inverted = SETTING_SERIALRX_INVERTED_DEFAULT,
 124     .mincheck = SETTING_MIN_CHECK_DEFAULT,
 125     .maxcheck = SETTING_MAX_CHECK_DEFAULT,
 126     .rx_min_usec = SETTING_RX_MIN_USEC_DEFAULT,          // any of first 4 channels below this value will trigger rx loss detection
 127     .rx_max_usec = SETTING_RX_MAX_USEC_DEFAULT,          // any of first 4 channels above this value will trigger rx loss detection
 128     .rssi_channel = SETTING_RSSI_CHANNEL_DEFAULT,
 129     .rssiMin = SETTING_RSSI_MIN_DEFAULT,
 130     .rssiMax = SETTING_RSSI_MAX_DEFAULT,
 131     .sbusSyncInterval = SETTING_SBUS_SYNC_INTERVAL_DEFAULT,
 132     .rcFilterFrequency = SETTING_RC_FILTER_LPF_HZ_DEFAULT,
 133     .autoSmooth = SETTING_RC_FILTER_AUTO_DEFAULT,
 134     .autoSmoothFactor = SETTING_RC_FILTER_SMOOTHING_FACTOR_DEFAULT,
 135 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
 136     .mspOverrideChannels = SETTING_MSP_OVERRIDE_CHANNELS_DEFAULT,
 137 #endif
 138     .rssi_source = SETTING_RSSI_SOURCE_DEFAULT,
 139 #ifdef USE_SERIALRX_SRXL2
 140     .srxl2_unit_id = SETTING_SRXL2_UNIT_ID_DEFAULT,
 141     .srxl2_baud_fast = SETTING_SRXL2_BAUD_FAST_DEFAULT,
 142 #endif
 143 );
 144
 145 void resetAllRxChannelRangeConfigurations(void)
 146 {
 147     // set default calibration to full range and 1:1 mapping
 148     for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) {
 149         rxChannelRangeConfigsMutable(i)->min = PWM_RANGE_MIN;
 150         rxChannelRangeConfigsMutable(i)->max = PWM_RANGE_MAX;
 151     }
 152 }
 153
 154 PG_REGISTER_ARRAY_WITH_RESET_FN(rxChannelRangeConfig_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs, PG_RX_CHANNEL_RANGE_CONFIG, 0);
 155
 156 void pgResetFn_rxChannelRangeConfigs(rxChannelRangeConfig_t *rxChannelRangeConfigs)
 157 {
 158     // set default calibration to full range and 1:1 mapping
 159     for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) {
 160         rxChannelRangeConfigs[i].min = PWM_RANGE_MIN;
 161         rxChannelRangeConfigs[i].max = PWM_RANGE_MAX;
 162     }
 163 }
 164
 165 static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel)
 166 {
 167     UNUSED(rxRuntimeConfig);
 168     UNUSED(channel);
 169
 170     return 0;
 171 }
 172
 173 static uint8_t nullFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 174 {
 175     UNUSED(rxRuntimeConfig);
 176     return RX_FRAME_PENDING;
 177 }
 178
 179 bool isRxPulseValid(uint16_t pulseDuration)
 180 {
 181     return  pulseDuration >= rxConfig()->rx_min_usec &&
 182             pulseDuration <= rxConfig()->rx_max_usec;
 183 }
 184
 185 #ifdef USE_SERIAL_RX
 186 bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 187 {
 188     bool enabled = false;
 189     switch (rxConfig->serialrx_provider) {
 190 #ifdef USE_SERIALRX_SRXL2
 191     case SERIALRX_SRXL2:
 192         enabled = srxl2RxInit(rxConfig, rxRuntimeConfig);
 193         break;
 194 #endif
 195 #ifdef USE_SERIALRX_SPEKTRUM
 196     case SERIALRX_SPEKTRUM1024:
 197     case SERIALRX_SPEKTRUM2048:
 198         enabled = spektrumInit(rxConfig, rxRuntimeConfig);
 199         break;
 200 #endif
 201 #ifdef USE_SERIALRX_SBUS
 202     case SERIALRX_SBUS:
 203         enabled = sbusInit(rxConfig, rxRuntimeConfig);
 204         break;
 205     case SERIALRX_SBUS_FAST:
 206         enabled = sbusInitFast(rxConfig, rxRuntimeConfig);
 207         break;
 208 #endif
 209 #ifdef USE_SERIALRX_SUMD
 210     case SERIALRX_SUMD:
 211         enabled = sumdInit(rxConfig, rxRuntimeConfig);
 212         break;
 213 #endif
 214 #ifdef USE_SERIALRX_IBUS
 215     case SERIALRX_IBUS:
 216         enabled = ibusInit(rxConfig, rxRuntimeConfig);
 217         break;
 218 #endif
 219 #ifdef USE_SERIALRX_JETIEXBUS
 220     case SERIALRX_JETIEXBUS:
 221         enabled = jetiExBusInit(rxConfig, rxRuntimeConfig);
 222         break;
 223 #endif
 224 #ifdef USE_SERIALRX_CRSF
 225     case SERIALRX_CRSF:
 226         enabled = crsfRxInit(rxConfig, rxRuntimeConfig);
 227         break;
 228 #endif
 229 #ifdef USE_SERIALRX_FPORT
 230     case SERIALRX_FPORT:
 231         enabled = fportRxInit(rxConfig, rxRuntimeConfig);
 232         break;
 233 #endif
 234 #ifdef USE_SERIALRX_FPORT2
 235     case SERIALRX_FPORT2:
 236         enabled = fport2RxInit(rxConfig, rxRuntimeConfig);
 237         break;
 238 #endif
 239 #ifdef USE_SERIALRX_GHST
 240     case SERIALRX_GHST:
 241         enabled = ghstRxInit(rxConfig, rxRuntimeConfig);
 242         break;
 243 #endif
 244 #ifdef USE_SERIALRX_MAVLINK
 245     case SERIALRX_MAVLINK:
 246         enabled = mavlinkRxInit(rxConfig, rxRuntimeConfig);
 247         break;
 248 #endif
 249     default:
 250         enabled = false;
 251         break;
 252     }
 253     return enabled;
 254 }
 255 #endif
 256
 257 void rxInit(void)
 258 {
 259     lqTrackerReset(&rxLQTracker);
 260
 261     rxRuntimeConfig.lqTracker = &rxLQTracker;
 262     rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
 263     rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
 264     rxRuntimeConfig.rxSignalTimeout = DELAY_10_HZ;
 265     rcSampleIndex = 0;
 266
 267     timeMs_t nowMs = millis();
 268
 269     for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
 270         rcChannels[i].raw = PWM_RANGE_MIDDLE;
 271         rcChannels[i].data = PWM_RANGE_MIDDLE;
 272         rcChannels[i].expiresAt = nowMs + MAX_INVALID_RX_PULSE_TIME;
 273     }
 274
 275     rcChannels[THROTTLE].raw = (feature(FEATURE_REVERSIBLE_MOTORS)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec;
 276     rcChannels[THROTTLE].data = rcChannels[THROTTLE].raw;
 277
 278     // Initialize ARM switch to OFF position when arming via switch is defined
 279     for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 280         if (modeActivationConditions(i)->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationConditions(i)->range)) {
 281             // ARM switch is defined, determine an OFF value
 282             uint16_t value;
 283             if (modeActivationConditions(i)->range.startStep > 0) {
 284                 value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationConditions(i)->range.startStep - 1));
 285             } else {
 286                 value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationConditions(i)->range.endStep + 1));
 287             }
 288             // Initialize ARM AUX channel to OFF value
 289             rcChannel_t *armChannel = &rcChannels[modeActivationConditions(i)->auxChannelIndex + NON_AUX_CHANNEL_COUNT];
 290             armChannel->raw = value;
 291             armChannel->data = value;
 292         }
 293     }
 294
 295     switch (rxConfig()->receiverType) {
 296
 297 #ifdef USE_SERIAL_RX
 298         case RX_TYPE_SERIAL:
 299             if (!serialRxInit(rxConfig(), &rxRuntimeConfig)) {
 300                 rxConfigMutable()->receiverType = RX_TYPE_NONE;
 301                 rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
 302                 rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
 303             }
 304             break;
 305 #endif
 306
 307 #ifdef USE_RX_MSP
 308         case RX_TYPE_MSP:
 309             rxMspInit(rxConfig(), &rxRuntimeConfig);
 310             break;
 311 #endif
 312
 313         default:
 314         case RX_TYPE_NONE:
 315             rxConfigMutable()->receiverType = RX_TYPE_NONE;
 316             rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
 317             rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
 318             break;
 319     }
 320
 321     rxUpdateRSSISource();
 322
 323 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
 324     if (rxConfig()->receiverType != RX_TYPE_MSP) {
 325         mspOverrideInit();
 326     }
 327 #endif
 328
 329     rxChannelCount = MIN(MAX_SUPPORTED_RC_CHANNEL_COUNT, rxRuntimeConfig.channelCount);
 330 }
 331
 332 void rxUpdateRSSISource(void)
 333 {
 334     activeRssiSource = RSSI_SOURCE_NONE;
 335
 336     if (rxConfig()->rssi_source == RSSI_SOURCE_NONE) {
 337         return;
 338     }
 339
 340 #if defined(USE_ADC)
 341     if (rxConfig()->rssi_source == RSSI_SOURCE_ADC || rxConfig()->rssi_source == RSSI_SOURCE_AUTO) {
 342         if (feature(FEATURE_RSSI_ADC)) {
 343             activeRssiSource = RSSI_SOURCE_ADC;
 344             return;
 345         }
 346     }
 347 #endif
 348
 349     if (rxConfig()->rssi_source == RSSI_SOURCE_RX_CHANNEL || rxConfig()->rssi_source == RSSI_SOURCE_AUTO) {
 350         if (rxConfig()->rssi_channel > 0) {
 351             activeRssiSource = RSSI_SOURCE_RX_CHANNEL;
 352             return;
 353         }
 354     }
 355
 356     if (rxConfig()->rssi_source == RSSI_SOURCE_RX_PROTOCOL || rxConfig()->rssi_source == RSSI_SOURCE_AUTO) {
 357         activeRssiSource = RSSI_SOURCE_RX_PROTOCOL;
 358         return;
 359     }
 360 }
 361
 362 uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap)
 363 {
 364     if (channelToRemap < channelMapEntryCount) {
 365         return channelMap[channelToRemap];
 366     }
 367     return channelToRemap;
 368 }
 369
 370 bool rxIsReceivingSignal(void)
 371 {
 372     return rxSignalReceived;
 373 }
 374
 375 bool rxAreFlightChannelsValid(void)
 376 {
 377     return rxFlightChannelsValid;
 378 }
 379
 380 void suspendRxSignal(void)
 381 {
 382     failsafeOnRxSuspend();
 383     suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
 384     skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
 385 }
 386
 387 void resumeRxSignal(void)
 388 {
 389     suspendRxSignalUntil = micros();
 390     skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
 391     failsafeOnRxResume();
 392 }
 393
 394 bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
 395 {
 396     UNUSED(currentDeltaTime);
 397
 398     if (rxSignalReceived) {
 399         if (currentTimeUs >= needRxSignalBefore) {
 400             rxSignalReceived = false;
 401         }
 402     }
 403
 404     const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
 405
 406     if (frameStatus & RX_FRAME_COMPLETE) {
 407         // RX_FRAME_COMPLETE updated the failsafe status regardless
 408         rxSignalReceived = (frameStatus & RX_FRAME_FAILSAFE) == 0;
 409         needRxSignalBefore = currentTimeUs + rxRuntimeConfig.rxSignalTimeout;
 410         rxDataProcessingRequired = true;
 411     }
 412     else if ((frameStatus & RX_FRAME_FAILSAFE) && rxSignalReceived) {
 413         // All other receiver statuses are allowed to report failsafe, but not allowed to leave it
 414         rxSignalReceived = false;
 415     }
 416
 417     if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) {
 418         auxiliaryProcessingRequired = true;
 419     }
 420
 421     if (cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) {
 422         rxDataProcessingRequired = true;
 423     }
 424
 425     bool result = rxDataProcessingRequired || auxiliaryProcessingRequired;
 426
 427 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
 428     if (rxConfig()->receiverType != RX_TYPE_MSP) {
 429         mspOverrideDataProcessingRequired = mspOverrideUpdateCheck(currentTimeUs, currentDeltaTime);
 430         result = result || mspOverrideDataProcessingRequired;
 431     }
 432 #endif
 433
 434     return result;
 435 }
 436
 437 bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
 438 {
 439     int16_t rcStaging[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 440     const timeMs_t currentTimeMs = millis();
 441
 442 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
 443     if ((rxConfig()->receiverType != RX_TYPE_MSP) && mspOverrideDataProcessingRequired) {
 444         mspOverrideCalculateChannels(currentTimeUs);
 445     }
 446 #endif
 447
 448     if (auxiliaryProcessingRequired) {
 449         auxiliaryProcessingRequired = !rxRuntimeConfig.rcProcessFrameFn(&rxRuntimeConfig);
 450     }
 451
 452     if (!rxDataProcessingRequired) {
 453         return false;
 454     }
 455
 456     rxDataProcessingRequired = false;
 457     rxNextUpdateAtUs = currentTimeUs + DELAY_10_HZ;
 458
 459     // only proceed when no more samples to skip and suspend period is over
 460     if (skipRxSamples) {
 461         if (currentTimeUs > suspendRxSignalUntil) {
 462             skipRxSamples--;
 463         }
 464
 465         return true;
 466     }
 467
 468     rxFlightChannelsValid = true;
 469
 470     // Read and process channel data
 471     for (int channel = 0; channel < rxChannelCount; channel++) {
 472         const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel);
 473
 474         // sample the channel
 475         uint16_t sample = (*rxRuntimeConfig.rcReadRawFn)(&rxRuntimeConfig, rawChannel);
 476
 477         // apply the rx calibration to flight channel
 478         if (channel < NON_AUX_CHANNEL_COUNT && sample != 0) {
 479             sample = scaleRange(sample, rxChannelRangeConfigs(channel)->min, rxChannelRangeConfigs(channel)->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
 480             sample = MIN(MAX(PWM_PULSE_MIN, sample), PWM_PULSE_MAX);
 481         }
 482
 483         // Store as rxRaw
 484         rcChannels[channel].raw = sample;
 485
 486         // Apply invalid pulse value logic
 487         if (!isRxPulseValid(sample)) {
 488             sample = rcChannels[channel].data;   // hold channel, replace with old value
 489             if ((currentTimeMs > rcChannels[channel].expiresAt) && (channel < NON_AUX_CHANNEL_COUNT)) {
 490                 rxFlightChannelsValid = false;
 491             }
 492         } else {
 493             rcChannels[channel].expiresAt = currentTimeMs + MAX_INVALID_RX_PULSE_TIME;
 494         }
 495
 496         // Save channel value
 497         rcStaging[channel] = sample;
 498     }
 499
 500     // Update channel input value if receiver is not in failsafe mode
 501     // If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good input values are retained
 502     if (rxFlightChannelsValid && rxSignalReceived) {
 503         for (int channel = 0; channel < rxChannelCount; channel++) {
 504             rcChannels[channel].data = rcStaging[channel];
 505         }
 506     }
 507
 508 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
 509     if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && !mspOverrideIsInFailsafe()) {
 510         mspOverrideChannels(rcChannels);
 511     }
 512 #endif
 513
 514     // Update failsafe
 515     if (rxFlightChannelsValid && rxSignalReceived) {
 516         failsafeOnValidDataReceived();
 517     } else {
 518         failsafeOnValidDataFailed();
 519     }
 520
 521     rcSampleIndex++;
 522     return true;
 523 }
 524
 525 void parseRcChannels(const char *input)
 526 {
 527     for (const char *c = input; *c; c++) {
 528         const char *s = strchr(rcChannelLetters, *c);
 529         if (s && (s < rcChannelLetters + MAX_MAPPABLE_RX_INPUTS))
 530             rxConfigMutable()->rcmap[s - rcChannelLetters] = c - input;
 531     }
 532 }
 533
 534 #define RSSI_SAMPLE_COUNT 16
 535
 536 static void setRSSIValue(uint16_t rssiValue, rssiSource_e source, bool filtered)
 537 {
 538     if (source != activeRssiSource) {
 539         return;
 540     }
 541
 542     static uint16_t rssiSamples[RSSI_SAMPLE_COUNT];
 543     static uint8_t rssiSampleIndex = 0;
 544     static unsigned sum = 0;
 545
 546     if (filtered) {
 547         // Value is already filtered
 548         rssi = rssiValue;
 549
 550     } else {
 551         sum = sum + rssiValue;
 552         sum = sum - rssiSamples[rssiSampleIndex];
 553         rssiSamples[rssiSampleIndex] = rssiValue;
 554         rssiSampleIndex = (rssiSampleIndex + 1) % RSSI_SAMPLE_COUNT;
 555
 556         int16_t rssiMean = sum / RSSI_SAMPLE_COUNT;
 557
 558         rssi = rssiMean;
 559     }
 560
 561     // Apply min/max values
 562     int rssiMin = rxConfig()->rssiMin * RSSI_VISIBLE_FACTOR;
 563     int rssiMax = rxConfig()->rssiMax * RSSI_VISIBLE_FACTOR;
 564     if (rssiMin > rssiMax) {
 565         int tmp = rssiMax;
 566         rssiMax = rssiMin;
 567         rssiMin = tmp;
 568         int delta = rssi >= rssiMin ? rssi - rssiMin : 0;
 569         rssi = rssiMax >= delta ? rssiMax - delta : 0;
 570     }
 571     rssi = constrain(scaleRange(rssi, rssiMin, rssiMax, 0, RSSI_MAX_VALUE), 0, RSSI_MAX_VALUE);
 572 }
 573
 574 void setRSSIFromMSP(uint8_t newMspRssi)
 575 {
 576     if (activeRssiSource == RSSI_SOURCE_NONE && (rxConfig()->rssi_source == RSSI_SOURCE_MSP || rxConfig()->rssi_source == RSSI_SOURCE_AUTO)) {
 577         activeRssiSource = RSSI_SOURCE_MSP;
 578     }
 579
 580     if (activeRssiSource == RSSI_SOURCE_MSP) {
 581         rssi = ((uint16_t)newMspRssi) << 2;
 582         lastMspRssiUpdateUs = micros();
 583     }
 584 }
 585
 586 static void updateRSSIFromChannel(void)
 587 {
 588     if (rxConfig()->rssi_channel > 0) {
 589         int pwmRssi = rcChannels[rxConfig()->rssi_channel - 1].raw;
 590         int rawRSSI = (uint16_t)((constrain(pwmRssi - 1000, 0, 1000) / 1000.0f) * (RSSI_MAX_VALUE * 1.0f));
 591         setRSSIValue(rawRSSI, RSSI_SOURCE_RX_CHANNEL, false);
 592     }
 593 }
 594
 595 static void updateRSSIFromADC(void)
 596 {
 597 #ifdef USE_ADC
 598     uint16_t rawRSSI = adcGetChannel(ADC_RSSI) / 4;    // Reduce to [0;1023]
 599     setRSSIValue(rawRSSI, RSSI_SOURCE_ADC, false);
 600 #else
 601     setRSSIValue(0, RSSI_SOURCE_ADC, false);
 602 #endif
 603 }
 604
 605 static void updateRSSIFromProtocol(void)
 606 {
 607     setRSSIValue(lqTrackerGet(&rxLQTracker), RSSI_SOURCE_RX_PROTOCOL, false);
 608 }
 609
 610 void updateRSSI(timeUs_t currentTimeUs)
 611 {
 612     // Read RSSI
 613     switch (activeRssiSource) {
 614     case RSSI_SOURCE_ADC:
 615         updateRSSIFromADC();
 616         break;
 617     case RSSI_SOURCE_RX_CHANNEL:
 618         updateRSSIFromChannel();
 619         break;
 620     case RSSI_SOURCE_RX_PROTOCOL:
 621         updateRSSIFromProtocol();
 622         break;
 623     case RSSI_SOURCE_MSP:
 624         if (cmpTimeUs(currentTimeUs, lastMspRssiUpdateUs) > MSP_RSSI_TIMEOUT_US) {
 625             rssi = 0;
 626         }
 627         break;
 628     default:
 629         rssi = 0;
 630         break;
 631     }
 632 }
 633
 634 uint16_t getRSSI(void)
 635 {
 636     return rssi;
 637 }
 638
 639 rssiSource_e getRSSISource(void)
 640 {
 641     return activeRssiSource;
 642 }
 643
 644 int16_t rxGetChannelValue(unsigned channelNumber)
 645 {
 646     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL)) {
 647         return getRcChannelOverride(channelNumber, rcChannels[channelNumber].data);
 648     } else {
 649         return rcChannels[channelNumber].data;
 650     }
 651 }
 652
 653 void lqTrackerReset(rxLinkQualityTracker_e * lqTracker)
 654 {
 655     lqTracker->lastUpdatedMs = millis();
 656     lqTracker->lqAccumulator = 0;
 657     lqTracker->lqCount = 0;
 658     lqTracker->lqValue = 0;
 659 }
 660
 661 void lqTrackerAccumulate(rxLinkQualityTracker_e * lqTracker, uint16_t rawValue)
 662 {
 663     const timeMs_t currentTimeMs = millis();
 664
 665     if (((currentTimeMs - lqTracker->lastUpdatedMs) > RX_LQ_INTERVAL_MS) && lqTracker->lqCount) {
 666         lqTrackerSet(lqTracker, lqTracker->lqAccumulator / lqTracker->lqCount);
 667         lqTracker->lqAccumulator = 0;
 668         lqTracker->lqCount = 0;
 669     }
 670
 671     lqTracker->lqAccumulator += rawValue;
 672     lqTracker->lqCount += 1;
 673 }
 674
 675 void lqTrackerSet(rxLinkQualityTracker_e * lqTracker, uint16_t rawValue)
 676 {
 677     lqTracker->lqValue = rawValue;
 678     lqTracker->lastUpdatedMs = millis();
 679 }
 680
 681 uint16_t lqTrackerGet(rxLinkQualityTracker_e * lqTracker)
 682 {
 683     if ((millis() - lqTracker->lastUpdatedMs) > RX_LQ_TIMEOUT_MS) {
 684         lqTracker->lqValue = 0;
 685     }
 686
 687     return lqTracker->lqValue;
 688 }