diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 50830f5b32..045d0914da 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -905,7 +905,7 @@ static void loadSlowState(blackboxSlowState_t *slow) memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags; slow->stateFlags = stateFlags; slow->failsafePhase = failsafePhase(); - slow->rxSignalReceived = rxIsReceivingSignal(); + slow->rxSignalReceived = isRxReceivingSignal(); slow->rxFlightChannelsValid = rxAreFlightChannelsValid(); } diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 378d6a0615..18c901babb 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -4770,10 +4770,9 @@ if (buildKey) { // Run status const int gyroRate = getTaskDeltaTimeUs(TASK_GYRO) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTimeUs(TASK_GYRO))); - int rxRate = getCurrentRxIntervalUs(); - if (rxRate != 0) { - rxRate = (int)(1000000.0f / ((float)rxRate)); - } + + int rxRate = getRxRateValid() ? getCurrentRxRateHz() : 0; + const int systemRate = getTaskDeltaTimeUs(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTimeUs(TASK_SYSTEM))); cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d", constrain(getAverageSystemLoadPercent(), 0, LOAD_PERCENTAGE_ONE), getTaskDeltaTimeUs(TASK_GYRO), gyroRate, rxRate, systemRate); @@ -4960,12 +4959,11 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline) if (rxConfig()->rc_smoothing_mode) { cliPrintLine("FILTER"); if (rcSmoothingAutoCalculate()) { - const uint16_t smoothedRxRateHz = lrintf(rcSmoothingData->smoothedRxRateHz); cliPrint("# Detected Rx frequency: "); - if (getCurrentRxIntervalUs() == 0) { - cliPrintLine("NO SIGNAL"); + if (getRxRateValid()) { + cliPrintLinef("%dHz", lrintf(rcSmoothingData->smoothedRxRateHz)); } else { - cliPrintLinef("%dHz", smoothedRxRateHz); + cliPrintLine("NO SIGNAL"); } } cliPrintf("# Active setpoint cutoff: %dhz ", rcSmoothingData->setpointCutoffFrequency); diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 80d555ffed..3b9bcd16b9 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -292,7 +292,7 @@ void updateArmingStatus(void) // If switch is used for arming then check it is not defaulting to on when the RX link recovers from a fault if (!isUsingSticksForArming()) { static bool hadRx = false; - const bool haveRx = rxIsReceivingSignal(); + const bool haveRx = isRxReceivingSignal(); const bool justGotRxBack = !hadRx && haveRx; @@ -762,7 +762,7 @@ bool processRx(timeUs_t currentTimeUs) return false; } - updateRcRefreshRate(currentTimeUs); + updateRcRefreshRate(currentTimeUs, isRxReceivingSignal()); // in 3D mode, we need to be able to disarm by switch at any time if (featureIsEnabled(FEATURE_3D)) { diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index b66fc4818a..72d7bd26e1 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -56,6 +56,8 @@ #include "rc.h" +#define RX_INTERVAL_MIN_US 950 // 0.950ms to fit 1kHz without an issue +#define RX_INTERVAL_MAX_US 65500 // 65.5ms or 15.26hz typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs); // note that rcCommand[] is an external float @@ -68,11 +70,12 @@ static float maxRcDeflectionAbs; static bool reverseMotors = false; static applyRatesFn *applyRates; -static uint16_t currentRxIntervalUs; // packet interval in microseconds -static float currentRxRateHz; // packet rate in hertz +static uint16_t currentRxIntervalUs; // packet interval in microseconds, constrained to above range +static uint16_t previousRxIntervalUs; // previous packet interval in microseconds +static float currentRxRateHz; // packet interval in Hz, constrained as above static bool isRxDataNew = false; -static bool isRxIntervalValid = false; +static bool isRxRateValid = false; static float rcCommandDivider = 500.0f; static float rcCommandYawDivider = 500.0f; @@ -110,9 +113,6 @@ static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData; static float rcDeflectionSmoothed[3]; #endif // USE_RC_SMOOTHING_FILTER -#define RX_INTERVAL_MIN_US 950 // 0.950ms to fit 1kHz without an issue -#define RX_INTERVAL_MAX_US 65500 // 65.5ms or 15.26hz - float getSetpointRate(int axis) { #ifdef USE_RC_SMOOTHING_FILTER @@ -265,32 +265,59 @@ static void scaleRawSetpointToFpvCamAngle(void) rawSetpoint[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, SETPOINT_RATE_LIMIT_MIN, SETPOINT_RATE_LIMIT_MAX); } -void updateRcRefreshRate(timeUs_t currentTimeUs) +void updateRcRefreshRate(timeUs_t currentTimeUs, bool rxReceivingSignal) { - static timeUs_t lastRxTimeUs; + // this function runs from processRx in core.c + // rxReceivingSignal is true: + // - every time a new frame is detected, + // - if we stop getting data, at the expiry of RXLOSS_TRIGGER_INTERVAL since the last good frame + // - if that interval is exceeded and still no data, every RX_FRAME_RECHECK_INTERVAL, until a new frame is detected + static timeUs_t lastRxTimeUs = 0; + timeDelta_t delta = 0; - timeDelta_t frameAgeUs; - timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs); + if (rxReceivingSignal) { // true while receiving data and until RXLOSS_TRIGGER_INTERVAL expires, otherwise false + previousRxIntervalUs = currentRxIntervalUs; + // use driver rx time if available, current time otherwise + const timeUs_t rxTime = rxRuntimeState.lastRcFrameTimeUs ? rxRuntimeState.lastRcFrameTimeUs : currentTimeUs; - if (!frameDeltaUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) { - frameDeltaUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); + if (lastRxTimeUs) { // report delta only if previous time is available + delta = cmpTimeUs(rxTime, lastRxTimeUs); + } + lastRxTimeUs = rxTime; + DEBUG_SET(DEBUG_RX_TIMING, 1, rxTime / 100); // output value in tenths of ms + } else { + if (lastRxTimeUs) { + // no packet received, use current time for delta + delta = cmpTimeUs(currentTimeUs, lastRxTimeUs); + } } - DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX)); - DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX)); + // temporary debugs + DEBUG_SET(DEBUG_RX_TIMING, 4, MIN(delta / 10, INT16_MAX)); // time between frames based on rxFrameCheck +#ifdef USE_RX_LINK_QUALITY_INFO + DEBUG_SET(DEBUG_RX_TIMING, 6, rxGetLinkQualityPercent()); // raw link quality value +#endif + DEBUG_SET(DEBUG_RX_TIMING, 7, isRxReceivingSignal()); // flag to initiate RXLOSS signal and Stage 1 values - lastRxTimeUs = currentTimeUs; - currentRxIntervalUs = constrain(frameDeltaUs, RX_INTERVAL_MIN_US, RX_INTERVAL_MAX_US); - isRxIntervalValid = frameDeltaUs == currentRxIntervalUs; + // constrain to a frequency range no lower than about 15Hz and up to about 1000Hz + // these intervals and rates will be used for RCSmoothing, Feedforward, etc. + currentRxIntervalUs = constrain(delta, RX_INTERVAL_MIN_US, RX_INTERVAL_MAX_US); + currentRxRateHz = 1e6f / currentRxIntervalUs; + isRxRateValid = delta == currentRxIntervalUs; // delta is not constrained, therefore not outside limits - currentRxRateHz = 1e6f / currentRxIntervalUs; // cannot be zero due to preceding constraint - DEBUG_SET(DEBUG_RX_TIMING, 2, isRxIntervalValid); + DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(delta / 10, INT16_MAX)); // output value in hundredths of ms + DEBUG_SET(DEBUG_RX_TIMING, 2, isRxRateValid); DEBUG_SET(DEBUG_RX_TIMING, 3, MIN(currentRxIntervalUs / 10, INT16_MAX)); } -uint16_t getCurrentRxIntervalUs(void) +uint16_t getCurrentRxRateHz(void) { - return currentRxIntervalUs; + return currentRxRateHz; +} + +bool getRxRateValid(void) +{ + return isRxRateValid; } #ifdef USE_RC_SMOOTHING_FILTER @@ -416,19 +443,19 @@ static FAST_CODE void processRcSmoothingFilter(void) const bool ready = (currentTimeMs > 1000) && (targetPidLooptime > 0); if (ready) { // skip during FC initialization // Wait 1000ms after power to let the PID loop stabilize before starting average frame rate calculation - if (rxIsReceivingSignal() && isRxIntervalValid) { + if (isRxReceivingSignal() && isRxRateValid) { - static uint16_t previousRxIntervalUs; if (abs(currentRxIntervalUs - previousRxIntervalUs) < (previousRxIntervalUs - (previousRxIntervalUs / 8))) { // exclude large steps, eg after dropouts or telemetry // by using interval here, we catch a dropout/telemetry where the inteval increases by 100%, but accept // the return to normal value, which is only 50% different from the 100% interval of a single drop, and 66% of a return after a double drop. - static float prevRxRateHz; + static float prevSmoothedRxRateHz; // smooth the current Rx link frequency estimates - const float kF = 0.1f; // first order lowpass smoothing filter coefficient - const float smoothedRxRateHz = prevRxRateHz + kF * (currentRxRateHz - prevRxRateHz); - prevRxRateHz = smoothedRxRateHz; - + const float kF = 0.1f; // first order kind of lowpass smoothing filter coefficient + // add one tenth of the new estimate to the smoothed estimate. + const float smoothedRxRateHz = prevSmoothedRxRateHz + kF * (currentRxRateHz - prevSmoothedRxRateHz); + prevSmoothedRxRateHz = smoothedRxRateHz; + // recalculate cutoffs every 3 acceptable samples if (rcSmoothingData.sampleCount) { rcSmoothingData.sampleCount --; @@ -441,7 +468,6 @@ static FAST_CODE void processRcSmoothingFilter(void) sampleState = 2; } } - previousRxIntervalUs = currentRxIntervalUs; } else { // either we stopped receiving rx samples (failsafe?) or the sample interval is unreasonable // require a full re-evaluation period after signal is restored diff --git a/src/main/fc/rc.h b/src/main/fc/rc.h index 34911c7a6d..75aca335d0 100644 --- a/src/main/fc/rc.h +++ b/src/main/fc/rc.h @@ -46,6 +46,6 @@ bool rcSmoothingInitializationComplete(void); float getMaxRcRate(int axis); float getFeedforward(int axis); -void updateRcRefreshRate(timeUs_t currentTimeUs); -uint16_t getCurrentRxIntervalUs(void); +void updateRcRefreshRate(timeUs_t currentTimeUs, bool rxReceivingSignal); +uint16_t getCurrentRxRateHz(void); bool getRxRateValid(void); diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index ad4cbf0529..04c96c0c2b 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -835,7 +835,7 @@ static void processContinuosAdjustments(controlRateConfig_t *controlRateConfig) void processRcAdjustments(controlRateConfig_t *controlRateConfig) { - const bool canUseRxData = rxIsReceivingSignal(); + const bool canUseRxData = isRxReceivingSignal(); // Recalculate the new active adjustments if required if (stepwiseAdjustmentCount == ADJUSTMENT_RANGE_COUNT_INVALID) { diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 2cc2044438..ce9213d90e 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -149,7 +149,7 @@ static bool failsafeShouldHaveCausedLandingByNow(void) bool failsafeIsReceivingRxData(void) { return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP); - // False with BOXFAILSAFE switch or when no valid packets for 100ms or any flight channel invalid for 300ms, + // False with BOXFAILSAFE switch or when no valid packets for RXLOSS_TRIGGER_INTERVAL or any flight channel invalid for 300ms, // becomes true immediately BOXFAILSAFE switch reverts, or after recovery period expires when valid packets are received // rxLinkState RXLINK_DOWN (not up) is the trigger for the various failsafe stage 2 outcomes. } @@ -184,7 +184,7 @@ void failsafeOnValidDataReceived(void) if (cmp32(failsafeState.validRxDataReceivedAt, failsafeState.validRxDataFailedAt) > (int32_t)failsafeState.receivingRxDataPeriodPreset) { // receivingRxDataPeriodPreset is rxDataRecoveryPeriod unless set to zero to allow immediate control recovery after switch induced failsafe - // rxDataRecoveryPeriod defaults to 1.0s with minimum of PERIOD_RXDATA_RECOVERY (200ms) + // rxDataRecoveryPeriod defaults to 1.0s with minimum of PERIOD_RXDATA_RECOVERY (100ms) // link is not considered 'up', after it has been 'down', until that recovery period has expired failsafeState.rxLinkState = FAILSAFE_RXLINK_UP; // after the rxDataRecoveryPeriod, typically 1s after receiving valid data, clear RXLOSS in OSD and permit arming @@ -193,11 +193,11 @@ void failsafeOnValidDataReceived(void) } void failsafeOnValidDataFailed(void) -// run from rc.c when packets are lost for more than the signal validation period (100ms), or immediately BOXFAILSAFE switch is active +// run from rc.c when packets are lost for more than RXLOSS_TRIGGER_INTERVAL, or immediately BOXFAILSAFE switch is active // after the stage 1 delay has expired, sets the rxLinkState to RXLINK_DOWN, ie not up, causing failsafeIsReceivingRxData to become false // if failsafe is configured to go direct to stage 2, this is emulated immediately in failsafeUpdateState() { - // set RXLOSS in OSD and block arming after 100ms of signal loss + // set RXLOSS in OSD and block arming after RXLOSS_TRIGGER_INTERVAL of frame loss setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); failsafeState.validRxDataFailedAt = millis(); diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index a9e35378e2..3d0277e424 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -425,7 +425,7 @@ static void performSanityChecks(void) // Handle events that set a failure mode to other than healthy. // Disarm via Abort when sanity on, or for hard Rx loss in FS_ONLY mode // Otherwise allow 20s of semi-controlled descent with impact disarm detection - const bool hardFailsafe = !rxIsReceivingSignal(); + const bool hardFailsafe = !isRxReceivingSignal(); if (rescueState.failure != RESCUE_HEALTHY) { // Default to 20s semi-controlled descent with impact detection, then abort diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 8560053ee3..adcbd37339 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -203,7 +203,7 @@ static void updateRxStatus(void) { i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0); char rxStatus = '!'; - if (rxIsReceivingSignal()) { + if (isRxReceivingSignal()) { rxStatus = 'r'; } if (rxAreFlightChannelsValid()) { rxStatus = 'R'; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 0a13d9f7df..fa6deeaafb 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -962,7 +962,7 @@ static void applyLedIndicatorLayer(bool updateNow, timeUs_t *timer) static bool flash = 0; if (updateNow) { - if (rxIsReceivingSignal()) { + if (isRxReceivingSignal()) { // calculate update frequency int scale = MAX(fabsf(rcCommand[ROLL]), fabsf(rcCommand[PITCH])); // 0 - 500 scale = scale - INDICATOR_DEADBAND; // start increasing frequency right after deadband diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index fe167105b9..5eb815b3e6 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -630,7 +630,6 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) rxRuntimeState->channelCount = CRSF_MAX_CHANNEL; rxRuntimeState->rcReadRawFn = crsfReadRawRC; rxRuntimeState->rcFrameStatusFn = crsfFrameStatus; - rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c index 2076781f77..09f4259561 100644 --- a/src/main/rx/fport.c +++ b/src/main/rx/fport.c @@ -394,7 +394,6 @@ bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) rxRuntimeState->channelCount = SBUS_MAX_CHANNEL; rxRuntimeState->rcFrameStatusFn = fportFrameStatus; rxRuntimeState->rcProcessFrameFn = fportProcessFrame; - rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { diff --git a/src/main/rx/ghst.c b/src/main/rx/ghst.c index ad91fd0bd9..cddf725622 100644 --- a/src/main/rx/ghst.c +++ b/src/main/rx/ghst.c @@ -417,7 +417,6 @@ bool ghstRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) rxRuntimeState->channelCount = GHST_MAX_NUM_CHANNELS; rxRuntimeState->rcReadRawFn = ghstReadRawRC; rxRuntimeState->rcFrameStatusFn = ghstFrameStatus; - rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs; rxRuntimeState->rcProcessFrameFn = ghstProcessFrame; for (int iChan = 0; iChan < rxRuntimeState->channelCount; ++iChan) { diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index a80389249a..c39355bb9a 100644 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -211,7 +211,6 @@ bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) rxRuntimeState->channelCount = IBUS_MAX_CHANNEL; rxRuntimeState->rcReadRawFn = ibusReadRawRC; rxRuntimeState->rcFrameStatusFn = ibusFrameStatus; - rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index 77bfbcd7a5..1284b2d587 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -262,7 +262,6 @@ bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) rxRuntimeState->channelCount = JETIEXBUS_CHANNEL_COUNT; rxRuntimeState->rcReadRawFn = jetiExBusReadRawRC; rxRuntimeState->rcFrameStatusFn = jetiExBusFrameStatus; - rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs; jetiExBusFrameReset(); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 8704051e3a..218893f9b3 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -132,8 +132,9 @@ uint32_t validRxSignalTimeout[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // will not be actioned until the nearest multiple of 100ms #define PPM_AND_PWM_SAMPLE_COUNT 3 -#define DELAY_20_MS (20 * 1000) // 20ms in us -#define DELAY_100_MS (100 * 1000) // 100ms in us +#define RSSI_UPDATE_INTERVAL (20 * 1000) // 20ms in us +#define RX_FRAME_RECHECK_INTERVAL (50 * 1000) // 50ms in us +#define RXLOSS_TRIGGER_INTERVAL (150 * 1000) // 150ms in us #define DELAY_1500_MS (1500 * 1000) // 1.5 seconds in us #define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent) @@ -294,7 +295,7 @@ void rxInit(void) rxRuntimeState.rcReadRawFn = nullReadRawRC; rxRuntimeState.rcFrameStatusFn = nullFrameStatus; rxRuntimeState.rcProcessFrameFn = nullProcessFrame; - rxRuntimeState.lastRcFrameTimeUs = 0; + rxRuntimeState.lastRcFrameTimeUs = 0; // zero when driver does not provide timing info rcSampleIndex = 0; uint32_t now = millis(); @@ -396,7 +397,7 @@ void rxInit(void) rxChannelCount = MIN(rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT, rxRuntimeState.channelCount); } -bool rxIsReceivingSignal(void) +bool isRxReceivingSignal(void) { return rxSignalReceived; } @@ -502,9 +503,10 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs) FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs) { - bool signalReceived = false; + bool rxDataReceived = false; bool useDataDrivenProcessing = true; - timeDelta_t needRxSignalMaxDelayUs = DELAY_100_MS; + timeDelta_t needRxSignalMaxDelayUs = RXLOSS_TRIGGER_INTERVAL; + timeDelta_t reCheckRxSignalInterval = RX_FRAME_RECHECK_INTERVAL; DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 2, MIN(2000, currentDeltaTimeUs / 100)); @@ -520,14 +522,14 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current #if defined(USE_RX_PWM) || defined(USE_RX_PPM) case RX_PROVIDER_PPM: if (isPPMDataBeingReceived()) { - signalReceived = true; + rxDataReceived = true; resetPPMDataReceivedState(); } break; case RX_PROVIDER_PARALLEL_PWM: if (isPWMDataBeingReceived()) { - signalReceived = true; + rxDataReceived = true; useDataDrivenProcessing = false; } @@ -540,15 +542,15 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current { const uint8_t frameStatus = rxRuntimeState.rcFrameStatusFn(&rxRuntimeState); DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, (frameStatus & RX_FRAME_FAILSAFE)); - signalReceived = (frameStatus & RX_FRAME_COMPLETE) && !(frameStatus & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED)); - setLinkQuality(signalReceived, currentDeltaTimeUs); + rxDataReceived = (frameStatus & RX_FRAME_COMPLETE) && !(frameStatus & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED)); + setLinkQuality(rxDataReceived, currentDeltaTimeUs); auxiliaryProcessingRequired |= (frameStatus & RX_FRAME_PROCESSING_REQUIRED); } break; } - if (signalReceived) { + if (rxDataReceived) { // true only when a new packet arrives needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; rxSignalReceived = true; // immediately process packet data @@ -559,10 +561,10 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current } else { // watch for next packet if (cmpTimeUs(currentTimeUs, needRxSignalBefore) > 0) { - // initial time to signalReceived failure is 100ms, then we check every 100ms - rxSignalReceived = false; - needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; - // review and process rcData values every 100ms in case failsafe changed them + // initial time to rxDataReceived failure is RXLOSS_TRIGGER_INTERVAL (150ms), + // after that, we check every RX_FRAME_RECHECK_INTERVAL (50ms) + rxSignalReceived = false; // results in `RXLOSS` message etc + needRxSignalBefore += reCheckRxSignalInterval; rxDataProcessingRequired = true; } } @@ -686,7 +688,7 @@ void detectAndApplySignalLossBehaviour(void) const uint32_t currentTimeMs = millis(); const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); rxFlightChannelsValid = rxSignalReceived && !boxFailsafeSwitchIsOn; - // rxFlightChannelsValid is false after 100ms of no packets, or as soon as use the BOXFAILSAFE switch is actioned + // rxFlightChannelsValid is false after RXLOSS_TRIGGER_INTERVAL of no packets, or as soon as use the BOXFAILSAFE switch is actioned // rxFlightChannelsValid is true the instant we get a good packet or the BOXFAILSAFE switch is reverted // can also go false with good packets but where one flight channel is bad > 300ms (PPM type receiver error) @@ -862,7 +864,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs) if ((int32_t)(currentTimeUs - rssiUpdateAt) < 0) { return; } - rssiUpdateAt = currentTimeUs + DELAY_20_MS; + rssiUpdateAt = currentTimeUs + RSSI_UPDATE_INTERVAL; const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); uint16_t rssiValue = adcRssiSample / RSSI_ADC_DIVISOR; @@ -1022,27 +1024,3 @@ bool isRssiConfigured(void) return rssiSource != RSSI_SOURCE_NONE; } -timeDelta_t rxGetFrameDelta(timeDelta_t *frameAgeUs) -{ - static timeUs_t previousFrameTimeUs = 0; - static timeDelta_t frameTimeDeltaUs = 0; - - if (rxRuntimeState.rcFrameTimeUsFn) { - const timeUs_t frameTimeUs = rxRuntimeState.rcFrameTimeUsFn(); - - *frameAgeUs = cmpTimeUs(micros(), frameTimeUs); - - const timeDelta_t deltaUs = cmpTimeUs(frameTimeUs, previousFrameTimeUs); - if (deltaUs) { - frameTimeDeltaUs = deltaUs; - previousFrameTimeUs = frameTimeUs; - } - } - - return frameTimeDeltaUs; -} - -timeUs_t rxFrameTimeUs(void) -{ - return rxRuntimeState.lastRcFrameTimeUs; -} diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 878131ee4c..95393ff8ec 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -144,7 +144,6 @@ typedef struct rxRuntimeState_s { rcReadRawDataFnPtr rcReadRawFn; rcFrameStatusFnPtr rcFrameStatusFn; rcProcessFrameFnPtr rcProcessFrameFn; - rcGetFrameTimeUsFn *rcFrameTimeUsFn; uint16_t *channelData; void *frameData; timeUs_t lastRcFrameTimeUs; @@ -176,7 +175,7 @@ void rxInit(void); void rxProcessPending(bool state); bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs); void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs); -bool rxIsReceivingSignal(void); +bool isRxReceivingSignal(void); bool rxAreFlightChannelsValid(void); bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs); @@ -225,6 +224,6 @@ void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRange void suspendRxSignal(void); void resumeRxSignal(void); -timeDelta_t rxGetFrameDelta(timeDelta_t *frameAgeUs); +timeDelta_t rxGetFrameDelta(void); timeUs_t rxFrameTimeUs(void); diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c index 553601bf0b..e7c86e6e22 100644 --- a/src/main/rx/rx_spi.c +++ b/src/main/rx/rx_spi.c @@ -205,14 +205,15 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol) */ static uint8_t rxSpiFrameStatus(rxRuntimeState_t *rxRuntimeState) { - UNUSED(rxRuntimeState); - uint8_t status = RX_FRAME_PENDING; rx_spi_received_e result = protocolDataReceived(rxSpiPayload); if (result & RX_SPI_RECEIVED_DATA) { rxSpiNewPacketAvailable = true; + // use SPI EXTI time as frame time + // note that there is not rx time without EXTI + rxRuntimeState->lastRcFrameTimeUs = rxSpiGetLastExtiTimeUs(); status = RX_FRAME_COMPLETE; } @@ -260,8 +261,6 @@ bool rxSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeStat if (rxSpiExtiConfigured()) { rxSpiExtiInit(extiConfig.ioConfig, extiConfig.trigger); - - rxRuntimeState->rcFrameTimeUsFn = rxSpiGetLastExtiTimeUs; } rxSpiNewPacketAvailable = false; diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index 03e4abc346..9c1be0c0c7 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -177,7 +177,6 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) } rxRuntimeState->rcFrameStatusFn = sbusFrameStatus; - rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 1484ff2165..0f12fda67d 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -387,7 +387,6 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) rxRuntimeState->rcReadRawFn = spektrumReadRawRC; rxRuntimeState->rcFrameStatusFn = spektrumFrameStatus; - rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs; #if defined(USE_TELEMETRY_SRXL) rxRuntimeState->rcProcessFrameFn = spektrumProcessFrame; #endif diff --git a/src/main/rx/srxl2.c b/src/main/rx/srxl2.c index 4dbde31476..5f5accdcbd 100644 --- a/src/main/rx/srxl2.c +++ b/src/main/rx/srxl2.c @@ -491,7 +491,6 @@ bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) rxRuntimeState->channelCount = SRXL2_MAX_CHANNELS; rxRuntimeState->rcReadRawFn = srxl2ReadRawRC; rxRuntimeState->rcFrameStatusFn = srxl2FrameStatus; - rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs; rxRuntimeState->rcProcessFrameFn = srxl2ProcessFrame; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 9a5fb0b988..e4cb141d07 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -174,7 +174,6 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) rxRuntimeState->channelCount = MIN(SUMD_MAX_CHANNEL, MAX_SUPPORTED_RC_CHANNEL_COUNT); rxRuntimeState->rcReadRawFn = sumdReadRawRC; rxRuntimeState->rcFrameStatusFn = sumdFrameStatus; - rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 19bef4ff08..39d9f39d88 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -1051,7 +1051,7 @@ TEST(ArmingPreventionTest, Paralyze) extern "C" { uint32_t micros(void) { return simulationTime; } uint32_t millis(void) { return micros() / 1000; } - bool rxIsReceivingSignal(void) { return simulationHaveRx; } + bool isRxReceivingSignal(void) { return simulationHaveRx; } bool featureIsEnabled(uint32_t f) { return simulationFeatureFlags & f; } void warningLedFlash(void) {} @@ -1128,7 +1128,7 @@ extern "C" { bool isUpright(void) { return mockIsUpright; } void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {}; void gyroFiltering(timeUs_t) {}; - timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; } + timeDelta_t rxGetFrameDelta() { return 0; } void updateRcRefreshRate(timeUs_t) {}; uint16_t getAverageSystemLoadPercent(void) { return 0; } bool isMotorProtocolEnabled(void) { return true; } diff --git a/src/test/unit/blackbox_unittest.cc b/src/test/unit/blackbox_unittest.cc index faeb6a8156..833b71c749 100644 --- a/src/test/unit/blackbox_unittest.cc +++ b/src/test/unit/blackbox_unittest.cc @@ -380,7 +380,7 @@ void closeSerialPort(serialPort_t *) {} portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e ) {return PORTSHARING_UNUSED;} failsafePhase_e failsafePhase(void) {return FAILSAFE_IDLE;} bool rxAreFlightChannelsValid(void) {return false;} -bool rxIsReceivingSignal(void) {return false;} +bool isRxReceivingSignal(void) {return false;} bool isRssiConfigured(void) {return false;} float getMotorOutputLow(void) {return 0.0;} float getMotorOutputHigh(void) {return 0.0;} diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc index a83e91e3c9..cb3243a771 100644 --- a/src/test/unit/cli_unittest.cc +++ b/src/test/unit/cli_unittest.cc @@ -387,6 +387,7 @@ bool isModeActivationConditionConfigured(const modeActivationCondition_t *, cons void delay(uint32_t) {} displayPort_t *osdGetDisplayPort(osdDisplayPortDevice_e *) { return NULL; } mcuTypeId_e getMcuTypeId(void) { return MCU_TYPE_UNKNOWN; } -uint16_t getCurrentRxIntervalUs(void) { return 0; } +uint16_t getCurrentRxRateHz(void) { return 0; } uint16_t getAverageSystemLoadPercent(void) { return 0; } +bool getRxRateValid(void) { return false; } } diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 2393bc0112..723658ff0a 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -588,7 +588,7 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land) // when failsafeUpdateState(); - // now should be in monitoring mode, with switch holding signalReceived false + // now should be in monitoring mode, with switch holding rxDataReceived false EXPECT_TRUE(failsafeIsActive()); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_TRUE(isArmingDisabled()); diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 0179a25199..40de5f07bf 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -386,7 +386,7 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) } bool failsafeIsActive() { return false; } -bool rxIsReceivingSignal() { return true; } +bool isRxReceivingSignal() { return true; } bool isBeeperOn() { return false; }; diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 0a37ac2503..7be5ff2c60 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -622,7 +622,7 @@ void dashboardDisablePageCycling() {} void dashboardEnablePageCycling() {} bool failsafeIsActive() { return false; } -bool rxIsReceivingSignal() { return true; } +bool isRxReceivingSignal() { return true; } bool failsafeIsReceivingRxData() { return true; } uint8_t getCurrentControlRateProfileIndex(void) diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc index 058d942e0f..8da67bdc8d 100644 --- a/src/test/unit/vtx_unittest.cc +++ b/src/test/unit/vtx_unittest.cc @@ -130,7 +130,7 @@ extern "C" { uint8_t activePidLoopDenom = 1; uint32_t micros(void) { return simulationTime; } uint32_t millis(void) { return micros() / 1000; } - bool rxIsReceivingSignal(void) { return simulationHaveRx; } + bool isRxReceivingSignal(void) { return simulationHaveRx; } bool featureIsEnabled(uint32_t f) { return simulationFeatureFlags & f; } void warningLedFlash(void) {} @@ -202,7 +202,7 @@ extern "C" { bool isUpright(void) { return true; } void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {}; void gyroFiltering(timeUs_t) {}; - timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; } + timeDelta_t rxGetFrameDelta() { return 0; } void updateRcRefreshRate(timeUs_t) {}; uint16_t getAverageSystemLoadPercent(void) { return 0; } bool isMotorProtocolEnabled(void) { return false; }