From fa40fbe865017b0b81b982a95b67f1050f06cbe8 Mon Sep 17 00:00:00 2001 From: jirif Date: Fri, 4 May 2018 07:45:47 +0200 Subject: [PATCH] Fix RX rate calculation when using fport --- src/main/fc/fc_rc.c | 13 ++++--------- src/main/fc/fc_rc.h | 2 ++ src/main/fc/fc_tasks.c | 6 +++++- src/main/interface/cli.c | 5 +++-- src/test/unit/cli_unittest.cc | 1 + 5 files changed, 15 insertions(+), 12 deletions(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 52d7822c0..0a9c456e5 100644 --- a/src/main/fc/fc_rc.c +++ b/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) { - currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000); - if (isAntiGravityModeActive()) { - checkForThrottleErrorResetState(currentRxRefreshRate); - } + if (isRXDataNew && 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); } diff --git a/src/main/fc/fc_rc.h b/src/main/fc/fc_rc.h index cad3cd7cf..d2c1e264f 100644 --- a/src/main/fc/fc_rc.h +++ b/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); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index aad2d51c8..2c3fef225 100644 --- a/src/main/fc/fc_tasks.c +++ b/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)); diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index b2728bac8..3aec2c3a2 100644 --- a/src/main/interface/cli.c +++ b/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" @@ -3178,7 +3179,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); @@ -3931,7 +3932,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 }; diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc index 75675109d..24eef922e 100644 --- a/src/test/unit/cli_unittest.cc +++ b/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[]= {