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