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.h"
  37 #include "config/config_reset.h"
  38 #include "config/feature.h"
  39
  40 #include "drivers/adc.h"
  41 #include "drivers/rx/rx_pwm.h"
  42 #include "drivers/time.h"
  43
  44 #include "fc/rc_controls.h"
  45 #include "fc/rc_modes.h"
  46
  47 #include "flight/failsafe.h"
  48
  49 #include "io/serial.h"
  50
  51 #include "pg/pg.h"
  52 #include "pg/pg_ids.h"
  53 #include "pg/rx.h"
  54
  55 #include "rx/rx.h"
  56 #include "rx/pwm.h"
  57 #include "rx/fport.h"
  58 #include "rx/sbus.h"
  59 #include "rx/spektrum.h"
  60 #include "rx/srxl2.h"
  61 #include "rx/sumd.h"
  62 #include "rx/sumh.h"
  63 #include "rx/msp.h"
  64 #include "rx/xbus.h"
  65 #include "rx/ibus.h"
  66 #include "rx/jetiexbus.h"
  67 #include "rx/crsf.h"
  68 #include "rx/rx_spi.h"
  69 #include "rx/targetcustomserial.h"
  70
  71
  72 const char rcChannelLetters[] = "AERT12345678abcdefgh";
  73
  74 static uint16_t rssi = 0;                  // range: [0;1023]
  75 static int16_t rssiDbm = CRSF_RSSI_MIN;    // range: [-130,20]
  76 static timeUs_t lastMspRssiUpdateUs = 0;
  77
  78 static pt1Filter_t frameErrFilter;
  79
  80 #ifdef USE_RX_LINK_QUALITY_INFO
  81 static uint16_t linkQuality = 0;
  82 static uint8_t rfMode = 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 rxRuntimeState_t rxRuntimeState;
 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 rxRuntimeState_t *rxRuntimeState, uint8_t channel)
 155 {
 156     UNUSED(rxRuntimeState);
 157     UNUSED(channel);
 158
 159     return PPM_RCVR_TIMEOUT;
 160 }
 161
 162 static uint8_t nullFrameStatus(rxRuntimeState_t *rxRuntimeState)
 163 {
 164     UNUSED(rxRuntimeState);
 165
 166     return RX_FRAME_PENDING;
 167 }
 168
 169 static bool nullProcessFrame(const rxRuntimeState_t *rxRuntimeState)
 170 {
 171     UNUSED(rxRuntimeState);
 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, rxRuntimeState_t *rxRuntimeState)
 184 {
 185     bool enabled = false;
 186     switch (rxRuntimeState->serialrxProvider) {
 187 #ifdef USE_SERIALRX_SRXL2
 188     case SERIALRX_SRXL2:
 189         enabled = srxl2RxInit(rxConfig, rxRuntimeState);
 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, rxRuntimeState);
 197         break;
 198 #endif
 199 #ifdef USE_SERIALRX_SBUS
 200     case SERIALRX_SBUS:
 201         enabled = sbusInit(rxConfig, rxRuntimeState);
 202         break;
 203 #endif
 204 #ifdef USE_SERIALRX_SUMD
 205     case SERIALRX_SUMD:
 206         enabled = sumdInit(rxConfig, rxRuntimeState);
 207         break;
 208 #endif
 209 #ifdef USE_SERIALRX_SUMH
 210     case SERIALRX_SUMH:
 211         enabled = sumhInit(rxConfig, rxRuntimeState);
 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, rxRuntimeState);
 218         break;
 219 #endif
 220 #ifdef USE_SERIALRX_IBUS
 221     case SERIALRX_IBUS:
 222         enabled = ibusInit(rxConfig, rxRuntimeState);
 223         break;
 224 #endif
 225 #ifdef USE_SERIALRX_JETIEXBUS
 226     case SERIALRX_JETIEXBUS:
 227         enabled = jetiExBusInit(rxConfig, rxRuntimeState);
 228         break;
 229 #endif
 230 #ifdef USE_SERIALRX_CRSF
 231     case SERIALRX_CRSF:
 232         enabled = crsfRxInit(rxConfig, rxRuntimeState);
 233         break;
 234 #endif
 235 #ifdef USE_SERIALRX_TARGET_CUSTOM
 236     case SERIALRX_TARGET_CUSTOM:
 237         enabled = targetCustomSerialRxInit(rxConfig, rxRuntimeState);
 238         break;
 239 #endif
 240 #ifdef USE_SERIALRX_FPORT
 241     case SERIALRX_FPORT:
 242         enabled = fportRxInit(rxConfig, rxRuntimeState);
 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         rxRuntimeState.rxProvider = RX_PROVIDER_PARALLEL_PWM;
 257     } else if (featureIsEnabled(FEATURE_RX_PPM)) {
 258         rxRuntimeState.rxProvider = RX_PROVIDER_PPM;
 259     } else if (featureIsEnabled(FEATURE_RX_SERIAL)) {
 260         rxRuntimeState.rxProvider = RX_PROVIDER_SERIAL;
 261     } else if (featureIsEnabled(FEATURE_RX_MSP)) {
 262         rxRuntimeState.rxProvider = RX_PROVIDER_MSP;
 263     } else if (featureIsEnabled(FEATURE_RX_SPI)) {
 264         rxRuntimeState.rxProvider = RX_PROVIDER_SPI;
 265     } else {
 266         rxRuntimeState.rxProvider = RX_PROVIDER_NONE;
 267     }
 268     rxRuntimeState.serialrxProvider = rxConfig()->serialrx_provider;
 269     rxRuntimeState.rcReadRawFn = nullReadRawRC;
 270     rxRuntimeState.rcFrameStatusFn = nullFrameStatus;
 271     rxRuntimeState.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 (rxRuntimeState.rxProvider) {
 300     default:
 301
 302         break;
 303 #ifdef USE_SERIAL_RX
 304     case RX_PROVIDER_SERIAL:
 305         {
 306             const bool enabled = serialRxInit(rxConfig(), &rxRuntimeState);
 307             if (!enabled) {
 308                 rxRuntimeState.rcReadRawFn = nullReadRawRC;
 309                 rxRuntimeState.rcFrameStatusFn = nullFrameStatus;
 310             }
 311         }
 312
 313         break;
 314 #endif
 315
 316 #ifdef USE_RX_MSP
 317     case RX_PROVIDER_MSP:
 318         rxMspInit(rxConfig(), &rxRuntimeState);
 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(), &rxRuntimeState);
 328             if (!enabled) {
 329                 rxRuntimeState.rcReadRawFn = nullReadRawRC;
 330                 rxRuntimeState.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(), &rxRuntimeState);
 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, rxRuntimeState.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 (rxRuntimeState.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeState.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 (rxRuntimeState.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeState.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
 408 void rxSetRfMode(uint8_t rfModeValue)
 409 {
 410     rfMode = rfModeValue;
 411 }
 412 #endif
 413
 414 static void setLinkQuality(bool validFrame, timeDelta_t currentDeltaTimeUs)
 415 {
 416     static uint16_t rssiSum = 0;
 417     static uint16_t rssiCount = 0;
 418     static timeDelta_t resampleTimeUs = 0;
 419
 420 #ifdef USE_RX_LINK_QUALITY_INFO
 421     if (linkQualitySource != LQ_SOURCE_RX_PROTOCOL_CRSF) {
 422         // calculate new sample mean
 423         linkQuality = updateLinkQualitySamples(validFrame ? LINK_QUALITY_MAX_VALUE : 0);
 424     }
 425 #endif
 426
 427     if (rssiSource == RSSI_SOURCE_FRAME_ERRORS) {
 428         resampleTimeUs += currentDeltaTimeUs;
 429         rssiSum += validFrame ? RSSI_MAX_VALUE : 0;
 430         rssiCount++;
 431
 432         if (resampleTimeUs >= FRAME_ERR_RESAMPLE_US) {
 433             setRssi(rssiSum / rssiCount, rssiSource);
 434             rssiSum = 0;
 435             rssiCount = 0;
 436             resampleTimeUs -= FRAME_ERR_RESAMPLE_US;
 437         }
 438     }
 439 }
 440
 441 void setLinkQualityDirect(uint16_t linkqualityValue)
 442 {
 443 #ifdef USE_RX_LINK_QUALITY_INFO
 444     linkQuality = linkqualityValue;
 445 #else
 446     UNUSED(linkqualityValue);
 447 #endif
 448 }
 449
 450 bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
 451 {
 452     bool signalReceived = false;
 453     bool useDataDrivenProcessing = true;
 454
 455     switch (rxRuntimeState.rxProvider) {
 456     default:
 457
 458         break;
 459 #if defined(USE_PWM) || defined(USE_PPM)
 460     case RX_PROVIDER_PPM:
 461         if (isPPMDataBeingReceived()) {
 462             signalReceived = true;
 463             rxIsInFailsafeMode = false;
 464             needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
 465             resetPPMDataReceivedState();
 466         }
 467
 468         break;
 469     case RX_PROVIDER_PARALLEL_PWM:
 470         if (isPWMDataBeingReceived()) {
 471             signalReceived = true;
 472             rxIsInFailsafeMode = false;
 473             needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
 474             useDataDrivenProcessing = false;
 475         }
 476
 477         break;
 478 #endif
 479     case RX_PROVIDER_SERIAL:
 480     case RX_PROVIDER_MSP:
 481     case RX_PROVIDER_SPI:
 482         {
 483             const uint8_t frameStatus = rxRuntimeState.rcFrameStatusFn(&rxRuntimeState);
 484             if (frameStatus & RX_FRAME_COMPLETE) {
 485                 rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
 486                 bool rxFrameDropped = (frameStatus & RX_FRAME_DROPPED) != 0;
 487                 signalReceived = !(rxIsInFailsafeMode || rxFrameDropped);
 488                 if (signalReceived) {
 489                     needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
 490                 }
 491
 492                 setLinkQuality(signalReceived, currentDeltaTimeUs);
 493             }
 494
 495             if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) {
 496                 auxiliaryProcessingRequired = true;
 497             }
 498         }
 499
 500         break;
 501     }
 502
 503     if (signalReceived) {
 504         rxSignalReceived = true;
 505     } else if (currentTimeUs >= needRxSignalBefore) {
 506         rxSignalReceived = false;
 507     }
 508
 509     if ((signalReceived && useDataDrivenProcessing) || cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) {
 510         rxDataProcessingRequired = true;
 511     }
 512
 513     return rxDataProcessingRequired || auxiliaryProcessingRequired; // data driven or 50Hz
 514 }
 515
 516 #if defined(USE_PWM) || defined(USE_PPM)
 517 static uint16_t calculateChannelMovingAverage(uint8_t chan, uint16_t sample)
 518 {
 519     static int16_t rcSamples[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT][PPM_AND_PWM_SAMPLE_COUNT];
 520     static int16_t rcDataMean[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT];
 521     static bool rxSamplesCollected = false;
 522
 523     const uint8_t currentSampleIndex = rcSampleIndex % PPM_AND_PWM_SAMPLE_COUNT;
 524
 525     // update the recent samples and compute the average of them
 526     rcSamples[chan][currentSampleIndex] = sample;
 527
 528     // avoid returning an incorrect average which would otherwise occur before enough samples
 529     if (!rxSamplesCollected) {
 530         if (rcSampleIndex < PPM_AND_PWM_SAMPLE_COUNT) {
 531             return sample;
 532         }
 533         rxSamplesCollected = true;
 534     }
 535
 536     rcDataMean[chan] = 0;
 537     for (int sampleIndex = 0; sampleIndex < PPM_AND_PWM_SAMPLE_COUNT; sampleIndex++) {
 538         rcDataMean[chan] += rcSamples[chan][sampleIndex];
 539     }
 540     return rcDataMean[chan] / PPM_AND_PWM_SAMPLE_COUNT;
 541 }
 542 #endif
 543
 544 static uint16_t getRxfailValue(uint8_t channel)
 545 {
 546     const rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigs(channel);
 547
 548     switch (channelFailsafeConfig->mode) {
 549     case RX_FAILSAFE_MODE_AUTO:
 550         switch (channel) {
 551         case ROLL:
 552         case PITCH:
 553         case YAW:
 554             return rxConfig()->midrc;
 555         case THROTTLE:
 556             if (featureIsEnabled(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3D) && !flight3DConfig()->switched_mode3d) {
 557                 return rxConfig()->midrc;
 558             } else {
 559                 return rxConfig()->rx_min_usec;
 560             }
 561         }
 562
 563     FALLTHROUGH;
 564     default:
 565     case RX_FAILSAFE_MODE_INVALID:
 566     case RX_FAILSAFE_MODE_HOLD:
 567         return rcData[channel];
 568
 569     case RX_FAILSAFE_MODE_SET:
 570         return RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfig->step);
 571     }
 572 }
 573
 574 STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range)
 575 {
 576     // Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT
 577     if (sample == PPM_RCVR_TIMEOUT) {
 578         return PPM_RCVR_TIMEOUT;
 579     }
 580
 581     sample = scaleRange(sample, range->min, range->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
 582     sample = constrain(sample, PWM_PULSE_MIN, PWM_PULSE_MAX);
 583
 584     return sample;
 585 }
 586
 587 static void readRxChannelsApplyRanges(void)
 588 {
 589     for (int channel = 0; channel < rxChannelCount; channel++) {
 590
 591         const uint8_t rawChannel = channel < RX_MAPPABLE_CHANNEL_COUNT ? rxConfig()->rcmap[channel] : channel;
 592
 593         // sample the channel
 594         uint16_t sample = rxRuntimeState.rcReadRawFn(&rxRuntimeState, rawChannel);
 595
 596         // apply the rx calibration
 597         if (channel < NON_AUX_CHANNEL_COUNT) {
 598             sample = applyRxChannelRangeConfiguraton(sample, rxChannelRangeConfigs(channel));
 599         }
 600
 601         rcRaw[channel] = sample;
 602     }
 603 }
 604
 605 static void detectAndApplySignalLossBehaviour(void)
 606 {
 607     const uint32_t currentTimeMs = millis();
 608
 609     const bool useValueFromRx = rxSignalReceived && !rxIsInFailsafeMode;
 610
 611     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 0, rxSignalReceived);
 612     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, rxIsInFailsafeMode);
 613
 614     rxFlightChannelsValid = true;
 615     for (int channel = 0; channel < rxChannelCount; channel++) {
 616         uint16_t sample = rcRaw[channel];
 617
 618         const bool validPulse = useValueFromRx && isPulseValid(sample);
 619
 620         if (validPulse) {
 621             rcInvalidPulsPeriod[channel] = currentTimeMs + MAX_INVALID_PULS_TIME;
 622         } else {
 623             if (cmp32(currentTimeMs, rcInvalidPulsPeriod[channel]) < 0) {
 624                 continue;           // skip to next channel to hold channel value MAX_INVALID_PULS_TIME
 625             } else {
 626                 sample = getRxfailValue(channel);   // after that apply rxfail value
 627                 if (channel < NON_AUX_CHANNEL_COUNT) {
 628                     rxFlightChannelsValid = false;
 629                 }
 630             }
 631         }
 632 #if defined(USE_PWM) || defined(USE_PPM)
 633         if (rxRuntimeState.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeState.rxProvider == RX_PROVIDER_PPM) {
 634             // smooth output for PWM and PPM
 635             rcData[channel] = calculateChannelMovingAverage(channel, sample);
 636         } else
 637 #endif
 638         {
 639             rcData[channel] = sample;
 640         }
 641     }
 642
 643     if (rxFlightChannelsValid && !IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
 644         failsafeOnValidDataReceived();
 645     } else {
 646         rxIsInFailsafeMode = true;
 647         failsafeOnValidDataFailed();
 648         for (int channel = 0; channel < rxChannelCount; channel++) {
 649             rcData[channel] = getRxfailValue(channel);
 650         }
 651     }
 652     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 3, rcData[THROTTLE]);
 653 }
 654
 655 bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
 656 {
 657     if (auxiliaryProcessingRequired) {
 658         auxiliaryProcessingRequired = !rxRuntimeState.rcProcessFrameFn(&rxRuntimeState);
 659     }
 660
 661     if (!rxDataProcessingRequired) {
 662         return false;
 663     }
 664
 665     rxDataProcessingRequired = false;
 666     rxNextUpdateAtUs = currentTimeUs + DELAY_33_HZ;
 667
 668     // only proceed when no more samples to skip and suspend period is over
 669     if (skipRxSamples || currentTimeUs <= suspendRxSignalUntil) {
 670         if (currentTimeUs > suspendRxSignalUntil) {
 671             skipRxSamples--;
 672         }
 673
 674         return true;
 675     }
 676
 677     readRxChannelsApplyRanges();
 678     detectAndApplySignalLossBehaviour();
 679
 680     rcSampleIndex++;
 681
 682     return true;
 683 }
 684
 685 void parseRcChannels(const char *input, rxConfig_t *rxConfig)
 686 {
 687     for (const char *c = input; *c; c++) {
 688         const char *s = strchr(rcChannelLetters, *c);
 689         if (s && (s < rcChannelLetters + RX_MAPPABLE_CHANNEL_COUNT)) {
 690             rxConfig->rcmap[s - rcChannelLetters] = c - input;
 691         }
 692     }
 693 }
 694
 695 void setRssiDirect(uint16_t newRssi, rssiSource_e source)
 696 {
 697     if (source != rssiSource) {
 698         return;
 699     }
 700
 701     rssi = newRssi;
 702 }
 703
 704 #define RSSI_SAMPLE_COUNT 16
 705
 706 static uint16_t updateRssiSamples(uint16_t value)
 707 {
 708     static uint16_t samples[RSSI_SAMPLE_COUNT];
 709     static uint8_t sampleIndex = 0;
 710     static unsigned sum = 0;
 711
 712     sum += value - samples[sampleIndex];
 713     samples[sampleIndex] = value;
 714     sampleIndex = (sampleIndex + 1) % RSSI_SAMPLE_COUNT;
 715     return sum / RSSI_SAMPLE_COUNT;
 716 }
 717
 718 void setRssi(uint16_t rssiValue, rssiSource_e source)
 719 {
 720     if (source != rssiSource) {
 721         return;
 722     }
 723
 724     // Filter RSSI value
 725     if (source == RSSI_SOURCE_FRAME_ERRORS) {
 726         rssi = pt1FilterApply(&frameErrFilter, rssiValue);
 727     } else {
 728         // calculate new sample mean
 729         rssi = updateRssiSamples(rssiValue);
 730     }
 731 }
 732
 733 void setRssiMsp(uint8_t newMspRssi)
 734 {
 735     if (rssiSource == RSSI_SOURCE_NONE) {
 736         rssiSource = RSSI_SOURCE_MSP;
 737     }
 738
 739     if (rssiSource == RSSI_SOURCE_MSP) {
 740         rssi = ((uint16_t)newMspRssi) << 2;
 741         lastMspRssiUpdateUs = micros();
 742     }
 743 }
 744
 745 static void updateRSSIPWM(void)
 746 {
 747     // Read value of AUX channel as rssi
 748     int16_t pwmRssi = rcData[rxConfig()->rssi_channel - 1];
 749
 750     // Range of rawPwmRssi is [1000;2000]. rssi should be in [0;1023];
 751     setRssiDirect(scaleRange(pwmRssi, PWM_RANGE_MIN, PWM_RANGE_MAX, 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     setRssi(rssiValue, RSSI_SOURCE_ADC);
 770 #endif
 771 }
 772
 773 void updateRSSI(timeUs_t currentTimeUs)
 774 {
 775     switch (rssiSource) {
 776     case RSSI_SOURCE_RX_CHANNEL:
 777         updateRSSIPWM();
 778         break;
 779     case RSSI_SOURCE_ADC:
 780         updateRSSIADC(currentTimeUs);
 781         break;
 782     case RSSI_SOURCE_MSP:
 783         if (cmpTimeUs(micros(), lastMspRssiUpdateUs) > MSP_RSSI_TIMEOUT_US) {
 784             rssi = 0;
 785         }
 786         break;
 787     default:
 788         break;
 789     }
 790 }
 791
 792 uint16_t getRssi(void)
 793 {
 794     uint16_t rssiValue = rssi;
 795
 796     // RSSI_Invert option
 797     if (rxConfig()->rssi_invert) {
 798         rssiValue = RSSI_MAX_VALUE - rssiValue;
 799     }
 800
 801     return rxConfig()->rssi_scale / 100.0f * rssiValue + rxConfig()->rssi_offset * RSSI_OFFSET_SCALING;
 802 }
 803
 804 uint8_t getRssiPercent(void)
 805 {
 806     return scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 100);
 807 }
 808
 809 int16_t getRssiDbm(void)
 810 {
 811     return rssiDbm;
 812 }
 813
 814 #define RSSI_SAMPLE_COUNT_DBM 16
 815
 816 static int16_t updateRssiDbmSamples(int16_t value)
 817 {
 818     static int16_t samplesdbm[RSSI_SAMPLE_COUNT_DBM];
 819     static uint8_t sampledbmIndex = 0;
 820     static int sumdbm = 0;
 821
 822     sumdbm += value - samplesdbm[sampledbmIndex];
 823     samplesdbm[sampledbmIndex] = value;
 824     sampledbmIndex = (sampledbmIndex + 1) % RSSI_SAMPLE_COUNT_DBM;
 825     return sumdbm / RSSI_SAMPLE_COUNT_DBM;
 826 }
 827
 828 void setRssiDbm(int16_t rssiDbmValue, rssiSource_e source)
 829 {
 830     if (source != rssiSource) {
 831         return;
 832     }
 833
 834     rssiDbm = updateRssiDbmSamples(rssiDbmValue);
 835 }
 836
 837 void setRssiDbmDirect(int16_t newRssiDbm, rssiSource_e source)
 838 {
 839     if (source != rssiSource) {
 840         return;
 841     }
 842
 843     rssiDbm = newRssiDbm;
 844 }
 845
 846 #ifdef USE_RX_LINK_QUALITY_INFO
 847 uint16_t rxGetLinkQuality(void)
 848 {
 849     return linkQuality;
 850 }
 851
 852 uint8_t rxGetRfMode(void)
 853 {
 854     return rfMode;
 855 }
 856
 857 uint16_t rxGetLinkQualityPercent(void)
 858 {
 859     return (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) ?  linkQuality : scaleRange(linkQuality, 0, LINK_QUALITY_MAX_VALUE, 0, 100);
 860 }
 861 #endif
 862
 863 uint16_t rxGetRefreshRate(void)
 864 {
 865     return rxRuntimeState.rxRefreshRate;
 866 }
 867
 868 bool isRssiConfigured(void)
 869 {
 870     return rssiSource != RSSI_SOURCE_NONE;
 871 }
 872
 873 timeDelta_t rxGetFrameDelta(timeDelta_t *frameAgeUs)
 874 {
 875     static timeUs_t previousFrameTimeUs = 0;
 876     static timeDelta_t frameTimeDeltaUs = 0;
 877
 878     if (rxRuntimeState.rcFrameTimeUsFn) {
 879         const timeUs_t frameTimeUs = rxRuntimeState.rcFrameTimeUsFn();
 880
 881         *frameAgeUs = cmpTimeUs(micros(), frameTimeUs);
 882
 883         const timeDelta_t deltaUs = cmpTimeUs(frameTimeUs, previousFrameTimeUs);
 884         if (deltaUs) {
 885             frameTimeDeltaUs = deltaUs;
 886             previousFrameTimeUs = frameTimeUs;
 887         }
 888     }
 889
 890     return frameTimeDeltaUs;
 891 }