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 (where stick values should remain)
 657             if (channel < NON_AUX_CHANNEL_COUNT) {
 658                 if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
 659                     if (channel == THROTTLE ) {
 660                         sample = failsafeConfig()->failsafe_throttle;
 661                     } else {
 662                         sample = rxConfig()->midrc;
 663                     }
 664                 }
 665             } else if (!failsafeAuxSwitch) {
 666                 // aux channels as Set in Configurator, unless failsafe initiated by switch
 667                 sample = getRxfailValue(channel);
 668             }
 669         } else {
 670             if (failsafeAuxSwitch) {
 671                 if (channel < NON_AUX_CHANNEL_COUNT) {
 672                     sample = getRxfailValue(channel);
 673                     //  set RPYT values to Stage 1 values immediately if initiated by switch
 674                 }
 675             } else if (!thisChannelValid) {
 676                 if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) {
 677                     //  HOLD bad channel/s for MAX_INVALID_PULSE_TIME_MS (300ms) after Rx loss
 678                 } else {
 679                     //  then use STAGE 1 failsafe values
 680                     if (channel < NON_AUX_CHANNEL_COUNT) {
 681                         allAuxChannelsAreGood = false;
 682                         //  declare signal lost after 300ms of at least one bad flight channel
 683                     }
 684                     sample = getRxfailValue(channel);
 685                     //  set all channels to Stage 1 values
 686                 }
 687             }
 688         }
 689
 690 #if defined(USE_PWM) || defined(USE_PPM)
 691         if (rxRuntimeState.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeState.rxProvider == RX_PROVIDER_PPM) {
 692             //  smooth output for PWM and PPM using moving average
 693             rcData[channel] = calculateChannelMovingAverage(channel, sample);
 694         } else
 695 #endif
 696
 697         {
 698             //  set rcData to either clean raw incoming values, or failsafe-modified values
 699             rcData[channel] = sample;
 700         }
 701     }
 702
 703     if (rxFlightChannelsValid && allAuxChannelsAreGood) {
 704         failsafeOnValidDataReceived();
 705         //  --> start the timer to exit stage 2 failsafe
 706     } else {
 707         failsafeOnValidDataFailed();
 708         //  -> start timer to enter stage2 failsafe
 709     }
 710
 711     DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 3, rcData[THROTTLE]);
 712 }
 713
 714 bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
 715 {
 716     if (auxiliaryProcessingRequired) {
 717         auxiliaryProcessingRequired = !rxRuntimeState.rcProcessFrameFn(&rxRuntimeState);
 718     }
 719
 720     if (!rxDataProcessingRequired) {
 721         return false;
 722     }
 723
 724     rxDataProcessingRequired = false;
 725
 726     // only proceed when no more samples to skip and suspend period is over
 727     if (skipRxSamples || currentTimeUs <= suspendRxSignalUntil) {
 728         if (currentTimeUs > suspendRxSignalUntil) {
 729             skipRxSamples--;
 730         }
 731
 732         return true;
 733     }
 734
 735     readRxChannelsApplyRanges();            // returns rcRaw
 736     detectAndApplySignalLossBehaviour();    // returns rcData
 737
 738     rcSampleIndex++;
 739
 740     return true;
 741 }
 742
 743 void parseRcChannels(const char *input, rxConfig_t *rxConfig)
 744 {
 745     for (const char *c = input; *c; c++) {
 746         const char *s = strchr(rcChannelLetters, *c);
 747         if (s && (s < rcChannelLetters + RX_MAPPABLE_CHANNEL_COUNT)) {
 748             rxConfig->rcmap[s - rcChannelLetters] = c - input;
 749         }
 750     }
 751 }
 752
 753 void setRssiDirect(uint16_t newRssi, rssiSource_e source)
 754 {
 755     if (source != rssiSource) {
 756         return;
 757     }
 758
 759     rssi = newRssi;
 760 }
 761
 762 #define RSSI_SAMPLE_COUNT 16
 763
 764 static uint16_t updateRssiSamples(uint16_t value)
 765 {
 766     static uint16_t samples[RSSI_SAMPLE_COUNT];
 767     static uint8_t sampleIndex = 0;
 768     static unsigned sum = 0;
 769
 770     sum += value - samples[sampleIndex];
 771     samples[sampleIndex] = value;
 772     sampleIndex = (sampleIndex + 1) % RSSI_SAMPLE_COUNT;
 773     return sum / RSSI_SAMPLE_COUNT;
 774 }
 775
 776 void setRssi(uint16_t rssiValue, rssiSource_e source)
 777 {
 778     if (source != rssiSource) {
 779         return;
 780     }
 781
 782     // Filter RSSI value
 783     if (source == RSSI_SOURCE_FRAME_ERRORS) {
 784         rssi = pt1FilterApply(&frameErrFilter, rssiValue);
 785     } else {
 786         // calculate new sample mean
 787         rssi = updateRssiSamples(rssiValue);
 788     }
 789 }
 790
 791 void setRssiMsp(uint8_t newMspRssi)
 792 {
 793     if (rssiSource == RSSI_SOURCE_NONE) {
 794         rssiSource = RSSI_SOURCE_MSP;
 795     }
 796
 797     if (rssiSource == RSSI_SOURCE_MSP) {
 798         rssi = ((uint16_t)newMspRssi) << 2;
 799         lastMspRssiUpdateUs = micros();
 800     }
 801 }
 802
 803 static void updateRSSIPWM(void)
 804 {
 805     // Read value of AUX channel as rssi
 806     int16_t pwmRssi = rcData[rxConfig()->rssi_channel - 1];
 807
 808     // Range of rawPwmRssi is [1000;2000]. rssi should be in [0;1023];
 809     setRssiDirect(scaleRange(constrain(pwmRssi, PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_CHANNEL);
 810 }
 811
 812 static void updateRSSIADC(timeUs_t currentTimeUs)
 813 {
 814 #ifndef USE_ADC
 815     UNUSED(currentTimeUs);
 816 #else
 817     static uint32_t rssiUpdateAt = 0;
 818
 819     if ((int32_t)(currentTimeUs - rssiUpdateAt) < 0) {
 820         return;
 821     }
 822     rssiUpdateAt = currentTimeUs + DELAY_20_MS;
 823
 824     const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
 825     uint16_t rssiValue = adcRssiSample / RSSI_ADC_DIVISOR;
 826
 827     setRssi(rssiValue, RSSI_SOURCE_ADC);
 828 #endif
 829 }
 830
 831 void updateRSSI(timeUs_t currentTimeUs)
 832 {
 833     switch (rssiSource) {
 834     case RSSI_SOURCE_RX_CHANNEL:
 835         updateRSSIPWM();
 836         break;
 837     case RSSI_SOURCE_ADC:
 838         updateRSSIADC(currentTimeUs);
 839         break;
 840     case RSSI_SOURCE_MSP:
 841         if (cmpTimeUs(micros(), lastMspRssiUpdateUs) > DELAY_1500_MS) {  // 1.5s
 842             rssi = 0;
 843         }
 844         break;
 845     default:
 846         break;
 847     }
 848 }
 849
 850 uint16_t getRssi(void)
 851 {
 852     uint16_t rssiValue = rssi;
 853
 854     // RSSI_Invert option
 855     if (rxConfig()->rssi_invert) {
 856         rssiValue = RSSI_MAX_VALUE - rssiValue;
 857     }
 858
 859     return rxConfig()->rssi_scale / 100.0f * rssiValue + rxConfig()->rssi_offset * RSSI_OFFSET_SCALING;
 860 }
 861
 862 uint8_t getRssiPercent(void)
 863 {
 864     return scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 100);
 865 }
 866
 867 int16_t getRssiDbm(void)
 868 {
 869     return rssiDbm;
 870 }
 871
 872 #define RSSI_SAMPLE_COUNT_DBM 16
 873
 874 static int16_t updateRssiDbmSamples(int16_t value)
 875 {
 876     static int16_t samplesdbm[RSSI_SAMPLE_COUNT_DBM];
 877     static uint8_t sampledbmIndex = 0;
 878     static int sumdbm = 0;
 879
 880     sumdbm += value - samplesdbm[sampledbmIndex];
 881     samplesdbm[sampledbmIndex] = value;
 882     sampledbmIndex = (sampledbmIndex + 1) % RSSI_SAMPLE_COUNT_DBM;
 883     return sumdbm / RSSI_SAMPLE_COUNT_DBM;
 884 }
 885
 886 void setRssiDbm(int16_t rssiDbmValue, rssiSource_e source)
 887 {
 888     if (source != rssiSource) {
 889         return;
 890     }
 891
 892     rssiDbm = updateRssiDbmSamples(rssiDbmValue);
 893 }
 894
 895 void setRssiDbmDirect(int16_t newRssiDbm, rssiSource_e source)
 896 {
 897     if (source != rssiSource) {
 898         return;
 899     }
 900
 901     rssiDbm = newRssiDbm;
 902 }
 903
 904 #ifdef USE_RX_LINK_QUALITY_INFO
 905 uint16_t rxGetLinkQuality(void)
 906 {
 907     return linkQuality;
 908 }
 909
 910 uint8_t rxGetRfMode(void)
 911 {
 912     return rfMode;
 913 }
 914
 915 uint16_t rxGetLinkQualityPercent(void)
 916 {
 917     return (linkQualitySource == LQ_SOURCE_NONE) ? scaleRange(linkQuality, 0, LINK_QUALITY_MAX_VALUE, 0, 100) : linkQuality;
 918 }
 919 #endif
 920
 921 #ifdef USE_RX_LINK_UPLINK_POWER
 922 uint16_t rxGetUplinkTxPwrMw(void)
 923 {
 924     return uplinkTxPwrMw;
 925 }
 926 #endif
 927
 928 uint16_t rxGetRefreshRate(void)
 929 {
 930     return rxRuntimeState.rxRefreshRate;
 931 }
 932
 933 bool isRssiConfigured(void)
 934 {
 935     return rssiSource != RSSI_SOURCE_NONE;
 936 }
 937
 938 timeDelta_t rxGetFrameDelta(timeDelta_t *frameAgeUs)
 939 {
 940     static timeUs_t previousFrameTimeUs = 0;
 941     static timeDelta_t frameTimeDeltaUs = 0;
 942
 943     if (rxRuntimeState.rcFrameTimeUsFn) {
 944         const timeUs_t frameTimeUs = rxRuntimeState.rcFrameTimeUsFn();
 945
 946         *frameAgeUs = cmpTimeUs(micros(), frameTimeUs);
 947
 948         const timeDelta_t deltaUs = cmpTimeUs(frameTimeUs, previousFrameTimeUs);
 949         if (deltaUs) {
 950             frameTimeDeltaUs = deltaUs;
 951             previousFrameTimeUs = frameTimeUs;
 952         }
 953     }
 954
 955     return frameTimeDeltaUs;
 956 }
 957
 958 timeUs_t rxFrameTimeUs(void)
 959 {
 960     return rxRuntimeState.lastRcFrameTimeUs;
 961 }