mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 04:15:44 +03:00
RC smoothing: improve rx frame rate detection, add rc_smoothing_info cli command
Improved the rx frame rate detection/training by delaying calculation to avoid loop time jitter during flight controller initialization. For auto cutoffs calculate a value appropriate for BIQUAD or PT1 depending on the configuration. Added a new rc_smoothing_info cli command to display internal details about its operation.
This commit is contained in:
Bruce Luckcuck
parent
95f8cfd8b8
commit
e49c10b573
5 changed files with 129 additions and 32 deletions
|
|
@ -69,11 +69,17 @@ enum {
|
|||
};
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
#define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 3000 // Wait 3 seconds after power to let the PID loop stabilize before starting average frame rate calculation
|
||||
#define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 5000 // Wait 5 seconds after power to let the PID loop stabilize before starting average frame rate calculation
|
||||
#define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50
|
||||
#define RC_SMOOTHING_FILTER_INPUT_AUTO_HZ 50.0f // Used to calculate the default cutoff based on rx frame rate. For example, 9ms frame should use 50hz
|
||||
#define RC_SMOOTHING_FILTER_DERIVATIVE_AUTO_HZ 60.0f // Used to calculate the derivative default cutoff based on rx frame rate. For example, 9ms frame should use 60hz
|
||||
#define RC_SMOOTHING_FILTER_AUTO_MS 9.0f // Formula used: RC_SMOOTHING_FILTER_AUTO_HZ / (measured rx frame delay / RC_SMOOTHING_FILTER_AUTO_HZ)
|
||||
|
||||
static FAST_RAM_ZERO_INIT uint16_t defaultInputCutoffFrequency;
|
||||
static FAST_RAM_ZERO_INIT uint16_t defaultDerivativeCutoffFrequency;
|
||||
static FAST_RAM_ZERO_INIT uint16_t filterCutoffFrequency;
|
||||
static FAST_RAM_ZERO_INIT uint16_t derivativeCutoffFrequency;
|
||||
static FAST_RAM_ZERO_INIT uint16_t calculatedFrameAverageMs;
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
float getSetpointRate(int axis)
|
||||
|
|
@ -259,9 +265,14 @@ FAST_CODE uint8_t processRcInterpolation(void)
|
|||
}
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
uint8_t calcRcSmoothingCutoff(float avgRxFrameRate, float filterBaseline)
|
||||
uint16_t calcRcSmoothingCutoff(float avgRxFrameRate, float filterBaseline, bool pt1)
|
||||
{
|
||||
return lrintf(filterBaseline / (avgRxFrameRate / RC_SMOOTHING_FILTER_AUTO_MS));
|
||||
float cutoff = filterBaseline / (avgRxFrameRate / RC_SMOOTHING_FILTER_AUTO_MS);
|
||||
if (pt1) { // Convert to a PT1 cutoff (2pi * cutoff)^2 / 500 / 2pi;
|
||||
const float twoPi = 2.0f * M_PIf;
|
||||
cutoff = (sq(cutoff * twoPi) / 500) / twoPi;
|
||||
}
|
||||
return lrintf(cutoff);
|
||||
}
|
||||
|
||||
FAST_CODE uint8_t processRcSmoothingFilter(void)
|
||||
|
|
@ -275,10 +286,8 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
|
|||
static FAST_RAM_ZERO_INIT bool filterInitialized;
|
||||
static FAST_RAM_ZERO_INIT float rxFrameTimeSum;
|
||||
static FAST_RAM_ZERO_INIT int rxFrameCount;
|
||||
static FAST_RAM_ZERO_INIT uint16_t defaultInputCutoffFrequency;
|
||||
static FAST_RAM_ZERO_INIT uint16_t defaultDerivativeCutoffFrequency;
|
||||
static FAST_RAM_ZERO_INIT uint16_t filterCutoffFrequency;
|
||||
static FAST_RAM_ZERO_INIT uint16_t derivativeCutoffFrequency;
|
||||
static FAST_RAM uint16_t minRxFrameInterval = UINT16_MAX;
|
||||
static FAST_RAM_ZERO_INIT uint16_t maxRxFrameInterval;
|
||||
|
||||
if (!initialized) {
|
||||
initialized = true;
|
||||
|
|
@ -298,24 +307,36 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
|
|||
if (rxIsReceivingSignal() && (targetPidLooptime > 0) && (millis() > RC_SMOOTHING_FILTER_TRAINING_DELAY_MS)) {
|
||||
rxFrameTimeSum += currentRxRefreshRate;
|
||||
rxFrameCount++;
|
||||
maxRxFrameInterval = MAX(maxRxFrameInterval, currentRxRefreshRate);
|
||||
minRxFrameInterval = MIN(minRxFrameInterval, currentRxRefreshRate);
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 0, rxFrameCount); // log the step count during training
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 3, currentRxRefreshRate); // log each frame interval during training
|
||||
if (rxFrameCount >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) {
|
||||
const float avgRxFrameRate = rxFrameTimeSum / rxFrameCount / 1000;
|
||||
defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameRate, RC_SMOOTHING_FILTER_INPUT_AUTO_HZ);
|
||||
defaultDerivativeCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameRate, RC_SMOOTHING_FILTER_DERIVATIVE_AUTO_HZ);
|
||||
rxFrameTimeSum = rxFrameTimeSum - minRxFrameInterval - maxRxFrameInterval; // Throw out high and low samples
|
||||
calculatedFrameAverageMs = lrintf(rxFrameTimeSum / (rxFrameCount - 2));
|
||||
const float avgRxFrameRate = (rxFrameTimeSum / (rxFrameCount - 2)) / 1000;
|
||||
|
||||
defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameRate, RC_SMOOTHING_FILTER_INPUT_AUTO_HZ, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1));
|
||||
filterCutoffFrequency = (filterCutoffFrequency == 0) ? defaultInputCutoffFrequency : filterCutoffFrequency;
|
||||
|
||||
if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) {
|
||||
derivativeCutoffFrequency = 0;
|
||||
} else {
|
||||
defaultDerivativeCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameRate, RC_SMOOTHING_FILTER_DERIVATIVE_AUTO_HZ, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1));
|
||||
derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultDerivativeCutoffFrequency : derivativeCutoffFrequency;
|
||||
}
|
||||
|
||||
const float dT = targetPidLooptime * 1e-6f;
|
||||
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
|
||||
if ((1 << i) & interpolationChannels) {
|
||||
switch (rxConfig()->rc_smoothing_input_type) {
|
||||
case RC_SMOOTHING_INPUT_BIQUAD:
|
||||
biquadFilterInitLPF(&rcCommandFilterBiquad[i], filterCutoffFrequency, targetPidLooptime);
|
||||
break;
|
||||
case RC_SMOOTHING_INPUT_PT1:
|
||||
default:
|
||||
pt1FilterInit(&rcCommandFilterPt1[i], pt1FilterGain(filterCutoffFrequency, dT));
|
||||
break;
|
||||
case RC_SMOOTHING_INPUT_BIQUAD:
|
||||
default:
|
||||
biquadFilterInitLPF(&rcCommandFilterBiquad[i], filterCutoffFrequency, targetPidLooptime);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -329,20 +350,24 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
|
|||
}
|
||||
}
|
||||
|
||||
if (filterInitialized && (debugMode = DEBUG_RC_SMOOTHING)) {
|
||||
// after training has completed then log the raw rc channel and the calculated
|
||||
// average rx frame rate that was used to calculate the automatic filter cutoffs
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rxConfig()->rc_smoothing_debug_axis]));
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 3, defaultInputCutoffFrequency);
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 3, calculatedFrameAverageMs);
|
||||
}
|
||||
|
||||
for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
|
||||
if ((1 << updatedChannel) & interpolationChannels) {
|
||||
if (filterInitialized) {
|
||||
switch (rxConfig()->rc_smoothing_input_type) {
|
||||
case RC_SMOOTHING_INPUT_BIQUAD:
|
||||
rcCommand[updatedChannel] = biquadFilterApply(&rcCommandFilterBiquad[updatedChannel], lastRxData[updatedChannel]);
|
||||
break;
|
||||
case RC_SMOOTHING_INPUT_PT1:
|
||||
default:
|
||||
rcCommand[updatedChannel] = pt1FilterApply(&rcCommandFilterPt1[updatedChannel], lastRxData[updatedChannel]);
|
||||
break;
|
||||
case RC_SMOOTHING_INPUT_BIQUAD:
|
||||
default:
|
||||
rcCommand[updatedChannel] = biquadFilterApply(&rcCommandFilterBiquad[updatedChannel], lastRxData[updatedChannel]);
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
// If filter isn't initialized yet then use the actual unsmoothed rx channel data
|
||||
|
|
@ -557,3 +582,23 @@ void initRcProcessing(void)
|
|||
break;
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
uint16_t rcSmoothingGetValue(int whichValue)
|
||||
{
|
||||
switch (whichValue) {
|
||||
case RC_SMOOTHING_VALUE_INPUT_AUTO:
|
||||
return defaultInputCutoffFrequency;
|
||||
case RC_SMOOTHING_VALUE_INPUT_ACTIVE:
|
||||
return filterCutoffFrequency;
|
||||
case RC_SMOOTHING_VALUE_DERIVATIVE_AUTO:
|
||||
return defaultDerivativeCutoffFrequency;
|
||||
case RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE:
|
||||
return derivativeCutoffFrequency;
|
||||
case RC_SMOOTHING_VALUE_AVERAGE_FRAME:
|
||||
return calculatedFrameAverageMs;
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
|
|
|||
|
|
@ -39,3 +39,4 @@ void updateRcCommands(void);
|
|||
void resetYawAxis(void);
|
||||
void initRcProcessing(void);
|
||||
bool isMotorsReversed(void);
|
||||
uint16_t rcSmoothingGetValue(int whichValue);
|
||||
|
|
|
|||
|
|
@ -76,6 +76,13 @@ typedef enum {
|
|||
RC_SMOOTHING_DERIVATIVE_BIQUAD
|
||||
} rcSmoothingDerivativeFilter_e;
|
||||
|
||||
typedef enum {
|
||||
RC_SMOOTHING_VALUE_INPUT_AUTO,
|
||||
RC_SMOOTHING_VALUE_INPUT_ACTIVE,
|
||||
RC_SMOOTHING_VALUE_DERIVATIVE_AUTO,
|
||||
RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE,
|
||||
RC_SMOOTHING_VALUE_AVERAGE_FRAME
|
||||
} rcSmoothingInfoType_e;
|
||||
|
||||
#define ROL_LO (1 << (2 * ROLL))
|
||||
#define ROL_CE (3 << (2 * ROLL))
|
||||
|
|
|
|||
|
|
@ -306,10 +306,12 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
}
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType) {
|
||||
setpointDerivativeLpfInitialized = true;
|
||||
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType)
|
||||
{
|
||||
rcSmoothingDebugAxis = debugAxis;
|
||||
rcSmoothingFilterType = filterType;
|
||||
if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
|
||||
setpointDerivativeLpfInitialized = true;
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
switch (rcSmoothingFilterType) {
|
||||
case RC_SMOOTHING_DERIVATIVE_PT1:
|
||||
|
|
@ -321,6 +323,7 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint
|
|||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
typedef struct pidCoefficient_s {
|
||||
|
|
@ -896,7 +899,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
|
|||
if (axis == rcSmoothingDebugAxis) {
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
|
||||
}
|
||||
if ((dynCd != 0) && setpointDerivativeLpfInitialized && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
|
||||
if ((dynCd != 0) && setpointDerivativeLpfInitialized) {
|
||||
switch (rcSmoothingFilterType) {
|
||||
case RC_SMOOTHING_DERIVATIVE_PT1:
|
||||
pidSetpointDelta = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);
|
||||
|
|
|
|||
|
|
@ -3654,6 +3654,44 @@ static void cliVersion(char *cmdline)
|
|||
);
|
||||
}
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
static void cliRcSmoothing(char *cmdline)
|
||||
{
|
||||
UNUSED(cmdline);
|
||||
cliPrint("# RC Smoothing Type: ");
|
||||
if (rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_FILTER) {
|
||||
cliPrintLine("FILTER");
|
||||
uint16_t avgRxFrameMs = rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME);
|
||||
cliPrint("# Detected RX frame rate: ");
|
||||
if (avgRxFrameMs == 0) {
|
||||
cliPrintLine("NO SIGNAL");
|
||||
} else {
|
||||
cliPrintLinef("%d.%dms", avgRxFrameMs / 1000, avgRxFrameMs % 1000);
|
||||
}
|
||||
cliPrintLinef("# Auto input cutoff: %dhz", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_AUTO));
|
||||
cliPrintf("# Active input cutoff: %dhz ", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE));
|
||||
if (rxConfig()->rc_smoothing_input_cutoff == 0) {
|
||||
cliPrintLine("(auto)");
|
||||
} else {
|
||||
cliPrintLine("(manual)");
|
||||
}
|
||||
cliPrintLinef("# Auto derivative cutoff: %dhz", rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_AUTO));
|
||||
cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE));
|
||||
if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) {
|
||||
cliPrintLine("off)");
|
||||
} else {
|
||||
if (rxConfig()->rc_smoothing_derivative_cutoff == 0) {
|
||||
cliPrintLine("auto)");
|
||||
} else {
|
||||
cliPrintLine("manual)");
|
||||
}
|
||||
}
|
||||
} else {
|
||||
cliPrintLine("INTERPOLATION");
|
||||
}
|
||||
}
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
#if defined(USE_RESOURCE_MGMT)
|
||||
|
||||
#define MAX_RESOURCE_INDEX(x) ((x) == 0 ? 1 : (x))
|
||||
|
|
@ -4441,6 +4479,9 @@ const clicmd_t cmdTable[] = {
|
|||
#endif
|
||||
CLI_COMMAND_DEF("profile", "change profile", "[<index>]", cliProfile),
|
||||
CLI_COMMAND_DEF("rateprofile", "change rate profile", "[<index>]", cliRateProfile),
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
CLI_COMMAND_DEF("rc_smoothing_info", "show rc_smoothing operational settings", NULL, cliRcSmoothing),
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
#ifdef USE_RESOURCE_MGMT
|
||||
CLI_COMMAND_DEF("resource", "show/set resources", NULL, cliResource),
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue