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