Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 22:05:17 +03:00
Code Issues Wiki Activity

Merge pull request #6051 from etracer65/rc_smoothing_training

Browse source 
RC smoothing: improve rx frame rate detection, add rc_smoothing_info cli command
This commit is contained in:
Michael Keller 2018-06-07 16:22:48 +12:00 • committed by GitHub
commit 2dd1d742bf
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 141 additions and 39 deletions
Download patch file Download diff file Expand all files Collapse all files

11
src/main/blackbox/blackbox.c
View file

@ -53,6 +53,7 @@
#include "fc/config.h"
#include "fc/controlrate_profile.h"
#include "fc/fc_rc.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
@ -1325,11 +1326,13 @@ static bool blackboxWriteSysinfo(void)
#ifdef USE_RC_SMOOTHING_FILTER
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_type", "%d", rxConfig()->rc_smoothing_type);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_input_cutoff", "%d", rxConfig()->rc_smoothing_input_cutoff);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_derivative_cutoff", "%d", rxConfig()->rc_smoothing_derivative_cutoff);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_debug_axis", "%d", rxConfig()->rc_smoothing_debug_axis);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_input_type", "%d", rxConfig()->rc_smoothing_input_type);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_derivative_type", "%d", rxConfig()->rc_smoothing_derivative_type);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rxConfig()->rc_smoothing_input_cutoff,
rxConfig()->rc_smoothing_derivative_cutoff);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_filter_type", "%d, %d", rxConfig()->rc_smoothing_input_type,
rxConfig()->rc_smoothing_derivative_type);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE),
rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE));
#endif // USE_RC_SMOOTHING_FILTER

95
src/main/fc/fc_rc.c
View file

@ -69,11 +69,15 @@ 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_IDENTITY_FREQUENCY 80 // Used in the formula to convert a BIQUAD cutoff frequency to PT1
#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 calculatedFrameTimeAverageUs;
#endif // USE_RC_SMOOTHING_FILTER
float getSetpointRate(int axis)
@ -259,9 +263,18 @@ FAST_CODE uint8_t processRcInterpolation(void)
}
#ifdef USE_RC_SMOOTHING_FILTER
uint8_t calcRcSmoothingCutoff(float avgRxFrameRate, float filterBaseline)
int calcRcSmoothingCutoff(float avgRxFrameTime, bool pt1)
{
return lrintf(filterBaseline / (avgRxFrameRate / RC_SMOOTHING_FILTER_AUTO_MS));
if (avgRxFrameTime > 0) {
float cutoff = (1 / avgRxFrameTime) / 2; // calculate the nyquist frequency
cutoff = cutoff * 0.90f; // Use 90% of the calculated nyquist frequency
if (pt1) {
cutoff = sq(cutoff) / RC_SMOOTHING_IDENTITY_FREQUENCY; // convert to a cutoff for pt1 that has similar characteristics
}
return lrintf(cutoff);
} else {
return 0;
}
}
FAST_CODE uint8_t processRcSmoothingFilter(void)
@ -275,10 +288,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 +309,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
calculatedFrameTimeAverageUs = lrintf(rxFrameTimeSum / (rxFrameCount - 2));
const float avgRxFrameTime = (rxFrameTimeSum / (rxFrameCount - 2)) * 1e-6f;
defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTime, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1));
filterCutoffFrequency = (filterCutoffFrequency == 0) ? defaultInputCutoffFrequency : filterCutoffFrequency;
derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultDerivativeCutoffFrequency : derivativeCutoffFrequency;
if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) {
derivativeCutoffFrequency = 0;
} else {
defaultDerivativeCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTime, (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 +352,24 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
}
}
DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rxConfig()->rc_smoothing_debug_axis]));
DEBUG_SET(DEBUG_RC_SMOOTHING, 3, defaultInputCutoffFrequency);
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, calculatedFrameTimeAverageUs);
}
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 +584,23 @@ void initRcProcessing(void)
break;
}
}
#ifdef USE_RC_SMOOTHING_FILTER
int 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 calculatedFrameTimeAverageUs;
default:
return 0;
}
}
#endif // USE_RC_SMOOTHING_FILTER

1
src/main/fc/fc_rc.h
View file

@ -39,3 +39,4 @@ void updateRcCommands(void);
void resetYawAxis(void);
void initRcProcessing(void);
bool isMotorsReversed(void);
int rcSmoothingGetValue(int whichValue);

7
src/main/fc/rc_controls.h
View file

@ -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))

25
src/main/flight/pid.c
View file

@ -308,18 +308,21 @@ 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;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
switch (rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
biquadFilterInitLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
break;
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:
pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
biquadFilterInitLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
break;
}
}
}
}
@ -902,7 +905,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);

41
src/main/interface/cli.c
View file

@ -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