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