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_SBUS:
 200         enabled = sbusInit(rxConfig, rxRuntimeConfig);
 201         break;
 202     case SERIALRX_SBUS_FAST:
 203         enabled = sbusInitFast(rxConfig, rxRuntimeConfig);
 204         break;
 205 #endif
 206 #ifdef USE_SERIALRX_SUMD
 207     case SERIALRX_SUMD:
 208         enabled = sumdInit(rxConfig, rxRuntimeConfig);
 209         break;
 210 #endif
 211 #ifdef USE_SERIALRX_IBUS
 212     case SERIALRX_IBUS:
 213         enabled = ibusInit(rxConfig, rxRuntimeConfig);
 214         break;
 215 #endif
 216 #ifdef USE_SERIALRX_JETIEXBUS
 217     case SERIALRX_JETIEXBUS:
 218         enabled = jetiExBusInit(rxConfig, rxRuntimeConfig);
 219         break;
 220 #endif
 221 #ifdef USE_SERIALRX_CRSF
 222     case SERIALRX_CRSF:
 223         enabled = crsfRxInit(rxConfig, rxRuntimeConfig);
 224         break;
 225 #endif
 226 #ifdef USE_SERIALRX_FPORT
 227     case SERIALRX_FPORT:
 228         enabled = fportRxInit(rxConfig, rxRuntimeConfig);
 229         break;
 230 #endif
 231 #ifdef USE_SERIALRX_FPORT2
 232     case SERIALRX_FPORT2:
 233         enabled = fport2RxInit(rxConfig, rxRuntimeConfig, false);
 234         break;
 235     case SERIALRX_FBUS:
 236         enabled = fport2RxInit(rxConfig, rxRuntimeConfig, true);
 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 #ifdef USE_RX_SIM
 314     case RX_TYPE_SIM:
 315         rxSimInit(rxConfig(), &rxRuntimeConfig);
 316         break;
 317 #endif
 318
 319         default:
 320         case RX_TYPE_NONE:
 321             rxConfigMutable()->receiverType = RX_TYPE_NONE;
 322             rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
 323             rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
 324             break;
 325     }
 326
 327     rxUpdateRSSISource();
 328
 329 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
 330     if (rxConfig()->receiverType != RX_TYPE_MSP) {
 331         mspOverrideInit();
 332     }
 333 #endif
 334
 335     rxChannelCount = MIN(MAX_SUPPORTED_RC_CHANNEL_COUNT, rxRuntimeConfig.channelCount);
 336 }
 337
 338 void rxUpdateRSSISource(void)
 339 {
 340     activeRssiSource = RSSI_SOURCE_NONE;
 341
 342     if (rxConfig()->rssi_source == RSSI_SOURCE_NONE) {
 343         return;
 344     }
 345
 346 #if defined(USE_ADC)
 347     if (rxConfig()->rssi_source == RSSI_SOURCE_ADC || rxConfig()->rssi_source == RSSI_SOURCE_AUTO) {
 348         if (feature(FEATURE_RSSI_ADC)) {
 349             activeRssiSource = RSSI_SOURCE_ADC;
 350             return;
 351         }
 352     }
 353 #endif
 354
 355     if (rxConfig()->rssi_source == RSSI_SOURCE_RX_CHANNEL || rxConfig()->rssi_source == RSSI_SOURCE_AUTO) {
 356         if (rxConfig()->rssi_channel > 0) {
 357             activeRssiSource = RSSI_SOURCE_RX_CHANNEL;
 358             return;
 359         }
 360     }
 361
 362     if (rxConfig()->rssi_source == RSSI_SOURCE_RX_PROTOCOL || rxConfig()->rssi_source == RSSI_SOURCE_AUTO) {
 363         activeRssiSource = RSSI_SOURCE_RX_PROTOCOL;
 364         return;
 365     }
 366 }
 367
 368 uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap)
 369 {
 370     if (channelToRemap < channelMapEntryCount) {
 371         return channelMap[channelToRemap];
 372     }
 373     return channelToRemap;
 374 }
 375
 376 bool rxIsReceivingSignal(void)
 377 {
 378     return rxSignalReceived;
 379 }
 380
 381 bool rxAreFlightChannelsValid(void)
 382 {
 383     return rxFlightChannelsValid;
 384 }
 385
 386 void suspendRxSignal(void)
 387 {
 388     failsafeOnRxSuspend();
 389     isRxSuspended = true;
 390 }
 391
 392 void resumeRxSignal(void)
 393 {
 394     isRxSuspended = false;
 395     failsafeOnRxResume();
 396 }
 397
 398 bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
 399 {
 400     UNUSED(currentDeltaTime);
 401
 402     if (rxSignalReceived) {
 403         if (currentTimeUs >= needRxSignalBefore) {
 404             rxSignalReceived = false;
 405         }
 406     }
 407
 408     const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
 409
 410     if (frameStatus & RX_FRAME_COMPLETE) {
 411         // RX_FRAME_COMPLETE updated the failsafe status regardless
 412         rxSignalReceived = (frameStatus & RX_FRAME_FAILSAFE) == 0;
 413         needRxSignalBefore = currentTimeUs + rxRuntimeConfig.rxSignalTimeout;
 414         rxDataProcessingRequired = true;
 415     }
 416     else if ((frameStatus & RX_FRAME_FAILSAFE) && rxSignalReceived) {
 417         // All other receiver statuses are allowed to report failsafe, but not allowed to leave it
 418         rxSignalReceived = false;
 419     }
 420
 421     if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) {
 422         auxiliaryProcessingRequired = true;
 423     }
 424
 425     if (cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) {
 426         rxDataProcessingRequired = true;
 427     }
 428
 429     bool result = rxDataProcessingRequired || auxiliaryProcessingRequired;
 430
 431 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
 432     if (rxConfig()->receiverType != RX_TYPE_MSP) {
 433         mspOverrideDataProcessingRequired = mspOverrideUpdateCheck(currentTimeUs, currentDeltaTime);
 434         result = result || mspOverrideDataProcessingRequired;
 435     }
 436 #endif
 437
 438     return result;
 439 }
 440
 441 bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
 442 {
 443     int16_t rcStaging[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 444     const timeMs_t currentTimeMs = millis();
 445
 446 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
 447     if ((rxConfig()->receiverType != RX_TYPE_MSP) && mspOverrideDataProcessingRequired) {
 448         mspOverrideCalculateChannels(currentTimeUs);
 449     }
 450 #endif
 451
 452     if (auxiliaryProcessingRequired) {
 453         auxiliaryProcessingRequired = !rxRuntimeConfig.rcProcessFrameFn(&rxRuntimeConfig);
 454     }
 455
 456     if (!rxDataProcessingRequired) {
 457         return false;
 458     }
 459
 460     rxDataProcessingRequired = false;
 461     rxNextUpdateAtUs = currentTimeUs + DELAY_10_HZ;
 462
 463     // If RX is suspended, do not process any data
 464     if (isRxSuspended) {
 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 }