diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index e1c95813d0..cb6c828ff5 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -4670,7 +4670,10 @@ static void cliStatus(char *cmdline) // Run status const int gyroRate = getTaskDeltaTime(TASK_GYRO) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_GYRO))); - const int rxRate = currentRxRefreshRate == 0 ? 0 : (int)(1000000.0f / ((float)currentRxRefreshRate)); + int rxRate = getCurrentRxRefreshRate(); + if (rxRate != 0) { + rxRate = (int)(1000000.0f / ((float)rxRate)); + } const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM))); cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d", constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYRO), gyroRate, rxRate, systemRate); diff --git a/src/main/fc/core.c b/src/main/fc/core.c index b444fd5ac6..c8428bdc95 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -149,7 +149,6 @@ static bool flipOverAfterCrashActive = false; static timeUs_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero -bool isRXDataNew; static int lastArmingDisabledReason = 0; static timeUs_t lastDisarmTimeUs; static int tryingToArm = ARMING_DELAYED_DISARMED; @@ -752,6 +751,8 @@ bool processRx(timeUs_t currentTimeUs) return false; } + updateRcRefreshRate(currentTimeUs); + // in 3D mode, we need to be able to disarm by switch at any time if (featureIsEnabled(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) diff --git a/src/main/fc/core.h b/src/main/fc/core.h index 6afd626c46..4072c0e53e 100644 --- a/src/main/fc/core.h +++ b/src/main/fc/core.h @@ -27,8 +27,6 @@ extern int16_t magHold; #endif -extern bool isRXDataNew; - typedef struct throttleCorrectionConfig_s { uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 429cebaba1..074efb31f0 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -33,8 +33,6 @@ #include "config/config.h" #include "config/feature.h" -#include "drivers/time.h" - #include "fc/controlrate_profile.h" #include "fc/core.h" #include "fc/rc.h" @@ -71,7 +69,8 @@ static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; static float throttlePIDAttenuation; static bool reverseMotors = false; static applyRatesFn *applyRates; -uint16_t currentRxRefreshRate; +static uint16_t currentRxRefreshRate; +static bool isRxDataNew = false; FAST_RAM_ZERO_INIT uint8_t interpolationChannels; static FAST_RAM_ZERO_INIT uint32_t rcFrameNumber; @@ -330,7 +329,7 @@ static FAST_CODE uint8_t processRcInterpolation(void) rxRefreshRate = rxGetRefreshRate(); } - if (isRXDataNew && rxRefreshRate > 0) { + if (isRxDataNew && rxRefreshRate > 0) { rcInterpolationStepCount = rxRefreshRate / targetPidLooptime; for (int channel = 0; channel < PRIMARY_CHANNEL_COUNT; channel++) { @@ -364,6 +363,26 @@ static FAST_CODE uint8_t processRcInterpolation(void) } +void updateRcRefreshRate(timeUs_t currentTimeUs) +{ + static timeUs_t lastRxTimeUs; + + timeDelta_t frameAgeUs; + timeDelta_t refreshRateUs = rxGetFrameDelta(&frameAgeUs); + if (!refreshRateUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) { + if (!rxTryGetFrameDeltaOrZero(&refreshRateUs)) { + refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol + } + } + lastRxTimeUs = currentTimeUs; + currentRxRefreshRate = constrain(refreshRateUs, 1000, 30000); +} + +uint16_t getCurrentRxRefreshRate(void) +{ + return currentRxRefreshRate; +} + #ifdef USE_RC_SMOOTHING_FILTER // Determine a cutoff frequency based on filter type and the calculated // average rx frame time @@ -549,7 +568,7 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void) } } - if (isRXDataNew) { + if (isRxDataNew) { // store the new raw channel values for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { @@ -655,16 +674,16 @@ FAST_CODE void processRcCommand(void) { uint8_t updatedChannel; - if (isRXDataNew) { + if (isRxDataNew) { rcFrameNumber++; } - if (isRXDataNew && pidAntiGravityEnabled()) { + if (isRxDataNew && pidAntiGravityEnabled()) { checkForThrottleErrorResetState(currentRxRefreshRate); } #ifdef USE_INTERPOLATED_SP - if (isRXDataNew) { + if (isRxDataNew) { for (int i = FD_ROLL; i <= FD_YAW; i++) { oldRcCommand[i] = rcCommand[i]; const float rcCommandf = rcCommand[i] / 500.0f; @@ -687,8 +706,8 @@ FAST_CODE void processRcCommand(void) break; } - if (isRXDataNew || updatedChannel) { - const uint8_t maxUpdatedAxis = isRXDataNew ? FD_YAW : MIN(updatedChannel, FD_YAW); // throttle channel doesn't require rate calculation + if (isRxDataNew || updatedChannel) { + const uint8_t maxUpdatedAxis = isRxDataNew ? FD_YAW : MIN(updatedChannel, FD_YAW); // throttle channel doesn't require rate calculation #if defined(SIMULATOR_BUILD) #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunsafe-loop-optimizations" @@ -708,13 +727,15 @@ FAST_CODE void processRcCommand(void) } } - if (isRXDataNew) { - isRXDataNew = false; + if (isRxDataNew) { + isRxDataNew = false; } } FAST_CODE_NOINLINE void updateRcCommands(void) { + isRxDataNew = true; + // PITCH & ROLL only dynamic PID adjustment, depending on throttle value int32_t prop; if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { diff --git a/src/main/fc/rc.h b/src/main/fc/rc.h index ae2f0bc7ca..853269807a 100644 --- a/src/main/fc/rc.h +++ b/src/main/fc/rc.h @@ -20,6 +20,8 @@ #pragma once +#include "drivers/time.h" + #include "fc/rc_controls.h" typedef enum { @@ -30,8 +32,6 @@ typedef enum { INTERPOLATION_CHANNELS_RPT, } interpolationChannels_e; -extern uint16_t currentRxRefreshRate; - #ifdef USE_RC_SMOOTHING_FILTER #define RC_SMOOTHING_AUTO_FACTOR_MIN 0 #define RC_SMOOTHING_AUTO_FACTOR_MAX 50 @@ -55,3 +55,5 @@ float getRawDeflection(int axis); float applyCurve(int axis, float deflection); uint32_t getRcFrameNumber(); float getRcCurveSlope(int axis, float deflection); +void updateRcRefreshRate(timeUs_t currentTimeUs); +uint16_t getCurrentRxRefreshRate(void); diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 33643378c8..fa71dc2816 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -160,32 +160,19 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs) static void taskUpdateRxMain(timeUs_t currentTimeUs) { - static timeUs_t lastRxTimeUs; - if (!processRx(currentTimeUs)) { return; } - timeDelta_t frameAgeUs; - timeDelta_t refreshRateUs = rxGetFrameDelta(&frameAgeUs); - if (!refreshRateUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) { - if (!rxTryGetFrameDeltaOrZero(&refreshRateUs)) { - refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol - } - } - lastRxTimeUs = currentTimeUs; - currentRxRefreshRate = constrain(refreshRateUs, 1000, 30000); - isRXDataNew = true; + // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState + updateRcCommands(); + updateArmingStatus(); #ifdef USE_USB_CDC_HID if (!ARMING_FLAG(ARMED)) { sendRcDataToHid(); } #endif - - // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState - updateRcCommands(); - updateArmingStatus(); } #ifdef USE_BARO diff --git a/src/main/flight/interpolated_setpoint.c b/src/main/flight/interpolated_setpoint.c index 470a6bfb71..30da757249 100644 --- a/src/main/flight/interpolated_setpoint.c +++ b/src/main/flight/interpolated_setpoint.c @@ -65,7 +65,7 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp if (newRcFrame) { float rawSetpoint = getRawSetpoint(axis); - const float rxInterval = currentRxRefreshRate * 1e-6f; + const float rxInterval = getCurrentRxRefreshRate() * 1e-6f; const float rxRate = 1.0f / rxInterval; float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate; float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis]; diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index a2322a1864..646d94b72f 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -1102,4 +1102,5 @@ extern "C" { void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {}; void gyroFiltering(timeUs_t) {}; timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; } + void updateRcRefreshRate(timeUs_t) {}; } diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc index 1758c462f2..47acffa4e7 100644 --- a/src/test/unit/cli_unittest.cc +++ b/src/test/unit/cli_unittest.cc @@ -359,4 +359,5 @@ 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 getCurrentRxRefreshRate(void) { return 0; } } diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc index 60396d55f2..f2ce02093a 100644 --- a/src/test/unit/vtx_unittest.cc +++ b/src/test/unit/vtx_unittest.cc @@ -187,4 +187,5 @@ extern "C" { void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {}; void gyroFiltering(timeUs_t) {}; timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; } + void updateRcRefreshRate(timeUs_t) {}; }