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