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
  35 #include "config/config_reset.h"
  36 #include "config/feature.h"
  37
  38 #include "drivers/adc.h"
  39 #include "drivers/rx/rx_pwm.h"
  40 #include "drivers/rx/rx_spi.h"
  41 #include "drivers/time.h"
  42
  43 #include "fc/config.h"
  44 #include "fc/rc_controls.h"
  45 #include "fc/rc_modes.h"
  46
  47 #include "flight/failsafe.h"
  48
  49 #include "io/serial.h"
  50
  51 #include "pg/pg.h"
  52 #include "pg/pg_ids.h"
  53
  54 #include "rx/rx.h"
  55 #include "rx/pwm.h"
  56 #include "rx/fport.h"
  57 #include "rx/sbus.h"
  58 #include "rx/spektrum.h"
  59 #include "rx/sumd.h"
  60 #include "rx/sumh.h"
  61 #include "rx/msp.h"
  62 #include "rx/xbus.h"
  63 #include "rx/ibus.h"
  64 #include "rx/jetiexbus.h"
  65 #include "rx/crsf.h"
  66 #include "rx/rx_spi.h"
  67 #include "rx/targetcustomserial.h"
  68
  69
  70 //#define DEBUG_RX_SIGNAL_LOSS
  71
  72 const char rcChannelLetters[] = "AERT12345678abcdefgh";
  73
  74 static uint16_t rssi = 0;                  // range: [0;1023]
  75 static timeUs_t lastMspRssiUpdateUs = 0;
  76
  77 #define MSP_RSSI_TIMEOUT_US 1500000   // 1.5 sec
  78
  79 rssiSource_e rssiSource;
  80
  81 static bool rxDataProcessingRequired = false;
  82 static bool auxiliaryProcessingRequired = false;
  83
  84 static bool rxSignalReceived = false;
  85 static bool rxFlightChannelsValid = false;
  86 static bool rxIsInFailsafeMode = true;
  87 static uint8_t rxChannelCount;
  88
  89 static timeUs_t rxNextUpdateAtUs = 0;
  90 static uint32_t needRxSignalBefore = 0;
  91 static uint32_t needRxSignalMaxDelayUs;
  92 static uint32_t suspendRxSignalUntil = 0;
  93 static uint8_t  skipRxSamples = 0;
  94
  95 static int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
  96 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
  97 uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
  98
  99 #define MAX_INVALID_PULS_TIME    300
 100 #define PPM_AND_PWM_SAMPLE_COUNT 3
 101
 102 #define DELAY_50_HZ (1000000 / 50)
 103 #define DELAY_10_HZ (1000000 / 10)
 104 #define DELAY_5_HZ (1000000 / 5)
 105 #define SKIP_RC_ON_SUSPEND_PERIOD 1500000           // 1.5 second period in usec (call frequency independent)
 106 #define SKIP_RC_SAMPLES_ON_RESUME  2                // flush 2 samples to drop wrong measurements (timing independent)
 107
 108 rxRuntimeConfig_t rxRuntimeConfig;
 109 static uint8_t rcSampleIndex = 0;
 110
 111 #ifndef RX_SPI_DEFAULT_PROTOCOL
 112 #define RX_SPI_DEFAULT_PROTOCOL 0
 113 #endif
 114 #ifndef SERIALRX_PROVIDER
 115 #define SERIALRX_PROVIDER 0
 116 #endif
 117
 118 #define RX_MIN_USEC 885
 119 #define RX_MAX_USEC 2115
 120 #define RX_MID_USEC 1500
 121
 122 #ifndef SPEKTRUM_BIND_PIN
 123 #define SPEKTRUM_BIND_PIN NONE
 124 #endif
 125
 126 #ifndef BINDPLUG_PIN
 127 #define BINDPLUG_PIN NONE
 128 #endif
 129
 130 PG_REGISTER_WITH_RESET_FN(rxConfig_t, rxConfig, PG_RX_CONFIG, 2);
 131 void pgResetFn_rxConfig(rxConfig_t *rxConfig)
 132 {
 133     RESET_CONFIG_2(rxConfig_t, rxConfig,
 134         .halfDuplex = 0,
 135         .serialrx_provider = SERIALRX_PROVIDER,
 136         .rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL,
 137         .serialrx_inverted = 0,
 138         .spektrum_bind_pin_override_ioTag = IO_TAG(SPEKTRUM_BIND_PIN),
 139         .spektrum_bind_plug_ioTag = IO_TAG(BINDPLUG_PIN),
 140         .spektrum_sat_bind = 0,
 141         .spektrum_sat_bind_autoreset = 1,
 142         .midrc = RX_MID_USEC,
 143         .mincheck = 1050,
 144         .maxcheck = 1900,
 145         .rx_min_usec = RX_MIN_USEC,          // any of first 4 channels below this value will trigger rx loss detection
 146         .rx_max_usec = RX_MAX_USEC,         // any of first 4 channels above this value will trigger rx loss detection
 147         .rssi_src_frame_errors = false,
 148         .rssi_channel = 0,
 149         .rssi_scale = RSSI_SCALE_DEFAULT,
 150         .rssi_invert = 0,
 151         .rcInterpolation = RC_SMOOTHING_AUTO,
 152         .rcInterpolationChannels = 0,
 153         .rcInterpolationInterval = 19,
 154         .fpvCamAngleDegrees = 0,
 155         .airModeActivateThreshold = 32,
 156         .max_aux_channel = DEFAULT_AUX_CHANNEL_COUNT
 157     );
 158
 159 #ifdef RX_CHANNELS_TAER
 160     parseRcChannels("TAER1234", rxConfig);
 161 #else
 162     parseRcChannels("AETR1234", rxConfig);
 163 #endif
 164 }
 165
 166 PG_REGISTER_ARRAY_WITH_RESET_FN(rxChannelRangeConfig_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs, PG_RX_CHANNEL_RANGE_CONFIG, 0);
 167 void pgResetFn_rxChannelRangeConfigs(rxChannelRangeConfig_t *rxChannelRangeConfigs)
 168 {
 169     // set default calibration to full range and 1:1 mapping
 170     for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) {
 171         rxChannelRangeConfigs[i].min = PWM_RANGE_MIN;
 172         rxChannelRangeConfigs[i].max = PWM_RANGE_MAX;
 173     }
 174 }
 175
 176 PG_REGISTER_ARRAY_WITH_RESET_FN(rxFailsafeChannelConfig_t, MAX_SUPPORTED_RC_CHANNEL_COUNT, rxFailsafeChannelConfigs, PG_RX_FAILSAFE_CHANNEL_CONFIG, 0);
 177 void pgResetFn_rxFailsafeChannelConfigs(rxFailsafeChannelConfig_t *rxFailsafeChannelConfigs)
 178 {
 179     for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
 180         rxFailsafeChannelConfigs[i].mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD;
 181         rxFailsafeChannelConfigs[i].step = (i == THROTTLE)
 182             ? CHANNEL_VALUE_TO_RXFAIL_STEP(RX_MIN_USEC)
 183             : CHANNEL_VALUE_TO_RXFAIL_STEP(RX_MID_USEC);
 184     }
 185 }
 186
 187 void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig) {
 188     // set default calibration to full range and 1:1 mapping
 189     for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) {
 190         rxChannelRangeConfig->min = PWM_RANGE_MIN;
 191         rxChannelRangeConfig->max = PWM_RANGE_MAX;
 192         rxChannelRangeConfig++;
 193     }
 194 }
 195
 196 static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel)
 197 {
 198     UNUSED(rxRuntimeConfig);
 199     UNUSED(channel);
 200
 201     return PPM_RCVR_TIMEOUT;
 202 }
 203
 204 static uint8_t nullFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 205 {
 206     UNUSED(rxRuntimeConfig);
 207
 208     return RX_FRAME_PENDING;
 209 }
 210
 211 static bool nullProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
 212 {
 213     UNUSED(rxRuntimeConfig);
 214
 215     return true;
 216 }
 217
 218 STATIC_UNIT_TESTED bool isPulseValid(uint16_t pulseDuration)
 219 {
 220     return  pulseDuration >= rxConfig()->rx_min_usec &&
 221             pulseDuration <= rxConfig()->rx_max_usec;
 222 }
 223
 224 #ifdef USE_SERIAL_RX
 225 bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 226 {
 227     bool enabled = false;
 228     switch (rxConfig->serialrx_provider) {
 229 #ifdef USE_SERIALRX_SPEKTRUM
 230     case SERIALRX_SRXL:
 231     case SERIALRX_SPEKTRUM1024:
 232     case SERIALRX_SPEKTRUM2048:
 233         enabled = spektrumInit(rxConfig, rxRuntimeConfig);
 234         break;
 235 #endif
 236 #ifdef USE_SERIALRX_SBUS
 237     case SERIALRX_SBUS:
 238         enabled = sbusInit(rxConfig, rxRuntimeConfig);
 239         break;
 240 #endif
 241 #ifdef USE_SERIALRX_SUMD
 242     case SERIALRX_SUMD:
 243         enabled = sumdInit(rxConfig, rxRuntimeConfig);
 244         break;
 245 #endif
 246 #ifdef USE_SERIALRX_SUMH
 247     case SERIALRX_SUMH:
 248         enabled = sumhInit(rxConfig, rxRuntimeConfig);
 249         break;
 250 #endif
 251 #ifdef USE_SERIALRX_XBUS
 252     case SERIALRX_XBUS_MODE_B:
 253     case SERIALRX_XBUS_MODE_B_RJ01:
 254         enabled = xBusInit(rxConfig, rxRuntimeConfig);
 255         break;
 256 #endif
 257 #ifdef USE_SERIALRX_IBUS
 258     case SERIALRX_IBUS:
 259         enabled = ibusInit(rxConfig, rxRuntimeConfig);
 260         break;
 261 #endif
 262 #ifdef USE_SERIALRX_JETIEXBUS
 263     case SERIALRX_JETIEXBUS:
 264         enabled = jetiExBusInit(rxConfig, rxRuntimeConfig);
 265         break;
 266 #endif
 267 #ifdef USE_SERIALRX_CRSF
 268     case SERIALRX_CRSF:
 269         enabled = crsfRxInit(rxConfig, rxRuntimeConfig);
 270         break;
 271 #endif
 272 #ifdef USE_SERIALRX_TARGET_CUSTOM
 273     case SERIALRX_TARGET_CUSTOM:
 274         enabled = targetCustomSerialRxInit(rxConfig, rxRuntimeConfig);
 275         break;
 276 #endif
 277 #ifdef USE_SERIALRX_FPORT
 278     case SERIALRX_FPORT:
 279         enabled = fportRxInit(rxConfig, rxRuntimeConfig);
 280         break;
 281 #endif
 282     default:
 283         enabled = false;
 284         break;
 285     }
 286     return enabled;
 287 }
 288 #endif
 289
 290 void rxInit(void)
 291 {
 292     rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
 293     rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
 294     rxRuntimeConfig.rcProcessFrameFn = nullProcessFrame;
 295     rcSampleIndex = 0;
 296     needRxSignalMaxDelayUs = DELAY_10_HZ;
 297
 298     for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
 299         rcData[i] = rxConfig()->midrc;
 300         rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME;
 301     }
 302
 303     rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec;
 304
 305     // Initialize ARM switch to OFF position when arming via switch is defined
 306     // TODO - move to rc_mode.c
 307     for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 308         const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(i);
 309         if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
 310             // ARM switch is defined, determine an OFF value
 311             uint16_t value;
 312             if (modeActivationCondition->range.startStep > 0) {
 313                 value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationCondition->range.startStep - 1));
 314             } else {
 315                 value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationCondition->range.endStep + 1));
 316             }
 317             // Initialize ARM AUX channel to OFF value
 318             rcData[modeActivationCondition->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = value;
 319         }
 320     }
 321
 322 #ifdef USE_SERIAL_RX
 323     if (feature(FEATURE_RX_SERIAL)) {
 324         const bool enabled = serialRxInit(rxConfig(), &rxRuntimeConfig);
 325         if (!enabled) {
 326             featureClear(FEATURE_RX_SERIAL);
 327             rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
 328             rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
 329         }
 330     }
 331 #endif
 332
 333 #ifdef USE_RX_MSP
 334     if (feature(FEATURE_RX_MSP)) {
 335         rxMspInit(rxConfig(), &rxRuntimeConfig);
 336         needRxSignalMaxDelayUs = DELAY_5_HZ;
 337     }
 338 #endif
 339
 340 #ifdef USE_RX_SPI
 341     if (feature(FEATURE_RX_SPI)) {
 342         const bool enabled = rxSpiInit(rxConfig(), &rxRuntimeConfig);
 343         if (!enabled) {
 344             featureClear(FEATURE_RX_SPI);
 345             rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
 346             rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
 347         }
 348     }
 349 #endif
 350
 351 #if defined(USE_PWM) || defined(USE_PPM)
 352     if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) {
 353         rxPwmInit(rxConfig(), &rxRuntimeConfig);
 354     }
 355 #endif
 356
 357 #if defined(USE_ADC)
 358     if (feature(FEATURE_RSSI_ADC)) {
 359         rssiSource = RSSI_SOURCE_ADC;
 360     } else
 361 #endif
 362     if (rxConfig()->rssi_channel > 0) {
 363         rssiSource = RSSI_SOURCE_RX_CHANNEL;
 364     }
 365
 366     rxChannelCount = MIN(rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT, rxRuntimeConfig.channelCount);
 367 }
 368
 369 bool rxIsReceivingSignal(void)
 370 {
 371     return rxSignalReceived;
 372 }
 373
 374 bool rxAreFlightChannelsValid(void)
 375 {
 376     return rxFlightChannelsValid;
 377 }
 378
 379 void suspendRxSignal(void)
 380 {
 381     suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
 382     skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
 383     failsafeOnRxSuspend(SKIP_RC_ON_SUSPEND_PERIOD);
 384 }
 385
 386 void resumeRxSignal(void)
 387 {
 388     suspendRxSignalUntil = micros();
 389     skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
 390     failsafeOnRxResume();
 391 }
 392
 393 bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
 394 {
 395     UNUSED(currentDeltaTime);
 396
 397     bool signalReceived = false;
 398     bool useDataDrivenProcessing = true;
 399
 400 #if defined(USE_PWM) || defined(USE_PPM)
 401     if (feature(FEATURE_RX_PPM)) {
 402         if (isPPMDataBeingReceived()) {
 403             signalReceived = true;
 404             rxIsInFailsafeMode = false;
 405             needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
 406             resetPPMDataReceivedState();
 407         }
 408     } else if (feature(FEATURE_RX_PARALLEL_PWM)) {
 409         if (isPWMDataBeingReceived()) {
 410             signalReceived = true;
 411             rxIsInFailsafeMode = false;
 412             needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
 413             useDataDrivenProcessing = false;
 414         }
 415     } else
 416 #endif
 417     {
 418         const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
 419         if (frameStatus & RX_FRAME_COMPLETE) {
 420             rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
 421             bool rxFrameDropped = (frameStatus & RX_FRAME_DROPPED) != 0;
 422             signalReceived = !(rxIsInFailsafeMode || rxFrameDropped);
 423             if (signalReceived) {
 424                 needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
 425             }
 426
 427             if (frameStatus & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED)) {
 428                 // No (0%) signal
 429                 setRssiUnfiltered(0, RSSI_SOURCE_FRAME_ERRORS);
 430             } else {
 431                 // Valid (100%) signal
 432                 setRssiUnfiltered(RSSI_MAX_VALUE, RSSI_SOURCE_FRAME_ERRORS);
 433             }
 434         }
 435
 436         if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) {
 437             auxiliaryProcessingRequired = true;
 438         }
 439     }
 440
 441     if (signalReceived) {
 442         rxSignalReceived = true;
 443     } else if (currentTimeUs >= needRxSignalBefore) {
 444         rxSignalReceived = false;
 445     }
 446
 447     if ((signalReceived && useDataDrivenProcessing) || cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) {
 448         rxDataProcessingRequired = true;
 449     }
 450
 451     return rxDataProcessingRequired || auxiliaryProcessingRequired; // data driven or 50Hz
 452 }
 453
 454 static uint16_t calculateChannelMovingAverage(uint8_t chan, uint16_t sample)
 455 {
 456     static int16_t rcSamples[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT][PPM_AND_PWM_SAMPLE_COUNT];
 457     static int16_t rcDataMean[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT];
 458     static bool rxSamplesCollected = false;
 459
 460     const uint8_t currentSampleIndex = rcSampleIndex % PPM_AND_PWM_SAMPLE_COUNT;
 461
 462     // update the recent samples and compute the average of them
 463     rcSamples[chan][currentSampleIndex] = sample;
 464
 465     // avoid returning an incorrect average which would otherwise occur before enough samples
 466     if (!rxSamplesCollected) {
 467         if (rcSampleIndex < PPM_AND_PWM_SAMPLE_COUNT) {
 468             return sample;
 469         }
 470         rxSamplesCollected = true;
 471     }
 472
 473     rcDataMean[chan] = 0;
 474     for (int sampleIndex = 0; sampleIndex < PPM_AND_PWM_SAMPLE_COUNT; sampleIndex++) {
 475         rcDataMean[chan] += rcSamples[chan][sampleIndex];
 476     }
 477     return rcDataMean[chan] / PPM_AND_PWM_SAMPLE_COUNT;
 478 }
 479
 480 static uint16_t getRxfailValue(uint8_t channel)
 481 {
 482     const rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigs(channel);
 483
 484     switch (channelFailsafeConfig->mode) {
 485     case RX_FAILSAFE_MODE_AUTO:
 486         switch (channel) {
 487         case ROLL:
 488         case PITCH:
 489         case YAW:
 490             return rxConfig()->midrc;
 491         case THROTTLE:
 492             if (feature(FEATURE_3D))
 493                 return rxConfig()->midrc;
 494             else
 495                 return rxConfig()->rx_min_usec;
 496         }
 497         /* no break */
 498
 499     default:
 500     case RX_FAILSAFE_MODE_INVALID:
 501     case RX_FAILSAFE_MODE_HOLD:
 502         return rcData[channel];
 503
 504     case RX_FAILSAFE_MODE_SET:
 505         return RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfig->step);
 506     }
 507 }
 508
 509 STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range)
 510 {
 511     // Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT
 512     if (sample == PPM_RCVR_TIMEOUT) {
 513         return PPM_RCVR_TIMEOUT;
 514     }
 515
 516     sample = scaleRange(sample, range->min, range->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
 517     sample = constrain(sample, PWM_PULSE_MIN, PWM_PULSE_MAX);
 518
 519     return sample;
 520 }
 521
 522 static void readRxChannelsApplyRanges(void)
 523 {
 524     for (int channel = 0; channel < rxChannelCount; channel++) {
 525
 526         const uint8_t rawChannel = channel < RX_MAPPABLE_CHANNEL_COUNT ? rxConfig()->rcmap[channel] : channel;
 527
 528         // sample the channel
 529         uint16_t sample = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, rawChannel);
 530
 531         // apply the rx calibration
 532         if (channel < NON_AUX_CHANNEL_COUNT) {
 533             sample = applyRxChannelRangeConfiguraton(sample, rxChannelRangeConfigs(channel));
 534         }
 535
 536         rcRaw[channel] = sample;
 537     }
 538 }
 539
 540 static void detectAndApplySignalLossBehaviour(void)
 541 {
 542     const uint32_t currentTimeMs = millis();
 543
 544     const bool useValueFromRx = rxSignalReceived && !rxIsInFailsafeMode;
 545
 546 #ifdef DEBUG_RX_SIGNAL_LOSS
 547     debug[0] = rxSignalReceived;
 548     debug[1] = rxIsInFailsafeMode;
 549     debug[2] = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, 0);
 550 #endif
 551
 552     rxFlightChannelsValid = true;
 553     for (int channel = 0; channel < rxChannelCount; channel++) {
 554         uint16_t sample = rcRaw[channel];
 555
 556         const bool validPulse = useValueFromRx && isPulseValid(sample);
 557
 558         if (validPulse) {
 559             rcInvalidPulsPeriod[channel] = currentTimeMs + MAX_INVALID_PULS_TIME;
 560         } else {
 561             if (cmp32(currentTimeMs, rcInvalidPulsPeriod[channel]) < 0) {
 562                 continue;           // skip to next channel to hold channel value MAX_INVALID_PULS_TIME
 563             } else {
 564                 sample = getRxfailValue(channel);   // after that apply rxfail value
 565                 if (channel < NON_AUX_CHANNEL_COUNT) {
 566                     rxFlightChannelsValid = false;
 567                 }
 568             }
 569         }
 570         if (feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) {
 571             // smooth output for PWM and PPM
 572             rcData[channel] = calculateChannelMovingAverage(channel, sample);
 573         } else {
 574             rcData[channel] = sample;
 575         }
 576     }
 577
 578     if (rxFlightChannelsValid && !IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
 579         failsafeOnValidDataReceived();
 580     } else {
 581         rxIsInFailsafeMode = true;
 582         failsafeOnValidDataFailed();
 583         for (int channel = 0; channel < rxChannelCount; channel++) {
 584             rcData[channel] = getRxfailValue(channel);
 585         }
 586     }
 587
 588 #ifdef DEBUG_RX_SIGNAL_LOSS
 589     debug[3] = rcData[THROTTLE];
 590 #endif
 591 }
 592
 593 bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
 594 {
 595     if (auxiliaryProcessingRequired) {
 596         auxiliaryProcessingRequired = !rxRuntimeConfig.rcProcessFrameFn(&rxRuntimeConfig);
 597     }
 598
 599     if (!rxDataProcessingRequired) {
 600         return false;
 601     }
 602
 603     rxDataProcessingRequired = false;
 604     rxNextUpdateAtUs = currentTimeUs + DELAY_50_HZ;
 605
 606     // only proceed when no more samples to skip and suspend period is over
 607     if (skipRxSamples) {
 608         if (currentTimeUs > suspendRxSignalUntil) {
 609             skipRxSamples--;
 610         }
 611
 612         return true;
 613     }
 614
 615     readRxChannelsApplyRanges();
 616     detectAndApplySignalLossBehaviour();
 617
 618     rcSampleIndex++;
 619
 620     return true;
 621 }
 622
 623 void parseRcChannels(const char *input, rxConfig_t *rxConfig)
 624 {
 625     for (const char *c = input; *c; c++) {
 626         const char *s = strchr(rcChannelLetters, *c);
 627         if (s && (s < rcChannelLetters + RX_MAPPABLE_CHANNEL_COUNT)) {
 628             rxConfig->rcmap[s - rcChannelLetters] = c - input;
 629         }
 630     }
 631 }
 632
 633 void setRssiFiltered(uint16_t newRssi, rssiSource_e source)
 634 {
 635     if (source != rssiSource) {
 636         return;
 637     }
 638
 639     rssi = newRssi;
 640 }
 641
 642 #define RSSI_SAMPLE_COUNT 16
 643
 644 void setRssiUnfiltered(uint16_t rssiValue, rssiSource_e source)
 645 {
 646     if (source != rssiSource) {
 647         return;
 648     }
 649
 650     static uint16_t rssiSamples[RSSI_SAMPLE_COUNT];
 651     static uint8_t rssiSampleIndex = 0;
 652     static unsigned sum = 0;
 653
 654     sum = sum + rssiValue;
 655     sum = sum - rssiSamples[rssiSampleIndex];
 656     rssiSamples[rssiSampleIndex] = rssiValue;
 657     rssiSampleIndex = (rssiSampleIndex + 1) % RSSI_SAMPLE_COUNT;
 658
 659     int16_t rssiMean = sum / RSSI_SAMPLE_COUNT;
 660
 661     rssi = rssiMean;
 662 }
 663
 664 void setRssiMsp(uint8_t newMspRssi)
 665 {
 666     if (rssiSource == RSSI_SOURCE_NONE) {
 667         rssiSource = RSSI_SOURCE_MSP;
 668     }
 669
 670     if (rssiSource == RSSI_SOURCE_MSP) {
 671         rssi = ((uint16_t)newMspRssi) << 2;
 672         lastMspRssiUpdateUs = micros();
 673     }
 674 }
 675
 676 static void updateRSSIPWM(void)
 677 {
 678     // Read value of AUX channel as rssi
 679     int16_t pwmRssi = rcData[rxConfig()->rssi_channel - 1];
 680
 681     // RSSI_Invert option
 682     if (rxConfig()->rssi_invert) {
 683         pwmRssi = ((2000 - pwmRssi) + 1000);
 684     }
 685
 686     // Range of rawPwmRssi is [1000;2000]. rssi should be in [0;1023];
 687     setRssiFiltered(constrain((uint16_t)(((pwmRssi - 1000) / 1000.0f) * 1024.0f), 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_CHANNEL);
 688 }
 689
 690 static void updateRSSIADC(timeUs_t currentTimeUs)
 691 {
 692 #ifndef USE_ADC
 693     UNUSED(currentTimeUs);
 694 #else
 695     static uint32_t rssiUpdateAt = 0;
 696
 697     if ((int32_t)(currentTimeUs - rssiUpdateAt) < 0) {
 698         return;
 699     }
 700     rssiUpdateAt = currentTimeUs + DELAY_50_HZ;
 701
 702     const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
 703     uint16_t rssiValue = (uint16_t)((1024.0f * adcRssiSample) / (rxConfig()->rssi_scale * 100.0f));
 704     rssiValue = constrain(rssiValue, 0, RSSI_MAX_VALUE);
 705
 706     // RSSI_Invert option
 707     if (rxConfig()->rssi_invert) {
 708         rssiValue = RSSI_MAX_VALUE - rssiValue;
 709     }
 710
 711     setRssiUnfiltered((uint16_t)rssiValue, RSSI_SOURCE_ADC);
 712 #endif
 713 }
 714
 715 void updateRSSI(timeUs_t currentTimeUs)
 716 {
 717     switch (rssiSource) {
 718     case RSSI_SOURCE_RX_CHANNEL:
 719         updateRSSIPWM();
 720         break;
 721     case RSSI_SOURCE_ADC:
 722         updateRSSIADC(currentTimeUs);
 723         break;
 724     case RSSI_SOURCE_MSP:
 725         if (cmpTimeUs(micros(), lastMspRssiUpdateUs) > MSP_RSSI_TIMEOUT_US) {
 726             rssi = 0;
 727         }
 728         break;
 729     default:
 730         break;
 731     }
 732 }
 733
 734 uint16_t getRssi(void)
 735 {
 736     return rssi;
 737 }
 738
 739 uint16_t rxGetRefreshRate(void)
 740 {
 741     return rxRuntimeConfig.rxRefreshRate;
 742 }