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