Merge pull request #5809 from jirif/rx_rate_calculation

Fix rx rate calculation when using fport

src/main/fc/fc_rc.c

@@ -45,8 +45,6 @@
 #include "flight/pid.h"
 #include "rx/rx.h"
 
-#include "scheduler/scheduler.h"
-
 #include "sensors/battery.h"
 
 typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs);
@@ -55,6 +53,7 @@ static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
 static float throttlePIDAttenuation;
 static bool reverseMotors = false;
 static applyRatesFn *applyRates;
+uint16_t currentRxRefreshRate;
 
 float getSetpointRate(int axis)
 {
@@ -185,13 +184,9 @@ FAST_CODE NOINLINE void processRcCommand(void)
     static float rcCommandInterp[4] = { 0, 0, 0, 0 };
     static float rcStepSize[4] = { 0, 0, 0, 0 };
     static int16_t rcInterpolationStepCount;
-    static uint16_t currentRxRefreshRate;
 
-    if (isRXDataNew) {
+    if (isRXDataNew && isAntiGravityModeActive()) {
-        currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
+        checkForThrottleErrorResetState(currentRxRefreshRate);
-        if (isAntiGravityModeActive()) {
-            checkForThrottleErrorResetState(currentRxRefreshRate);
-        }
     }
 
     const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT"
@@ -222,7 +217,7 @@ FAST_CODE NOINLINE void processRcCommand(void)
 
             if (debugMode == DEBUG_RC_INTERPOLATION) {
                 debug[0] = lrintf(rcCommand[0]);
-                debug[1] = lrintf(getTaskDeltaTime(TASK_RX) / 1000);
+                debug[1] = lrintf(currentRxRefreshRate / 1000);
                 //debug[1] = lrintf(rcCommandInterp[0]);
                 //debug[1] = lrintf(rcStepSize[0]*100);
             }

src/main/fc/fc_rc.h

@@ -20,6 +20,8 @@
 
 #pragma once
 
+extern uint16_t currentRxRefreshRate;
+
 void processRcCommand(void);
 float getSetpointRate(int axis);
 float getRcDeflection(int axis);

src/main/fc/fc_tasks.c

@@ -44,6 +44,7 @@
 
 #include "fc/config.h"
 #include "fc/fc_core.h"
+#include "fc/fc_rc.h"
 #include "fc/fc_dispatch.h"
 #include "fc/fc_tasks.h"
 #include "fc/rc_controls.h"
@@ -151,13 +152,16 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
         return;
     }
 
+    static timeUs_t lastRxTimeUs;
+    currentRxRefreshRate = constrain(currentTimeUs - lastRxTimeUs, 1000, 20000);
+    lastRxTimeUs = currentTimeUs;
     isRXDataNew = true;
 
 #ifdef USE_USB_CDC_HID
     if (!ARMING_FLAG(ARMED)) {
         int8_t report[8];
         for (int i = 0; i < 8; i++) {
-	        	report[i] = scaleRange(constrain(rcData[i], 1000, 2000), 1000, 2000, -127, 127);
+                report[i] = scaleRange(constrain(rcData[i], 1000, 2000), 1000, 2000, -127, 127);
         }
 #ifdef STM32F4
         USBD_HID_SendReport(&USB_OTG_dev, (uint8_t*)report, sizeof(report));

src/main/interface/cli.c

@@ -87,6 +87,7 @@ extern uint8_t __config_end;
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/fc_core.h"
+#include "fc/fc_rc.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
@@ -3195,7 +3196,7 @@ static void cliStatus(char *cmdline)
     cliPrintLinef("I2C Errors: %d, config size: %d, max available config: %d", i2cErrorCounter, getEEPROMConfigSize(), CONFIG_SIZE);
 
     const int gyroRate = getTaskDeltaTime(TASK_GYROPID) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_GYROPID)));
-    const int rxRate = getTaskDeltaTime(TASK_RX) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_RX)));
+    const int rxRate = currentRxRefreshRate == 0 ? 0 : (int)(1000000.0f / ((float)currentRxRefreshRate));
     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_GYROPID), gyroRate, rxRate, systemRate);
@@ -3951,7 +3952,7 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx),
 #endif
 #ifdef USE_USB_MSC
-	CLI_COMMAND_DEF("msc", "switch into msc mode", NULL, cliMsc),
+    CLI_COMMAND_DEF("msc", "switch into msc mode", NULL, cliMsc),
 #endif
 };
 

src/test/unit/cli_unittest.cc

@@ -230,6 +230,7 @@ uint8_t __config_end = 0x10;
 uint16_t averageSystemLoadPercent = 0;
 
 timeDelta_t getTaskDeltaTime(cfTaskId_e){ return 0; }
+uint16_t currentRxRefreshRate = 9000;
 armingDisableFlags_e getArmingDisableFlags(void) { return ARMING_DISABLED_NO_GYRO; }
 
 const char *armingDisableFlagNames[]= {