src/main/rx/rx.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24
  25 #include <string.h>
  26
  27 #include "platform.h"
  28
  29 #include "build/build_config.h"
  30 #include "build/debug.h"
  31
  32 #include "common/maths.h"
  33 #include "common/utils.h"
  34 #include "common/filter.h"
  35
  36 #include "config/config.h"
  37 #include "config/config_reset.h"
  38 #include "config/feature.h"
  39
  40 #include "drivers/adc.h"
  41 #include "drivers/rx/rx_pwm.h"
  42 #include "drivers/time.h"
  43
  44 #include "fc/rc_controls.h"
  45 #include "fc/rc_modes.h"
  46 #include "fc/runtime_config.h"
  47 #include "fc/tasks.h"
  48
  49 #include "flight/failsafe.h"
  50
  51 #include "io/serial.h"
  52
  53 #include "pg/pg.h"
  54 #include "pg/pg_ids.h"
  55 #include "pg/rx.h"
  56
  57 #include "rx/rx.h"
  58 #include "rx/pwm.h"
  59 #include "rx/fport.h"
  60 #include "rx/sbus.h"
  61 #include "rx/spektrum.h"
  62 #include "rx/srxl2.h"
  63 #include "rx/sumd.h"
  64 #include "rx/sumh.h"
  65 #include "rx/msp.h"
  66 #include "rx/xbus.h"
  67 #include "rx/ibus.h"
  68 #include "rx/jetiexbus.h"
  69 #include "rx/crsf.h"
  70 #include "rx/ghst.h"
  71 #include "rx/rx_spi.h"
  72 #include "rx/targetcustomserial.h"
  73 #include "rx/msp_override.h"
  74
  75
  76 const char rcChannelLetters[] = "AERT12345678abcdefgh";
  77
  78 static uint16_t rssi = 0;                  // range: [0;1023]
  79 static uint16_t rssiRaw = 0;               // range: [0;1023]
  80 static timeUs_t lastRssiSmoothingUs = 0;
  81 #ifdef USE_RX_RSSI_DBM
  82 static int8_t activeAntenna;
  83 static int16_t rssiDbm = CRSF_RSSI_MIN;    // range: [-130,0]
  84 static int16_t rssiDbmRaw = CRSF_RSSI_MIN; // range: [-130,0]
  85 #endif //USE_RX_RSSI_DBM
  86 #ifdef USE_RX_RSNR
  87 static int16_t rsnr = CRSF_SNR_MIN;        // range: [-30,20]
  88 static int16_t rsnrRaw = CRSF_SNR_MIN;     // range: [-30,20]
  89 #endif //USE_RX_RSNR
  90 static timeUs_t lastMspRssiUpdateUs = 0;
  91
  92 static pt1Filter_t frameErrFilter;
  93 static pt1Filter_t rssiFilter;
  94 #ifdef USE_RX_RSSI_DBM
  95 static pt1Filter_t rssiDbmFilter;
  96 #endif //USE_RX_RSSI_DBM
  97 #ifdef USE_RX_RSNR
  98 static pt1Filter_t rsnrFilter;
  99 #endif //USE_RX_RSNR
 100
 101 #ifdef USE_RX_LINK_QUALITY_INFO
 102 static uint16_t linkQuality = 0;
 103 static uint8_t rfMode = 0;
 104 #endif
 105
 106 #ifdef USE_RX_LINK_UPLINK_POWER
 107 static uint16_t uplinkTxPwrMw = 0;  //Uplink Tx power in mW
 108 #endif
 109
 110 #define RSSI_ADC_DIVISOR (4096 / 1024)
 111 #define RSSI_OFFSET_SCALING (1024 / 100.0f)
 112
 113 rssiSource_e rssiSource;
 114 linkQualitySource_e linkQualitySource;
 115
 116 static bool rxDataProcessingRequired = false;
 117 static bool auxiliaryProcessingRequired = false;
 118
 119 static bool rxSignalReceived = false;
 120 static bool rxFlightChannelsValid = false;
 121 static uint8_t rxChannelCount;
 122
 123 static timeUs_t needRxSignalBefore = 0;
 124 static timeUs_t suspendRxSignalUntil = 0;
 125 static uint8_t  skipRxSamples = 0;
 126
 127 static float rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // last received raw value, as it comes
 128 float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];           // scaled, modified, checked and constrained values
 129 uint32_t validRxSignalTimeout[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 130
 131 #define MAX_INVALID_PULSE_TIME_MS 300                   // hold time in milliseconds after bad channel or Rx link loss
 132 // will not be actioned until the nearest multiple of 100ms
 133 #define PPM_AND_PWM_SAMPLE_COUNT 3
 134
 135 #define DELAY_20_MS (20 * 1000)                         // 20ms in us
 136 #define DELAY_100_MS (100 * 1000)                       // 100ms 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;
 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 rxIsReceivingSignal(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 signalReceived = false;
 506     bool useDataDrivenProcessing = true;
 507     timeDelta_t needRxSignalMaxDelayUs = DELAY_100_MS;
 508
 509     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 2, MIN(2000, currentDeltaTimeUs / 100));
 510
 511     if (taskUpdateRxMainInProgress()) {
 512         //  no need to check for new data as a packet is being processed already
 513         return;
 514     }
 515
 516     switch (rxRuntimeState.rxProvider) {
 517     default:
 518
 519         break;
 520 #if defined(USE_RX_PWM) || defined(USE_RX_PPM)
 521     case RX_PROVIDER_PPM:
 522         if (isPPMDataBeingReceived()) {
 523             signalReceived = true;
 524             resetPPMDataReceivedState();
 525         }
 526
 527         break;
 528     case RX_PROVIDER_PARALLEL_PWM:
 529         if (isPWMDataBeingReceived()) {
 530             signalReceived = true;
 531             useDataDrivenProcessing = false;
 532         }
 533
 534         break;
 535 #endif
 536     case RX_PROVIDER_SERIAL:
 537     case RX_PROVIDER_MSP:
 538     case RX_PROVIDER_SPI:
 539     case RX_PROVIDER_UDP:
 540         {
 541             const uint8_t frameStatus = rxRuntimeState.rcFrameStatusFn(&rxRuntimeState);
 542             DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, (frameStatus & RX_FRAME_FAILSAFE));
 543             signalReceived = (frameStatus & RX_FRAME_COMPLETE) && !(frameStatus & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED));
 544             setLinkQuality(signalReceived, currentDeltaTimeUs);
 545             auxiliaryProcessingRequired |= (frameStatus & RX_FRAME_PROCESSING_REQUIRED);
 546         }
 547
 548         break;
 549     }
 550
 551     if (signalReceived) {
 552         //  true only when a new packet arrives
 553         needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
 554         rxSignalReceived = true; // immediately process packet data
 555         if (useDataDrivenProcessing) {
 556             rxDataProcessingRequired = true;
 557             //  process the new Rx packet when it arrives
 558         }
 559     } else {
 560         //  watch for next packet
 561         if (cmpTimeUs(currentTimeUs, needRxSignalBefore) > 0) {
 562             //  initial time to signalReceived failure is 100ms, then we check every 100ms
 563             rxSignalReceived = false;
 564             needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
 565             //  review and process rcData values every 100ms in case failsafe changed them
 566             rxDataProcessingRequired = true;
 567         }
 568     }
 569
 570 #if defined(USE_RX_MSP_OVERRIDE)
 571     if (IS_RC_MODE_ACTIVE(BOXMSPOVERRIDE) && rxConfig()->msp_override_channels_mask && rxConfig()->msp_override_failsafe) {
 572         if (rxMspOverrideFrameStatus() & RX_FRAME_COMPLETE) {
 573             rxSignalReceived = true;
 574             rxDataProcessingRequired = true;
 575             needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
 576         }
 577     }
 578 #endif
 579
 580     DEBUG_SET(DEBUG_FAILSAFE, 1, rxSignalReceived);
 581     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 0, rxSignalReceived);
 582 }
 583
 584 #if defined(USE_RX_PWM) || defined(USE_RX_PPM)
 585 static uint16_t calculateChannelMovingAverage(uint8_t chan, uint16_t sample)
 586 {
 587     static int16_t rcSamples[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT][PPM_AND_PWM_SAMPLE_COUNT];
 588     static int16_t rcDataMean[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT];
 589     static bool rxSamplesCollected = false;
 590
 591     const uint8_t currentSampleIndex = rcSampleIndex % PPM_AND_PWM_SAMPLE_COUNT;
 592
 593     // update the recent samples and compute the average of them
 594     rcSamples[chan][currentSampleIndex] = sample;
 595
 596     // avoid returning an incorrect average which would otherwise occur before enough samples
 597     if (!rxSamplesCollected) {
 598         if (rcSampleIndex < PPM_AND_PWM_SAMPLE_COUNT) {
 599             return sample;
 600         }
 601         rxSamplesCollected = true;
 602     }
 603
 604     rcDataMean[chan] = 0;
 605     for (int sampleIndex = 0; sampleIndex < PPM_AND_PWM_SAMPLE_COUNT; sampleIndex++) {
 606         rcDataMean[chan] += rcSamples[chan][sampleIndex];
 607     }
 608     return rcDataMean[chan] / PPM_AND_PWM_SAMPLE_COUNT;
 609 }
 610 #endif
 611
 612 static uint16_t getRxfailValue(uint8_t channel)
 613 {
 614     const rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigs(channel);
 615     const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
 616
 617     switch (channelFailsafeConfig->mode) {
 618     case RX_FAILSAFE_MODE_AUTO:
 619         switch (channel) {
 620         case ROLL:
 621         case PITCH:
 622         case YAW:
 623             return rxConfig()->midrc;
 624         case THROTTLE:
 625             if (featureIsEnabled(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3D) && !flight3DConfig()->switched_mode3d) {
 626                 return rxConfig()->midrc;
 627             } else {
 628                 return rxConfig()->rx_min_usec;
 629             }
 630         }
 631
 632     FALLTHROUGH;
 633     default:
 634     case RX_FAILSAFE_MODE_INVALID:
 635     case RX_FAILSAFE_MODE_HOLD:
 636         if (boxFailsafeSwitchIsOn) {
 637             return rcRaw[channel]; // current values are allowed through on held channels with switch induced failsafe
 638         } else {
 639             return rcData[channel]; // last good value
 640         }
 641     case RX_FAILSAFE_MODE_SET:
 642         return RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfig->step);
 643     }
 644 }
 645
 646 STATIC_UNIT_TESTED float applyRxChannelRangeConfiguraton(float sample, const rxChannelRangeConfig_t *range)
 647 {
 648     // Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT
 649     if (sample == PPM_RCVR_TIMEOUT) {
 650         return PPM_RCVR_TIMEOUT;
 651     }
 652
 653     sample = scaleRangef(sample, range->min, range->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
 654     // out of range channel values are now constrained after the validity check in detectAndApplySignalLossBehaviour()
 655     return sample;
 656 }
 657
 658 static void readRxChannelsApplyRanges(void)
 659 {
 660     for (int channel = 0; channel < rxChannelCount; channel++) {
 661
 662         const uint8_t rawChannel = channel < RX_MAPPABLE_CHANNEL_COUNT ? rxConfig()->rcmap[channel] : channel;
 663
 664         // sample the channel
 665         float sample;
 666 #if defined(USE_RX_MSP_OVERRIDE)
 667         if (rxConfig()->msp_override_channels_mask) {
 668             sample = rxMspOverrideReadRawRc(&rxRuntimeState, rxConfig(), rawChannel);
 669         } else
 670 #endif
 671         {
 672             sample = rxRuntimeState.rcReadRawFn(&rxRuntimeState, rawChannel);
 673         }
 674
 675         // apply the rx calibration
 676         if (channel < NON_AUX_CHANNEL_COUNT) {
 677             sample = applyRxChannelRangeConfiguraton(sample, rxChannelRangeConfigs(channel));
 678         }
 679
 680         rcRaw[channel] = sample;
 681     }
 682 }
 683
 684 void detectAndApplySignalLossBehaviour(void)
 685 {
 686     const uint32_t currentTimeMs = millis();
 687     const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
 688     rxFlightChannelsValid = rxSignalReceived && !boxFailsafeSwitchIsOn;
 689     // rxFlightChannelsValid is false after 100ms of no packets, or as soon as use the BOXFAILSAFE switch is actioned
 690     // rxFlightChannelsValid is true the instant we get a good packet or the BOXFAILSAFE switch is reverted
 691     // can also go false with good packets but where one flight channel is bad > 300ms (PPM type receiver error)
 692
 693     for (int channel = 0; channel < rxChannelCount; channel++) {
 694         float sample = rcRaw[channel]; // sample has latest RC value, rcData has last 'accepted valid' value
 695         const bool thisChannelValid = rxFlightChannelsValid && isPulseValid(sample);
 696         // if the whole packet is bad, or BOXFAILSAFE switch is actioned, consider all channels bad
 697         if (thisChannelValid) {
 698             //  reset the invalid pulse period timer for every good channel
 699             validRxSignalTimeout[channel] = currentTimeMs + MAX_INVALID_PULSE_TIME_MS;
 700         }
 701
 702         if (failsafeIsActive()) {
 703             // we are in failsafe Stage 2, whether Rx loss or BOXFAILSAFE induced
 704             // pass valid incoming flight channel values to FC,
 705             // so that GPS Rescue can get the 30% requirement for termination of the rescue
 706             if (channel < NON_AUX_CHANNEL_COUNT) {
 707                 if (!thisChannelValid) {
 708                     if (channel == THROTTLE ) {
 709                         sample = failsafeConfig()->failsafe_throttle;
 710                         // stage 2 failsafe throttle value. In GPS Rescue Flight mode, gpsRescueGetThrottle overrides, late in mixer.c
 711                     } else {
 712                         sample = rxConfig()->midrc;
 713                     }
 714                 }
 715             } else {
 716                 // set aux channels as per Stage 1 failsafe hold/set values, allow all for Failsafe and GPS rescue MODE switches
 717                 sample = getRxfailValue(channel);
 718             }
 719         } else {
 720             // we are normal, or in failsafe stage 1
 721             if (boxFailsafeSwitchIsOn) {
 722                 // BOXFAILSAFE active, but not in stage 2 yet, use stage 1 values
 723                 sample = getRxfailValue(channel);
 724                 //  set channels to Stage 1 values immediately failsafe switch is activated
 725             } else if (!thisChannelValid) {
 726                 // everything is normal but this channel was invalid
 727                 if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) {
 728                     // first 300ms of Stage 1 failsafe
 729                     sample = rcData[channel];
 730                     //  HOLD last valid value on bad channel/s for MAX_INVALID_PULSE_TIME_MS (300ms)
 731                 } else {
 732                     // remaining Stage 1 failsafe period after 300ms
 733                     if (channel < NON_AUX_CHANNEL_COUNT) {
 734                         rxFlightChannelsValid = false;
 735                         //  declare signal lost after 300ms of any one bad flight channel
 736                     }
 737                     sample = getRxfailValue(channel);
 738                     // set channels that are invalid for more than 300ms to Stage 1 values
 739                 }
 740             }
 741             // everything is normal, ie rcData[channel] will be set to rcRaw(channel) via 'sample'
 742         }
 743
 744         sample = constrainf(sample, PWM_PULSE_MIN, PWM_PULSE_MAX);
 745
 746 #if defined(USE_RX_PWM) || defined(USE_RX_PPM)
 747         if (rxRuntimeState.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeState.rxProvider == RX_PROVIDER_PPM) {
 748             //  smooth output for PWM and PPM using moving average
 749             rcData[channel] = calculateChannelMovingAverage(channel, sample);
 750         } else
 751 #endif
 752
 753         {
 754             //  set rcData to either validated incoming values, or failsafe-modified values
 755             rcData[channel] = sample;
 756         }
 757     }
 758
 759     if (rxFlightChannelsValid) {
 760         failsafeOnValidDataReceived();
 761         //  --> start the timer to exit stage 2 failsafe 100ms after losing all packets or the BOXFAILSAFE switch is actioned
 762     } else {
 763         failsafeOnValidDataFailed();
 764         //  -> start stage 1 timer to enter stage2 failsafe the instant we get a good packet or the BOXFAILSAFE switch is reverted
 765     }
 766
 767     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 3, rcData[THROTTLE]);
 768 }
 769
 770 bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
 771 {
 772     if (auxiliaryProcessingRequired) {
 773         rxRuntimeState.rcProcessFrameFn(&rxRuntimeState);
 774         auxiliaryProcessingRequired = false;
 775     }
 776
 777     if (!rxDataProcessingRequired) {
 778         return false;
 779     }
 780
 781     rxDataProcessingRequired = false;
 782
 783     // only proceed when no more samples to skip and suspend period is over
 784     if (skipRxSamples || currentTimeUs <= suspendRxSignalUntil) {
 785         if (currentTimeUs > suspendRxSignalUntil) {
 786             skipRxSamples--;
 787         }
 788
 789         return true;
 790     }
 791
 792     readRxChannelsApplyRanges();            // returns rcRaw
 793     detectAndApplySignalLossBehaviour();    // returns rcData
 794
 795     rcSampleIndex++;
 796
 797     return true;
 798 }
 799
 800 void parseRcChannels(const char *input, rxConfig_t *rxConfig)
 801 {
 802     for (const char *c = input; *c; c++) {
 803         const char *s = strchr(rcChannelLetters, *c);
 804         if (s && (s < rcChannelLetters + RX_MAPPABLE_CHANNEL_COUNT)) {
 805             rxConfig->rcmap[s - rcChannelLetters] = c - input;
 806         }
 807     }
 808 }
 809
 810 void setRssiDirect(uint16_t newRssi, rssiSource_e source)
 811 {
 812     if (source != rssiSource) {
 813         return;
 814     }
 815
 816     rssi = newRssi;
 817     rssiRaw = newRssi;
 818 }
 819
 820 void setRssi(uint16_t rssiValue, rssiSource_e source)
 821 {
 822     if (source != rssiSource) {
 823         return;
 824     }
 825
 826     // Filter RSSI value
 827     if (source == RSSI_SOURCE_FRAME_ERRORS) {
 828         rssiRaw = pt1FilterApply(&frameErrFilter, rssiValue);
 829     } else {
 830         rssiRaw = rssiValue;
 831     }
 832 }
 833
 834 void setRssiMsp(uint8_t newMspRssi)
 835 {
 836     if (rssiSource == RSSI_SOURCE_NONE) {
 837         rssiSource = RSSI_SOURCE_MSP;
 838     }
 839
 840     if (rssiSource == RSSI_SOURCE_MSP) {
 841         rssi = ((uint16_t)newMspRssi) << 2;
 842         lastMspRssiUpdateUs = micros();
 843     }
 844 }
 845
 846 static void updateRSSIPWM(void)
 847 {
 848     // Read value of AUX channel as rssi
 849     int16_t pwmRssi = rcData[rxConfig()->rssi_channel - 1];
 850
 851     // Range of rawPwmRssi is [1000;2000]. rssi should be in [0;1023];
 852     setRssiDirect(scaleRange(constrain(pwmRssi, PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_CHANNEL);
 853 }
 854
 855 static void updateRSSIADC(timeUs_t currentTimeUs)
 856 {
 857 #ifndef USE_ADC
 858     UNUSED(currentTimeUs);
 859 #else
 860     static uint32_t rssiUpdateAt = 0;
 861
 862     if ((int32_t)(currentTimeUs - rssiUpdateAt) < 0) {
 863         return;
 864     }
 865     rssiUpdateAt = currentTimeUs + DELAY_20_MS;
 866
 867     const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
 868     uint16_t rssiValue = adcRssiSample / RSSI_ADC_DIVISOR;
 869
 870     setRssi(rssiValue, RSSI_SOURCE_ADC);
 871 #endif
 872 }
 873
 874 void updateRSSI(timeUs_t currentTimeUs)
 875 {
 876     switch (rssiSource) {
 877     case RSSI_SOURCE_RX_CHANNEL:
 878         updateRSSIPWM();
 879         break;
 880     case RSSI_SOURCE_ADC:
 881         updateRSSIADC(currentTimeUs);
 882         break;
 883     case RSSI_SOURCE_MSP:
 884         if (cmpTimeUs(micros(), lastMspRssiUpdateUs) > DELAY_1500_MS) {  // 1.5s
 885             rssi = 0;
 886         }
 887         break;
 888     default:
 889         break;
 890     }
 891
 892     if (cmpTimeUs(currentTimeUs, lastRssiSmoothingUs) > 250000) { // 0.25s
 893         lastRssiSmoothingUs = currentTimeUs;
 894     } else {
 895         if (lastRssiSmoothingUs != currentTimeUs) { // avoid div by 0
 896             float k = (256.0f - rxConfig()->rssi_smoothing) / 256.0f;
 897             float factor = ((currentTimeUs - lastRssiSmoothingUs) / 1000000.0f) / (1.0f / 4.0f);
 898             float k2  = (k * factor) / ((k * factor) - k + 1);
 899
 900             if (rssi != rssiRaw) {
 901                 pt1FilterUpdateCutoff(&rssiFilter, k2);
 902                 rssi = pt1FilterApply(&rssiFilter, rssiRaw);
 903             }
 904
 905 #ifdef USE_RX_RSSI_DBM
 906             if (rssiDbm != rssiDbmRaw) {
 907                 pt1FilterUpdateCutoff(&rssiDbmFilter, k2);
 908                 rssiDbm = pt1FilterApply(&rssiDbmFilter, rssiDbmRaw);
 909             }
 910 #endif //USE_RX_RSSI_DBM
 911
 912 #ifdef USE_RX_RSNR
 913             if (rsnr != rsnrRaw) {
 914                 pt1FilterUpdateCutoff(&rsnrFilter, k2);
 915                 rsnr = pt1FilterApply(&rsnrFilter, rsnrRaw);
 916             }
 917 #endif //USE_RX_RSNR
 918
 919             lastRssiSmoothingUs = currentTimeUs;
 920         }
 921     }
 922 }
 923
 924 uint16_t getRssi(void)
 925 {
 926     uint16_t rssiValue = rssi;
 927
 928     // RSSI_Invert option
 929     if (rxConfig()->rssi_invert) {
 930         rssiValue = RSSI_MAX_VALUE - rssiValue;
 931     }
 932
 933     return rxConfig()->rssi_scale / 100.0f * rssiValue + rxConfig()->rssi_offset * RSSI_OFFSET_SCALING;
 934 }
 935
 936 uint8_t getRssiPercent(void)
 937 {
 938     return scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 100);
 939 }
 940
 941 #ifdef USE_RX_RSSI_DBM
 942 int16_t getRssiDbm(void)
 943 {
 944     return rssiDbm;
 945 }
 946
 947 void setRssiDbm(int16_t rssiDbmValue, rssiSource_e source)
 948 {
 949     if (source != rssiSource) {
 950         return;
 951     }
 952
 953     rssiDbmRaw = rssiDbmValue;
 954 }
 955
 956 void setRssiDbmDirect(int16_t newRssiDbm, rssiSource_e source)
 957 {
 958     if (source != rssiSource) {
 959         return;
 960     }
 961
 962     rssiDbm = newRssiDbm;
 963     rssiDbmRaw = newRssiDbm;
 964 }
 965
 966 int8_t getActiveAntenna(void)
 967 {
 968     return activeAntenna;
 969 }
 970
 971 void setActiveAntenna(int8_t antenna)
 972 {
 973     activeAntenna = antenna;
 974 }
 975
 976 #endif //USE_RX_RSSI_DBM
 977
 978 #ifdef USE_RX_RSNR
 979 int16_t getRsnr(void)
 980 {
 981     return rsnr;
 982 }
 983
 984 void setRsnr(int16_t rsnrValue)
 985 {
 986     rsnrRaw = rsnrValue;
 987 }
 988
 989 void setRsnrDirect(int16_t newRsnr)
 990 {
 991     rsnr = newRsnr;
 992     rsnrRaw = newRsnr;
 993 }
 994 #endif //USE_RX_RSNR
 995
 996 #ifdef USE_RX_LINK_QUALITY_INFO
 997 uint16_t rxGetLinkQuality(void)
 998 {
 999     return linkQuality;
1000 }
1001
1002 uint8_t rxGetRfMode(void)
1003 {
1004     return rfMode;
1005 }
1006
1007 uint16_t rxGetLinkQualityPercent(void)
1008 {
1009     return (linkQualitySource == LQ_SOURCE_NONE) ? scaleRange(linkQuality, 0, LINK_QUALITY_MAX_VALUE, 0, 100) : linkQuality;
1010 }
1011 #endif
1012
1013 #ifdef USE_RX_LINK_UPLINK_POWER
1014 uint16_t rxGetUplinkTxPwrMw(void)
1015 {
1016     return uplinkTxPwrMw;
1017 }
1018 #endif
1019
1020 bool isRssiConfigured(void)
1021 {
1022     return rssiSource != RSSI_SOURCE_NONE;
1023 }
1024
1025 timeDelta_t rxGetFrameDelta(timeDelta_t *frameAgeUs)
1026 {
1027     static timeUs_t previousFrameTimeUs = 0;
1028     static timeDelta_t frameTimeDeltaUs = 0;
1029
1030     if (rxRuntimeState.rcFrameTimeUsFn) {
1031         const timeUs_t frameTimeUs = rxRuntimeState.rcFrameTimeUsFn();
1032
1033         *frameAgeUs = cmpTimeUs(micros(), frameTimeUs);
1034
1035         const timeDelta_t deltaUs = cmpTimeUs(frameTimeUs, previousFrameTimeUs);
1036         if (deltaUs) {
1037             frameTimeDeltaUs = deltaUs;
1038             previousFrameTimeUs = frameTimeUs;
1039         }
1040     }
1041
1042     return frameTimeDeltaUs;
1043 }
1044
1045 timeUs_t rxFrameTimeUs(void)
1046 {
1047     return rxRuntimeState.lastRcFrameTimeUs;
1048 }