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