Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity

Update RC smoothing "auto" settings to consider interpolated fe… (#9249)

Browse source 
Update RC smoothing "auto" settings to consider interpolated feedforward
This commit is contained in:
Michael Keller 2019-12-02 11:31:05 +13:00 • committed by GitHub
commit 054bd66bc4
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 50 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -4816,16 +4816,19 @@ static void cliRcSmoothing(char *cmdline)
cliPrintLinef("%d.%dms", avgRxFrameMs / 1000, avgRxFrameMs % 1000);
}
}
cliPrint("# Input filter type: ");
cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rcSmoothingData->inputFilterType]);
cliPrintLinef("# Input filter type: %s", lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rcSmoothingData->inputFilterType]);
cliPrintf("# Active input cutoff: %dhz ", rcSmoothingData->inputCutoffFrequency);
if (rcSmoothingData->inputCutoffSetting == 0) {
cliPrintLine("(auto)");
} else {
cliPrintLine("(manual)");
}
cliPrint("# Derivative filter type: ");
cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rcSmoothingData->derivativeFilterType]);
cliPrintf("# Derivative filter type: %s", lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rcSmoothingData->derivativeFilterType]);
if (rcSmoothingData->derivativeFilterTypeSetting == RC_SMOOTHING_DERIVATIVE_AUTO) {
cliPrintLine(" (auto)");
} else {
cliPrintLinefeed();
}
cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingData->derivativeCutoffFrequency);
if (rcSmoothingData->derivativeFilterType == RC_SMOOTHING_DERIVATIVE_OFF) {
cliPrintLine("off)");

2
src/main/cli/settings.c
View file

@ -400,7 +400,7 @@ static const char * const lookupTableRcSmoothingInputType[] = {
"PT1", "BIQUAD"
};
static const char * const lookupTableRcSmoothingDerivativeType[] = {
"OFF", "PT1", "BIQUAD"
"OFF", "PT1", "BIQUAD", "AUTO"
};
#endif // USE_RC_SMOOTHING_FILTER

50
src/main/fc/rc.c
View file

@ -44,6 +44,7 @@
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/interpolated_setpoint.h"
#include "flight/gps_rescue.h"
#include "flight/pid.h"
@ -91,6 +92,7 @@ enum {
#define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT 20 // Look for samples varying this much from the current detected frame rate to initiate retraining
#define RC_SMOOTHING_RX_RATE_MIN_US 1000 // 1ms
#define RC_SMOOTHING_RX_RATE_MAX_US 50000 // 50ms or 20hz
#define RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_DERIVATIVE_PT1_HZ 100 // The value to use for "auto" when interpolated feedforward is enabled
static FAST_RAM_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
#endif // USE_RC_SMOOTHING_FILTER
@ -401,7 +403,10 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
// update or initialize the derivative filter
oldCutoff = smoothingData->derivativeCutoffFrequency;
if ((smoothingData->derivativeCutoffSetting == 0) && (smoothingData->derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
if ((rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF)
&& (currentPidProfile->ff_interpolate_sp == FF_INTERPOLATE_OFF)
&& (rcSmoothingData.derivativeCutoffSetting == 0)) {
smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (smoothingData->derivativeFilterType == RC_SMOOTHING_DERIVATIVE_PT1), smoothingData->autoSmoothnessFactor);
}
@ -444,20 +449,18 @@ static FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothing
// examining the rx frame times completely
FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
{
bool ret = false;
// if the input cutoff is 0 (auto) then we need to calculate cutoffs
if (rcSmoothingData.inputCutoffSetting == 0) {
ret = true;
return true;
}
// if the derivative type isn't OFF and the cutoff is 0 then we need to calculate
if (rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF) {
if (rcSmoothingData.derivativeCutoffSetting == 0) {
ret = true;
}
// if the derivative type isn't OFF, and the cutoff is 0, and interpolated feedforward is not enabled then we need to calculate
if ((rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF)
&& (currentPidProfile->ff_interpolate_sp == FF_INTERPOLATE_OFF)
&& (rcSmoothingData.derivativeCutoffSetting == 0)) {
return true;
}
return ret;
return false;
}
static FAST_CODE uint8_t processRcSmoothingFilter(void)
@ -477,14 +480,37 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
rcSmoothingData.inputFilterType = rxConfig()->rc_smoothing_input_type;
rcSmoothingData.inputCutoffSetting = rxConfig()->rc_smoothing_input_cutoff;
rcSmoothingData.derivativeFilterType = rxConfig()->rc_smoothing_derivative_type;
rcSmoothingData.derivativeFilterTypeSetting = rxConfig()->rc_smoothing_derivative_type;
if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_AUTO) {
// for derivative filter type "AUTO" set to BIQUAD for classic FF and PT1 for interpolated FF
if (currentPidProfile->ff_interpolate_sp == FF_INTERPOLATE_OFF) {
rcSmoothingData.derivativeFilterType = RC_SMOOTHING_DERIVATIVE_BIQUAD;
} else {
rcSmoothingData.derivativeFilterType = RC_SMOOTHING_DERIVATIVE_PT1;
}
} else {
rcSmoothingData.derivativeFilterType = rxConfig()->rc_smoothing_derivative_type;
}
rcSmoothingData.derivativeCutoffSetting = rxConfig()->rc_smoothing_derivative_cutoff;
rcSmoothingResetAccumulation(&rcSmoothingData);
rcSmoothingData.inputCutoffFrequency = rcSmoothingData.inputCutoffSetting;
if (rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF) {
rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting;
if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) {
// calculate the fixed derivative cutoff used for interpolated feedforward
const float cutoffFactor = (100 - rcSmoothingData.autoSmoothnessFactor) / 100.0f;
float derivativeCutoff = RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_DERIVATIVE_PT1_HZ * cutoffFactor; // PT1 cutoff frequency
if (rcSmoothingData.derivativeFilterType == RC_SMOOTHING_DERIVATIVE_BIQUAD) {
// convert to an equivalent BIQUAD cutoff
derivativeCutoff = sqrt(derivativeCutoff * RC_SMOOTHING_IDENTITY_FREQUENCY);
}
rcSmoothingData.derivativeCutoffFrequency = lrintf(derivativeCutoff);
} else {
rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting;
}
}
calculateCutoffs = rcSmoothingAutoCalculate();

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

@ -74,7 +74,8 @@ typedef enum {
typedef enum {
RC_SMOOTHING_DERIVATIVE_OFF,
RC_SMOOTHING_DERIVATIVE_PT1,
RC_SMOOTHING_DERIVATIVE_BIQUAD
RC_SMOOTHING_DERIVATIVE_BIQUAD,
RC_SMOOTHING_DERIVATIVE_AUTO,
} rcSmoothingDerivativeFilter_e;
#define ROL_LO (1 << (2 * ROLL))
@ -122,6 +123,7 @@ typedef struct rcSmoothingFilter_s {
rcSmoothingInputFilter_e inputFilterType;
uint8_t inputCutoffSetting;
uint16_t inputCutoffFrequency;
rcSmoothingDerivativeFilter_e derivativeFilterTypeSetting;
rcSmoothingDerivativeFilter_e derivativeFilterType;
uint8_t derivativeCutoffSetting;
uint16_t derivativeCutoffFrequency;

2
src/main/pg/rx.c
View file

@ -67,7 +67,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
.rc_smoothing_derivative_cutoff = 0, // automatically calculate the cutoff by default
.rc_smoothing_debug_axis = ROLL, // default to debug logging for the roll axis
.rc_smoothing_input_type = RC_SMOOTHING_INPUT_BIQUAD,
.rc_smoothing_derivative_type = RC_SMOOTHING_DERIVATIVE_BIQUAD,
.rc_smoothing_derivative_type = RC_SMOOTHING_DERIVATIVE_AUTO, // automatically choose type based on feedforward method
.rc_smoothing_auto_factor = 10,
.srxl2_unit_id = 1,
.srxl2_baud_fast = true,