src/main/rx/rx.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 <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24
  25 #include <string.h>
  26
  27 #include "platform.h"
  28
  29 #include "build/build_config.h"
  30 #include "build/debug.h"
  31
  32 #include "common/maths.h"
  33 #include "common/utils.h"
  34 #include "common/filter.h"
  35
  36 #include "config/config_reset.h"
  37 #include "config/feature.h"
  38
  39 #include "drivers/adc.h"
  40 #include "drivers/rx/rx_pwm.h"
  41 #include "drivers/rx/rx_spi.h"
  42 #include "drivers/time.h"
  43
  44 #include "fc/config.h"
  45 #include "fc/rc_controls.h"
  46 #include "fc/rc_modes.h"
  47
  48 #include "flight/failsafe.h"
  49
  50 #include "io/serial.h"
  51
  52 #include "pg/pg.h"
  53 #include "pg/pg_ids.h"
  54 #include "pg/rx.h"
  55
  56 #include "rx/rx.h"
  57 #include "rx/pwm.h"
  58 #include "rx/fport.h"
  59 #include "rx/sbus.h"
  60 #include "rx/spektrum.h"
  61 #include "rx/srxl2.h"
  62 #include "rx/sumd.h"
  63 #include "rx/sumh.h"
  64 #include "rx/msp.h"
  65 #include "rx/xbus.h"
  66 #include "rx/ibus.h"
  67 #include "rx/jetiexbus.h"
  68 #include "rx/crsf.h"
  69 #include "rx/rx_spi.h"
  70 #include "rx/targetcustomserial.h"
  71
  72
  73 const char rcChannelLetters[] = "AERT12345678abcdefgh";
  74
  75 static uint16_t rssi = 0;                  // range: [0;1023]
  76 static uint8_t rssi_dbm = 130;             // range: [0;130] display 0 to -130
  77 static timeUs_t lastMspRssiUpdateUs = 0;
  78
  79 static pt1Filter_t frameErrFilter;
  80
  81 #ifdef USE_RX_LINK_QUALITY_INFO
  82 static uint16_t linkQuality = 0;
  83 #endif
  84
  85 #define MSP_RSSI_TIMEOUT_US 1500000   // 1.5 sec
  86
  87 #define RSSI_ADC_DIVISOR (4096 / 1024)
  88 #define RSSI_OFFSET_SCALING (1024 / 100.0f)
  89
  90 rssiSource_e rssiSource;
  91 linkQualitySource_e linkQualitySource;
  92
  93 static bool rxDataProcessingRequired = false;
  94 static bool auxiliaryProcessingRequired = false;
  95
  96 static bool rxSignalReceived = false;
  97 static bool rxFlightChannelsValid = false;
  98 static bool rxIsInFailsafeMode = true;
  99 static uint8_t rxChannelCount;
 100
 101 static timeUs_t rxNextUpdateAtUs = 0;
 102 static uint32_t needRxSignalBefore = 0;
 103 static uint32_t needRxSignalMaxDelayUs;
 104 static uint32_t suspendRxSignalUntil = 0;
 105 static uint8_t  skipRxSamples = 0;
 106
 107 static int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
 108 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
 109 uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 110
 111 #define MAX_INVALID_PULS_TIME    300
 112 #define PPM_AND_PWM_SAMPLE_COUNT 3
 113
 114 #define DELAY_50_HZ (1000000 / 50)
 115 #define DELAY_33_HZ (1000000 / 33)
 116 #define DELAY_10_HZ (1000000 / 10)
 117 #define DELAY_5_HZ (1000000 / 5)
 118 #define SKIP_RC_ON_SUSPEND_PERIOD 1500000           // 1.5 second period in usec (call frequency independent)
 119 #define SKIP_RC_SAMPLES_ON_RESUME  2                // flush 2 samples to drop wrong measurements (timing independent)
 120
 121 rxRuntimeConfig_t rxRuntimeConfig;
 122 static uint8_t rcSampleIndex = 0;
 123
 124 PG_REGISTER_ARRAY_WITH_RESET_FN(rxChannelRangeConfig_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs, PG_RX_CHANNEL_RANGE_CONFIG, 0);
 125 void pgResetFn_rxChannelRangeConfigs(rxChannelRangeConfig_t *rxChannelRangeConfigs)
 126 {
 127     // set default calibration to full range and 1:1 mapping
 128     for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) {
 129         rxChannelRangeConfigs[i].min = PWM_RANGE_MIN;
 130         rxChannelRangeConfigs[i].max = PWM_RANGE_MAX;
 131     }
 132 }
 133
 134 PG_REGISTER_ARRAY_WITH_RESET_FN(rxFailsafeChannelConfig_t, MAX_SUPPORTED_RC_CHANNEL_COUNT, rxFailsafeChannelConfigs, PG_RX_FAILSAFE_CHANNEL_CONFIG, 0);
 135 void pgResetFn_rxFailsafeChannelConfigs(rxFailsafeChannelConfig_t *rxFailsafeChannelConfigs)
 136 {
 137     for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
 138         rxFailsafeChannelConfigs[i].mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD;
 139         rxFailsafeChannelConfigs[i].step = (i == THROTTLE)
 140             ? CHANNEL_VALUE_TO_RXFAIL_STEP(RX_MIN_USEC)
 141             : CHANNEL_VALUE_TO_RXFAIL_STEP(RX_MID_USEC);
 142     }
 143 }
 144
 145 void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig) {
 146     // set default calibration to full range and 1:1 mapping
 147     for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) {
 148         rxChannelRangeConfig->min = PWM_RANGE_MIN;
 149         rxChannelRangeConfig->max = PWM_RANGE_MAX;
 150         rxChannelRangeConfig++;
 151     }
 152 }
 153
 154 static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel)
 155 {
 156     UNUSED(rxRuntimeConfig);
 157     UNUSED(channel);
 158
 159     return PPM_RCVR_TIMEOUT;
 160 }
 161
 162 static uint8_t nullFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 163 {
 164     UNUSED(rxRuntimeConfig);
 165
 166     return RX_FRAME_PENDING;
 167 }
 168
 169 static bool nullProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
 170 {
 171     UNUSED(rxRuntimeConfig);
 172
 173     return true;
 174 }
 175
 176 STATIC_UNIT_TESTED bool isPulseValid(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 static bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 184 {
 185     bool enabled = false;
 186     switch (rxRuntimeConfig->serialrxProvider) {
 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_SRXL:
 194     case SERIALRX_SPEKTRUM1024:
 195     case SERIALRX_SPEKTRUM2048:
 196         enabled = spektrumInit(rxConfig, rxRuntimeConfig);
 197         break;
 198 #endif
 199 #ifdef USE_SERIALRX_SBUS
 200     case SERIALRX_SBUS:
 201         enabled = sbusInit(rxConfig, rxRuntimeConfig);
 202         break;
 203 #endif
 204 #ifdef USE_SERIALRX_SUMD
 205     case SERIALRX_SUMD:
 206         enabled = sumdInit(rxConfig, rxRuntimeConfig);
 207         break;
 208 #endif
 209 #ifdef USE_SERIALRX_SUMH
 210     case SERIALRX_SUMH:
 211         enabled = sumhInit(rxConfig, rxRuntimeConfig);
 212         break;
 213 #endif
 214 #ifdef USE_SERIALRX_XBUS
 215     case SERIALRX_XBUS_MODE_B:
 216     case SERIALRX_XBUS_MODE_B_RJ01:
 217         enabled = xBusInit(rxConfig, rxRuntimeConfig);
 218         break;
 219 #endif
 220 #ifdef USE_SERIALRX_IBUS
 221     case SERIALRX_IBUS:
 222         enabled = ibusInit(rxConfig, rxRuntimeConfig);
 223         break;
 224 #endif
 225 #ifdef USE_SERIALRX_JETIEXBUS
 226     case SERIALRX_JETIEXBUS:
 227         enabled = jetiExBusInit(rxConfig, rxRuntimeConfig);
 228         break;
 229 #endif
 230 #ifdef USE_SERIALRX_CRSF
 231     case SERIALRX_CRSF:
 232         enabled = crsfRxInit(rxConfig, rxRuntimeConfig);
 233         break;
 234 #endif
 235 #ifdef USE_SERIALRX_TARGET_CUSTOM
 236     case SERIALRX_TARGET_CUSTOM:
 237         enabled = targetCustomSerialRxInit(rxConfig, rxRuntimeConfig);
 238         break;
 239 #endif
 240 #ifdef USE_SERIALRX_FPORT
 241     case SERIALRX_FPORT:
 242         enabled = fportRxInit(rxConfig, rxRuntimeConfig);
 243         break;
 244 #endif
 245     default:
 246         enabled = false;
 247         break;
 248     }
 249     return enabled;
 250 }
 251 #endif
 252
 253 void rxInit(void)
 254 {
 255     if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM)) {
 256         rxRuntimeConfig.rxProvider = RX_PROVIDER_PARALLEL_PWM;
 257     } else if (featureIsEnabled(FEATURE_RX_PPM)) {
 258         rxRuntimeConfig.rxProvider = RX_PROVIDER_PPM;
 259     } else if (featureIsEnabled(FEATURE_RX_SERIAL)) {
 260         rxRuntimeConfig.rxProvider = RX_PROVIDER_SERIAL;
 261     } else if (featureIsEnabled(FEATURE_RX_MSP)) {
 262         rxRuntimeConfig.rxProvider = RX_PROVIDER_MSP;
 263     } else if (featureIsEnabled(FEATURE_RX_SPI)) {
 264         rxRuntimeConfig.rxProvider = RX_PROVIDER_SPI;
 265     } else {
 266         rxRuntimeConfig.rxProvider = RX_PROVIDER_NONE;
 267     }
 268     rxRuntimeConfig.serialrxProvider = rxConfig()->serialrx_provider;
 269     rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
 270     rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
 271     rxRuntimeConfig.rcProcessFrameFn = nullProcessFrame;
 272     rcSampleIndex = 0;
 273     needRxSignalMaxDelayUs = DELAY_10_HZ;
 274
 275     for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
 276         rcData[i] = rxConfig()->midrc;
 277         rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME;
 278     }
 279
 280     rcData[THROTTLE] = (featureIsEnabled(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec;
 281
 282     // Initialize ARM switch to OFF position when arming via switch is defined
 283     // TODO - move to rc_mode.c
 284     for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 285         const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(i);
 286         if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
 287             // ARM switch is defined, determine an OFF value
 288             uint16_t value;
 289             if (modeActivationCondition->range.startStep > 0) {
 290                 value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationCondition->range.startStep - 1));
 291             } else {
 292                 value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationCondition->range.endStep + 1));
 293             }
 294             // Initialize ARM AUX channel to OFF value
 295             rcData[modeActivationCondition->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = value;
 296         }
 297     }
 298
 299     switch (rxRuntimeConfig.rxProvider) {
 300     default:
 301
 302         break;
 303 #ifdef USE_SERIAL_RX
 304     case RX_PROVIDER_SERIAL:
 305         {
 306             const bool enabled = serialRxInit(rxConfig(), &rxRuntimeConfig);
 307             if (!enabled) {
 308                 rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
 309                 rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
 310             }
 311         }
 312
 313         break;
 314 #endif
 315
 316 #ifdef USE_RX_MSP
 317     case RX_PROVIDER_MSP:
 318         rxMspInit(rxConfig(), &rxRuntimeConfig);
 319         needRxSignalMaxDelayUs = DELAY_5_HZ;
 320
 321         break;
 322 #endif
 323
 324 #ifdef USE_RX_SPI
 325     case RX_PROVIDER_SPI:
 326         {
 327             const bool enabled = rxSpiInit(rxSpiConfig(), &rxRuntimeConfig);
 328             if (!enabled) {
 329                 rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
 330                 rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
 331             }
 332         }
 333
 334         break;
 335 #endif
 336
 337 #if defined(USE_PWM) || defined(USE_PPM)
 338     case RX_PROVIDER_PPM:
 339     case RX_PROVIDER_PARALLEL_PWM:
 340         rxPwmInit(rxConfig(), &rxRuntimeConfig);
 341
 342         break;
 343 #endif
 344     }
 345
 346 #if defined(USE_ADC)
 347     if (featureIsEnabled(FEATURE_RSSI_ADC)) {
 348         rssiSource = RSSI_SOURCE_ADC;
 349     } else
 350 #endif
 351     if (rxConfig()->rssi_channel > 0) {
 352         rssiSource = RSSI_SOURCE_RX_CHANNEL;
 353     }
 354
 355     // Setup source frame RSSI filtering to take averaged values every FRAME_ERR_RESAMPLE_US
 356     pt1FilterInit(&frameErrFilter, pt1FilterGain(GET_FRAME_ERR_LPF_FREQUENCY(rxConfig()->rssi_src_frame_lpf_period), FRAME_ERR_RESAMPLE_US/1000000.0));
 357
 358     rxChannelCount = MIN(rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT, rxRuntimeConfig.channelCount);
 359 }
 360
 361 bool rxIsReceivingSignal(void)
 362 {
 363     return rxSignalReceived;
 364 }
 365
 366 bool rxAreFlightChannelsValid(void)
 367 {
 368     return rxFlightChannelsValid;
 369 }
 370
 371 void suspendRxPwmPpmSignal(void)
 372 {
 373 #if defined(USE_PWM) || defined(USE_PPM)
 374     if (rxRuntimeConfig.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeConfig.rxProvider == RX_PROVIDER_PPM) {
 375         suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
 376         skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
 377         failsafeOnRxSuspend(SKIP_RC_ON_SUSPEND_PERIOD);
 378     }
 379 #endif
 380 }
 381
 382 void resumeRxPwmPpmSignal(void)
 383 {
 384 #if defined(USE_PWM) || defined(USE_PPM)
 385     if (rxRuntimeConfig.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeConfig.rxProvider == RX_PROVIDER_PPM) {
 386         suspendRxSignalUntil = micros();
 387         skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
 388         failsafeOnRxResume();
 389     }
 390 #endif
 391 }
 392
 393 #ifdef USE_RX_LINK_QUALITY_INFO
 394 #define LINK_QUALITY_SAMPLE_COUNT 16
 395
 396 STATIC_UNIT_TESTED uint16_t updateLinkQualitySamples(uint16_t value)
 397 {
 398     static uint16_t samples[LINK_QUALITY_SAMPLE_COUNT];
 399     static uint8_t sampleIndex = 0;
 400     static uint16_t sum = 0;
 401
 402     sum += value - samples[sampleIndex];
 403     samples[sampleIndex] = value;
 404     sampleIndex = (sampleIndex + 1) % LINK_QUALITY_SAMPLE_COUNT;
 405     return sum / LINK_QUALITY_SAMPLE_COUNT;
 406 }
 407 #endif
 408
 409 static void setLinkQuality(bool validFrame, timeDelta_t currentDeltaTime)
 410 {
 411 #ifdef USE_RX_LINK_QUALITY_INFO
 412     if (linkQualitySource != LQ_SOURCE_RX_PROTOCOL_CRSF) {
 413         // calculate new sample mean
 414         linkQuality = updateLinkQualitySamples(validFrame ? LINK_QUALITY_MAX_VALUE : 0);
 415     }
 416 #endif
 417
 418     if (rssiSource == RSSI_SOURCE_FRAME_ERRORS) {
 419         static uint16_t tot_rssi = 0;
 420         static uint16_t cnt_rssi = 0;
 421         static timeDelta_t resample_time = 0;
 422
 423         resample_time += currentDeltaTime;
 424         tot_rssi += validFrame ? RSSI_MAX_VALUE : 0;
 425         cnt_rssi++;
 426
 427         if (resample_time >= FRAME_ERR_RESAMPLE_US) {
 428             setRssi(tot_rssi / cnt_rssi, rssiSource);
 429             tot_rssi = 0;
 430             cnt_rssi = 0;
 431             resample_time -= FRAME_ERR_RESAMPLE_US;
 432         }
 433     }
 434 }
 435
 436 void setLinkQualityDirect(uint16_t linkqualityValue)
 437 {
 438 #ifdef USE_RX_LINK_QUALITY_INFO
 439     linkQuality = linkqualityValue;
 440 #else
 441     UNUSED(linkqualityValue);
 442 #endif
 443 }
 444
 445 bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
 446 {
 447     bool signalReceived = false;
 448     bool useDataDrivenProcessing = true;
 449
 450     switch (rxRuntimeConfig.rxProvider) {
 451     default:
 452
 453         break;
 454 #if defined(USE_PWM) || defined(USE_PPM)
 455     case RX_PROVIDER_PPM:
 456         if (isPPMDataBeingReceived()) {
 457             signalReceived = true;
 458             rxIsInFailsafeMode = false;
 459             needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
 460             resetPPMDataReceivedState();
 461         }
 462
 463         break;
 464     case RX_PROVIDER_PARALLEL_PWM:
 465         if (isPWMDataBeingReceived()) {
 466             signalReceived = true;
 467             rxIsInFailsafeMode = false;
 468             needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
 469             useDataDrivenProcessing = false;
 470         }
 471
 472         break;
 473 #endif
 474     case RX_PROVIDER_SERIAL:
 475     case RX_PROVIDER_MSP:
 476     case RX_PROVIDER_SPI:
 477         {
 478             const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
 479             if (frameStatus & RX_FRAME_COMPLETE) {
 480                 rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
 481                 bool rxFrameDropped = (frameStatus & RX_FRAME_DROPPED) != 0;
 482                 signalReceived = !(rxIsInFailsafeMode || rxFrameDropped);
 483                 if (signalReceived) {
 484                     needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
 485                 }
 486
 487                 setLinkQuality(signalReceived, currentDeltaTime);
 488             }
 489
 490             if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) {
 491                 auxiliaryProcessingRequired = true;
 492             }
 493         }
 494
 495         break;
 496     }
 497
 498     if (signalReceived) {
 499         rxSignalReceived = true;
 500     } else if (currentTimeUs >= needRxSignalBefore) {
 501         rxSignalReceived = false;
 502     }
 503
 504     if ((signalReceived && useDataDrivenProcessing) || cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) {
 505         rxDataProcessingRequired = true;
 506     }
 507
 508     return rxDataProcessingRequired || auxiliaryProcessingRequired; // data driven or 50Hz
 509 }
 510
 511 #if defined(USE_PWM) || defined(USE_PPM)
 512 static uint16_t calculateChannelMovingAverage(uint8_t chan, uint16_t sample)
 513 {
 514     static int16_t rcSamples[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT][PPM_AND_PWM_SAMPLE_COUNT];
 515     static int16_t rcDataMean[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT];
 516     static bool rxSamplesCollected = false;
 517
 518     const uint8_t currentSampleIndex = rcSampleIndex % PPM_AND_PWM_SAMPLE_COUNT;
 519
 520     // update the recent samples and compute the average of them
 521     rcSamples[chan][currentSampleIndex] = sample;
 522
 523     // avoid returning an incorrect average which would otherwise occur before enough samples
 524     if (!rxSamplesCollected) {
 525         if (rcSampleIndex < PPM_AND_PWM_SAMPLE_COUNT) {
 526             return sample;
 527         }
 528         rxSamplesCollected = true;
 529     }
 530
 531     rcDataMean[chan] = 0;
 532     for (int sampleIndex = 0; sampleIndex < PPM_AND_PWM_SAMPLE_COUNT; sampleIndex++) {
 533         rcDataMean[chan] += rcSamples[chan][sampleIndex];
 534     }
 535     return rcDataMean[chan] / PPM_AND_PWM_SAMPLE_COUNT;
 536 }
 537 #endif
 538
 539 static uint16_t getRxfailValue(uint8_t channel)
 540 {
 541     const rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigs(channel);
 542
 543     switch (channelFailsafeConfig->mode) {
 544     case RX_FAILSAFE_MODE_AUTO:
 545         switch (channel) {
 546         case ROLL:
 547         case PITCH:
 548         case YAW:
 549             return rxConfig()->midrc;
 550         case THROTTLE:
 551             if (featureIsEnabled(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3D) && !flight3DConfig()->switched_mode3d) {
 552                 return rxConfig()->midrc;
 553             } else {
 554                 return rxConfig()->rx_min_usec;
 555             }
 556         }
 557
 558     FALLTHROUGH;
 559     default:
 560     case RX_FAILSAFE_MODE_INVALID:
 561     case RX_FAILSAFE_MODE_HOLD:
 562         return rcData[channel];
 563
 564     case RX_FAILSAFE_MODE_SET:
 565         return RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfig->step);
 566     }
 567 }
 568
 569 STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range)
 570 {
 571     // Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT
 572     if (sample == PPM_RCVR_TIMEOUT) {
 573         return PPM_RCVR_TIMEOUT;
 574     }
 575
 576     sample = scaleRange(sample, range->min, range->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
 577     sample = constrain(sample, PWM_PULSE_MIN, PWM_PULSE_MAX);
 578
 579     return sample;
 580 }
 581
 582 static void readRxChannelsApplyRanges(void)
 583 {
 584     for (int channel = 0; channel < rxChannelCount; channel++) {
 585
 586         const uint8_t rawChannel = channel < RX_MAPPABLE_CHANNEL_COUNT ? rxConfig()->rcmap[channel] : channel;
 587
 588         // sample the channel
 589         uint16_t sample = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, rawChannel);
 590
 591         // apply the rx calibration
 592         if (channel < NON_AUX_CHANNEL_COUNT) {
 593             sample = applyRxChannelRangeConfiguraton(sample, rxChannelRangeConfigs(channel));
 594         }
 595
 596         rcRaw[channel] = sample;
 597     }
 598 }
 599
 600 static void detectAndApplySignalLossBehaviour(void)
 601 {
 602     const uint32_t currentTimeMs = millis();
 603
 604     const bool useValueFromRx = rxSignalReceived && !rxIsInFailsafeMode;
 605
 606     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 0, rxSignalReceived);
 607     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, rxIsInFailsafeMode);
 608
 609     rxFlightChannelsValid = true;
 610     for (int channel = 0; channel < rxChannelCount; channel++) {
 611         uint16_t sample = rcRaw[channel];
 612
 613         const bool validPulse = useValueFromRx && isPulseValid(sample);
 614
 615         if (validPulse) {
 616             rcInvalidPulsPeriod[channel] = currentTimeMs + MAX_INVALID_PULS_TIME;
 617         } else {
 618             if (cmp32(currentTimeMs, rcInvalidPulsPeriod[channel]) < 0) {
 619                 continue;           // skip to next channel to hold channel value MAX_INVALID_PULS_TIME
 620             } else {
 621                 sample = getRxfailValue(channel);   // after that apply rxfail value
 622                 if (channel < NON_AUX_CHANNEL_COUNT) {
 623                     rxFlightChannelsValid = false;
 624                 }
 625             }
 626         }
 627 #if defined(USE_PWM) || defined(USE_PPM)
 628         if (rxRuntimeConfig.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeConfig.rxProvider == RX_PROVIDER_PPM) {
 629             // smooth output for PWM and PPM
 630             rcData[channel] = calculateChannelMovingAverage(channel, sample);
 631         } else
 632 #endif
 633         {
 634             rcData[channel] = sample;
 635         }
 636     }
 637
 638     if (rxFlightChannelsValid && !IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
 639         failsafeOnValidDataReceived();
 640     } else {
 641         rxIsInFailsafeMode = true;
 642         failsafeOnValidDataFailed();
 643         for (int channel = 0; channel < rxChannelCount; channel++) {
 644             rcData[channel] = getRxfailValue(channel);
 645         }
 646     }
 647     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 3, rcData[THROTTLE]);
 648 }
 649
 650 bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
 651 {
 652     if (auxiliaryProcessingRequired) {
 653         auxiliaryProcessingRequired = !rxRuntimeConfig.rcProcessFrameFn(&rxRuntimeConfig);
 654     }
 655
 656     if (!rxDataProcessingRequired) {
 657         return false;
 658     }
 659
 660     rxDataProcessingRequired = false;
 661     rxNextUpdateAtUs = currentTimeUs + DELAY_33_HZ;
 662
 663     // only proceed when no more samples to skip and suspend period is over
 664     if (skipRxSamples || currentTimeUs <= suspendRxSignalUntil) {
 665         if (currentTimeUs > suspendRxSignalUntil) {
 666             skipRxSamples--;
 667         }
 668
 669         return true;
 670     }
 671
 672     readRxChannelsApplyRanges();
 673     detectAndApplySignalLossBehaviour();
 674
 675     rcSampleIndex++;
 676
 677     return true;
 678 }
 679
 680 void parseRcChannels(const char *input, rxConfig_t *rxConfig)
 681 {
 682     for (const char *c = input; *c; c++) {
 683         const char *s = strchr(rcChannelLetters, *c);
 684         if (s && (s < rcChannelLetters + RX_MAPPABLE_CHANNEL_COUNT)) {
 685             rxConfig->rcmap[s - rcChannelLetters] = c - input;
 686         }
 687     }
 688 }
 689
 690 void setRssiDirect(uint16_t newRssi, rssiSource_e source)
 691 {
 692     if (source != rssiSource) {
 693         return;
 694     }
 695
 696     rssi = newRssi;
 697 }
 698
 699 #define RSSI_SAMPLE_COUNT 16
 700
 701 static uint16_t updateRssiSamples(uint16_t value)
 702 {
 703     static uint16_t samples[RSSI_SAMPLE_COUNT];
 704     static uint8_t sampleIndex = 0;
 705     static unsigned sum = 0;
 706
 707     sum += value - samples[sampleIndex];
 708     samples[sampleIndex] = value;
 709     sampleIndex = (sampleIndex + 1) % RSSI_SAMPLE_COUNT;
 710     return sum / RSSI_SAMPLE_COUNT;
 711 }
 712
 713 void setRssi(uint16_t rssiValue, rssiSource_e source)
 714 {
 715     if (source != rssiSource) {
 716         return;
 717     }
 718
 719     // Filter RSSI value
 720     if (source == RSSI_SOURCE_FRAME_ERRORS) {
 721         rssi = pt1FilterApply(&frameErrFilter, rssiValue);
 722     } else {
 723         // calculate new sample mean
 724         rssi = updateRssiSamples(rssiValue);
 725     }
 726 }
 727
 728 void setRssiMsp(uint8_t newMspRssi)
 729 {
 730     if (rssiSource == RSSI_SOURCE_NONE) {
 731         rssiSource = RSSI_SOURCE_MSP;
 732     }
 733
 734     if (rssiSource == RSSI_SOURCE_MSP) {
 735         rssi = ((uint16_t)newMspRssi) << 2;
 736         lastMspRssiUpdateUs = micros();
 737     }
 738 }
 739
 740 static void updateRSSIPWM(void)
 741 {
 742     // Read value of AUX channel as rssi
 743     int16_t pwmRssi = rcData[rxConfig()->rssi_channel - 1];
 744
 745     // RSSI_Invert option
 746     if (rxConfig()->rssi_invert) {
 747         pwmRssi = ((2000 - pwmRssi) + 1000);
 748     }
 749
 750     // Range of rawPwmRssi is [1000;2000]. rssi should be in [0;1023];
 751     setRssiDirect(constrain(((pwmRssi - 1000) / 1000.0f) * RSSI_MAX_VALUE, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_CHANNEL);
 752 }
 753
 754 static void updateRSSIADC(timeUs_t currentTimeUs)
 755 {
 756 #ifndef USE_ADC
 757     UNUSED(currentTimeUs);
 758 #else
 759     static uint32_t rssiUpdateAt = 0;
 760
 761     if ((int32_t)(currentTimeUs - rssiUpdateAt) < 0) {
 762         return;
 763     }
 764     rssiUpdateAt = currentTimeUs + DELAY_50_HZ;
 765
 766     const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
 767     uint16_t rssiValue = adcRssiSample / RSSI_ADC_DIVISOR;
 768
 769     // RSSI_Invert option
 770     if (rxConfig()->rssi_invert) {
 771         rssiValue = RSSI_MAX_VALUE - rssiValue;
 772     }
 773
 774     setRssi(rssiValue, RSSI_SOURCE_ADC);
 775 #endif
 776 }
 777
 778 void updateRSSI(timeUs_t currentTimeUs)
 779 {
 780     switch (rssiSource) {
 781     case RSSI_SOURCE_RX_CHANNEL:
 782         updateRSSIPWM();
 783         break;
 784     case RSSI_SOURCE_ADC:
 785         updateRSSIADC(currentTimeUs);
 786         break;
 787     case RSSI_SOURCE_MSP:
 788         if (cmpTimeUs(micros(), lastMspRssiUpdateUs) > MSP_RSSI_TIMEOUT_US) {
 789             rssi = 0;
 790         }
 791         break;
 792     default:
 793         break;
 794     }
 795 }
 796
 797 uint16_t getRssi(void)
 798 {
 799     return rxConfig()->rssi_scale / 100.0f * rssi + rxConfig()->rssi_offset * RSSI_OFFSET_SCALING;
 800 }
 801
 802 uint8_t getRssiPercent(void)
 803 {
 804     return scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 100);
 805 }
 806
 807 uint8_t getRssiDbm(void)
 808 {
 809     return rssi_dbm;
 810 }
 811
 812 #define RSSI_SAMPLE_COUNT_DBM 16
 813
 814 static uint8_t updateRssiDbmSamples(uint8_t value)
 815 {
 816     static uint16_t samplesdbm[RSSI_SAMPLE_COUNT_DBM];
 817     static uint8_t sampledbmIndex = 0;
 818     static unsigned sumdbm = 0;
 819
 820     sumdbm += value - samplesdbm[sampledbmIndex];
 821     samplesdbm[sampledbmIndex] = value;
 822     sampledbmIndex = (sampledbmIndex + 1) % RSSI_SAMPLE_COUNT_DBM;
 823     return sumdbm / RSSI_SAMPLE_COUNT_DBM;
 824 }
 825
 826 void setRssiDbm(uint8_t rssiDbmValue, rssiSource_e source)
 827 {
 828     if (source != rssiSource) {
 829         return;
 830     }
 831
 832     rssi_dbm = updateRssiDbmSamples(rssiDbmValue);
 833 }
 834
 835 void setRssiDbmDirect(uint8_t newRssiDbm, rssiSource_e source)
 836 {
 837     if (source != rssiSource) {
 838         return;
 839     }
 840
 841     rssi_dbm = newRssiDbm;
 842 }
 843
 844 #ifdef USE_RX_LINK_QUALITY_INFO
 845 uint16_t rxGetLinkQuality(void)
 846 {
 847     return linkQuality;
 848 }
 849
 850 uint16_t rxGetLinkQualityPercent(void)
 851 {
 852     return (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) ?  (linkQuality / 3.41) : scaleRange(linkQuality, 0, LINK_QUALITY_MAX_VALUE, 0, 100);
 853 }
 854 #endif
 855
 856 uint16_t rxGetRefreshRate(void)
 857 {
 858     return rxRuntimeConfig.rxRefreshRate;
 859 }
 860
 861 bool isRssiConfigured(void)
 862 {
 863     return rssiSource != RSSI_SOURCE_NONE;
 864 }