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