src/main/rx/rx.c

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