Merge pull request #5809 from jirif/rx_rate_calculation
Fix rx rate calculation when using fport
This commit is contained in:
commit
8f50aad585
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -20,6 +20,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
extern uint16_t currentRxRefreshRate;
|
||||
|
||||
void processRcCommand(void);
|
||||
float getSetpointRate(int axis);
|
||||
float getRcDeflection(int axis);
|
||||
|
|
|
@ -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));
|
||||
|
|
|
@ -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
|
||||
};
|
||||
|
||||
|
|
|
@ -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[]= {
|
||||
|
|
Loading…
Reference in New Issue