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

PT2 and PT3 filter maths PT3 for RC smoothing

Browse source
This commit is contained in:
ctzsnooze 2021-04-26 11:18:11 +10:00 • committed by mikeller
parent a5d0f7e457
commit a096c99664
14 changed files with 132 additions and 165 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1461,8 +1461,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rcSmoothingData->inputCutoffSetting,
rcSmoothingData->derivativeCutoffSetting);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor", "%d", rcSmoothingData->autoSmoothnessFactor);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_filter_type", "%d, %d", rcSmoothingData->inputFilterType,
rcSmoothingData->derivativeFilterType);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingData->inputCutoffFrequency,
rcSmoothingData->derivativeCutoffFrequency);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingData->averageFrameTimeUs);

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

@ -4967,28 +4967,17 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline)
cliPrintLinef("%d.%03dms", avgRxFrameUs / 1000, avgRxFrameUs % 1000);
}
}
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)");
}
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)");
if (rcSmoothingData->derivativeCutoffSetting == 0) {
cliPrintLine("auto)");
} else {
if (rcSmoothingData->derivativeCutoffSetting == 0) {
cliPrintLine("auto)");
} else {
cliPrintLine("manual)");
}
cliPrintLine("manual)");
}
} else {
cliPrintLine("INTERPOLATION");

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

@ -412,12 +412,6 @@ static const char * const lookupTableRcSmoothingType[] = {
static const char * const lookupTableRcSmoothingDebug[] = {
"ROLL", "PITCH", "YAW", "THROTTLE"
};
static const char * const lookupTableRcSmoothingInputType[] = {
"PT1", "BIQUAD"
};
static const char * const lookupTableRcSmoothingDerivativeType[] = {
"OFF", "PT1", "BIQUAD", "AUTO"
};
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_VTX_COMMON
@ -605,8 +599,6 @@ const lookupTableEntry_t lookupTables[] = {
#ifdef USE_RC_SMOOTHING_FILTER
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingType),
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDebug),
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingInputType),
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType),
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_VTX_COMMON
LOOKUP_TABLE_ENTRY(lookupTableVtxLowPowerDisarm),
@ -750,8 +742,6 @@ const clivalue_t valueTable[] = {
{ "rc_smoothing_input_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_input_cutoff) },
{ "rc_smoothing_derivative_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_derivative_cutoff) },
{ "rc_smoothing_debug_axis", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_DEBUG }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_debug_axis) },
{ "rc_smoothing_input_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_INPUT_TYPE }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_input_type) },
{ "rc_smoothing_derivative_type",VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_DERIVATIVE_TYPE }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_derivative_type) },
{ "rc_smoothing_auto_smoothness",VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { RC_SMOOTHING_AUTO_FACTOR_MIN, RC_SMOOTHING_AUTO_FACTOR_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_auto_factor) },
#endif // USE_RC_SMOOTHING_FILTER

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

@ -112,8 +112,6 @@ typedef enum {
#ifdef USE_RC_SMOOTHING_FILTER
TABLE_RC_SMOOTHING_TYPE,
TABLE_RC_SMOOTHING_DEBUG,
TABLE_RC_SMOOTHING_INPUT_TYPE,
TABLE_RC_SMOOTHING_DERIVATIVE_TYPE,
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_VTX_COMMON
TABLE_VTX_LOW_POWER_DISARM,

65
src/main/common/filter.c
View file

@ -65,6 +65,71 @@ FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input)
return filter->state;
}
// PT2 Low Pass filter
float pt2FilterGain(float f_cut, float dT)
{
const float order = 2.0f;
const float orderCutoffCorrection = (1 / sqrtf(powf(2, 1.0f / (order)) - 1));
float RC = 1 / ( 2 * orderCutoffCorrection * M_PIf * f_cut);
//float RC = 1 / ( 2 * 1.553773974f * M_PIf * f_cut);
// where 1.553773974 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 2
return dT / (RC + dT);
}
void pt2FilterInit(pt2Filter_t *filter, float k)
{
filter->state = 0.0f;
filter->state1 = 0.0f;
filter->k = k;
}
void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k)
{
filter->k = k;
}
FAST_CODE float pt2FilterApply(pt2Filter_t *filter, float input)
{
filter->state1 = filter->state1 + filter->k * (input - filter->state1);
filter->state = filter->state + filter->k * (filter->state1 - filter->state);
return filter->state;
}
// PT3 Low Pass filter
float pt3FilterGain(float f_cut, float dT)
{
const float order = 3.0f;
const float orderCutoffCorrection = (1 / sqrtf(powf(2, 1.0f / (order)) - 1));
float RC = 1 / ( 2 * orderCutoffCorrection * M_PIf * f_cut);
// float RC = 1 / ( 2 * 1.961459177f * M_PIf * f_cut);
// where 1.961459177 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 3
return dT / (RC + dT);
}
void pt3FilterInit(pt3Filter_t *filter, float k)
{
filter->state = 0.0f;
filter->state1 = 0.0f;
filter->state2 = 0.0f;
filter->k = k;
}
void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k)
{
filter->k = k;
}
FAST_CODE float pt3FilterApply(pt3Filter_t *filter, float input)
{
filter->state1 = filter->state1 + filter->k * (input - filter->state1);
filter->state2 = filter->state2 + filter->k * (filter->state1 - filter->state2);
filter->state = filter->state + filter->k * (filter->state2 - filter->state);
return filter->state;
}
// Slew filter with limit
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold)

25
src/main/common/filter.h
View file

@ -29,6 +29,19 @@ typedef struct pt1Filter_s {
float k;
} pt1Filter_t;
typedef struct pt2Filter_s {
float state;
float state1;
float k;
} pt2Filter_t;
typedef struct pt3Filter_s {
float state;
float state1;
float state2;
float k;
} pt3Filter_t;
typedef struct slewFilter_s {
float state;
float slewLimit;
@ -52,6 +65,8 @@ typedef struct laggedMovingAverage_s {
typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD,
FILTER_PT2,
FILTER_PT3,
} lowpassFilterType_e;
typedef enum {
@ -81,5 +96,15 @@ void pt1FilterInit(pt1Filter_t *filter, float k);
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
float pt1FilterApply(pt1Filter_t *filter, float input);
float pt2FilterGain(float f_cut, float dT);
void pt2FilterInit(pt2Filter_t *filter, float k);
void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k);
float pt2FilterApply(pt2Filter_t *filter, float input);
float pt3FilterGain(float f_cut, float dT);
void pt3FilterInit(pt3Filter_t *filter, float k);
void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k);
float pt3FilterApply(pt3Filter_t *filter, float input);
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
float slewFilterApply(slewFilter_t *filter, float input);

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

@ -82,7 +82,6 @@ enum {
};
#ifdef USE_RC_SMOOTHING_FILTER
#define RC_SMOOTHING_IDENTITY_FREQUENCY 80 // Used in the formula to convert a BIQUAD cutoff frequency to PT1
#define RC_SMOOTHING_FILTER_STARTUP_DELAY_MS 5000 // Time to wait after power to let the PID loop stabilize before starting average frame rate calculation
#define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50 // Number of rx frame rate samples to average during initial training
#define RC_SMOOTHING_FILTER_RETRAINING_SAMPLES 20 // Number of rx frame rate samples to average during frame rate changes
@ -91,7 +90,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
#define RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_INITIAL_HZ 100 // Initial value for "auto" when interpolated feedforward is enabled
static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
#endif // USE_RC_SMOOTHING_FILTER
@ -395,16 +394,12 @@ uint16_t getCurrentRxRefreshRate(void)
#ifdef USE_RC_SMOOTHING_FILTER
// Determine a cutoff frequency based on filter type and the calculated
// average rx frame time
FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1, uint8_t autoSmoothnessFactor)
FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, uint8_t autoSmoothnessFactor)
{
if (avgRxFrameTimeUs > 0) {
const float cutoffFactor = (100 - autoSmoothnessFactor) / 100.0f;
float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)) / 2; // calculate the nyquist frequency
const float cutoffFactor = 1.5f / (1.0f + (autoSmoothnessFactor / 10.0f));
float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)); // link frequency
cutoff = cutoff * cutoffFactor;
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;
@ -426,31 +421,17 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
uint16_t oldCutoff = smoothingData->inputCutoffFrequency;
if (smoothingData->inputCutoffSetting == 0) {
smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (smoothingData->inputFilterType == RC_SMOOTHING_INPUT_PT1), smoothingData->autoSmoothnessFactor);
smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactor);
}
// initialize or update the input filter
if ((smoothingData->inputCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
if ((1 << i) & interpolationChannels) { // only update channels specified by rc_interp_ch
switch (smoothingData->inputFilterType) {
case RC_SMOOTHING_INPUT_PT1:
if (!smoothingData->filterInitialized) {
pt1FilterInit((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT));
} else {
pt1FilterUpdateCutoff((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT));
}
break;
case RC_SMOOTHING_INPUT_BIQUAD:
default:
if (!smoothingData->filterInitialized) {
biquadFilterInitLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime);
} else {
biquadFilterUpdateLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime);
}
break;
if (!smoothingData->filterInitialized) {
pt3FilterInit((pt3Filter_t*) &smoothingData->filter[i], pt3FilterGain(smoothingData->inputCutoffFrequency, dT));
} else {
pt3FilterUpdateCutoff((pt3Filter_t*) &smoothingData->filter[i], pt3FilterGain(smoothingData->inputCutoffFrequency, dT));
}
}
}
@ -458,15 +439,12 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
// update or initialize the derivative filter
oldCutoff = smoothingData->derivativeCutoffFrequency;
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);
if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) {
smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactor);
}
if (!smoothingData->filterInitialized) {
pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, smoothingData->debugAxis, smoothingData->derivativeFilterType);
pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, smoothingData->debugAxis);
} else if (smoothingData->derivativeCutoffFrequency != oldCutoff) {
pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency);
}
@ -505,14 +483,7 @@ static FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothing
FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
{
// if the input cutoff is 0 (auto) then we need to calculate cutoffs
if (rcSmoothingData.inputCutoffSetting == 0) {
return 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)) {
if ((rcSmoothingData.inputCutoffSetting == 0) || (rcSmoothingData.derivativeCutoffSetting == 0)) {
return true;
}
return false;
@ -533,39 +504,19 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
rcSmoothingData.averageFrameTimeUs = 0;
rcSmoothingData.autoSmoothnessFactor = rxConfig()->rc_smoothing_auto_factor;
rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
rcSmoothingData.inputFilterType = rxConfig()->rc_smoothing_input_type;
rcSmoothingData.inputCutoffSetting = rxConfig()->rc_smoothing_input_cutoff;
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) {
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;
}
if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) {
// calculate the initial derivative cutoff used for interpolated feedforward until RC interval is known
const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactor / 10.0f));
float derivativeCutoff = RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_INITIAL_HZ * cutoffFactor; // PT1 cutoff frequency
rcSmoothingData.derivativeCutoffFrequency = lrintf(derivativeCutoff);
} else {
rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting;
}
calculateCutoffs = rcSmoothingAutoCalculate();
@ -658,16 +609,7 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
if ((1 << updatedChannel) & interpolationChannels) { // only smooth selected channels base on the rc_interp_ch value
if (rcSmoothingData.filterInitialized) {
switch (rcSmoothingData.inputFilterType) {
case RC_SMOOTHING_INPUT_PT1:
rcCommand[updatedChannel] = pt1FilterApply((pt1Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
break;
case RC_SMOOTHING_INPUT_BIQUAD:
default:
rcCommand[updatedChannel] = biquadFilterApplyDF1((biquadFilter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
break;
}
rcCommand[updatedChannel] = pt3FilterApply((pt3Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
} else {
// If filter isn't initialized yet then use the actual unsmoothed rx channel data
rcCommand[updatedChannel] = lastRxData[updatedChannel];

2
src/main/fc/rc.h
View file

@ -34,7 +34,7 @@ typedef enum {
#ifdef USE_RC_SMOOTHING_FILTER
#define RC_SMOOTHING_AUTO_FACTOR_MIN 0
#define RC_SMOOTHING_AUTO_FACTOR_MAX 50
#define RC_SMOOTHING_AUTO_FACTOR_MAX 250
#endif
void processRcCommand(void);

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

@ -70,18 +70,6 @@ typedef enum {
RC_SMOOTHING_TYPE_FILTER
} rcSmoothingType_e;
typedef enum {
RC_SMOOTHING_INPUT_PT1,
RC_SMOOTHING_INPUT_BIQUAD
} rcSmoothingInputFilter_e;
typedef enum {
RC_SMOOTHING_DERIVATIVE_OFF,
RC_SMOOTHING_DERIVATIVE_PT1,
RC_SMOOTHING_DERIVATIVE_BIQUAD,
RC_SMOOTHING_DERIVATIVE_AUTO,
} rcSmoothingDerivativeFilter_e;
#define ROL_LO (1 << (2 * ROLL))
#define ROL_CE (3 << (2 * ROLL))
#define ROL_HI (2 << (2 * ROLL))
@ -116,19 +104,11 @@ typedef struct rcSmoothingFilterTraining_s {
uint16_t max;
} rcSmoothingFilterTraining_t;
typedef union rcSmoothingFilterTypes_u {
pt1Filter_t pt1Filter;
biquadFilter_t biquadFilter;
} rcSmoothingFilterTypes_t;
typedef struct rcSmoothingFilter_s {
bool filterInitialized;
rcSmoothingFilterTypes_t filter[4];
rcSmoothingInputFilter_e inputFilterType;
pt3Filter_t filter[4];
uint8_t inputCutoffSetting;
uint16_t inputCutoffFrequency;
rcSmoothingDerivativeFilter_e derivativeFilterTypeSetting;
rcSmoothingDerivativeFilter_e derivativeFilterType;
uint8_t derivativeCutoffSetting;
uint16_t derivativeCutoffFrequency;
int averageFrameTimeUs;

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

@ -205,7 +205,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.dyn_idle_max_increase = 150,
.ff_interpolate_sp = FF_INTERPOLATE_ON,
.ff_max_rate_limit = 100,
.ff_smooth_factor = 37,
.ff_smooth_factor = 0,
.ff_boost = 15,
.dyn_lpf_curve_expo = 5,
.level_race_mode = false,
@ -638,14 +638,7 @@ float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelt
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
}
if (pidRuntime.setpointDerivativeLpfInitialized) {
switch (pidRuntime.rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
ret = pt1FilterApply(&pidRuntime.setpointDerivativePt1[axis], pidSetpointDelta);
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
ret = biquadFilterApplyDF1(&pidRuntime.setpointDerivativeBiquad[axis], pidSetpointDelta);
break;
}
ret = pt3FilterApply(&pidRuntime.setpointDerivativePt3[axis], pidSetpointDelta);
if (axis == pidRuntime.rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f));
}

3
src/main/flight/pid.h
View file

@ -340,8 +340,7 @@ typedef struct pidRuntime_s {
#endif
#ifdef USE_RC_SMOOTHING_FILTER
pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
pt3Filter_t setpointDerivativePt3[XYZ_AXIS_COUNT];
bool setpointDerivativeLpfInitialized;
uint8_t rcSmoothingDebugAxis;
uint8_t rcSmoothingFilterType;

32
src/main/flight/pid_init.c
View file

@ -222,37 +222,22 @@ void pidInit(const pidProfile_t *pidProfile)
}
#ifdef USE_RC_SMOOTHING_FILTER
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType)
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis)
{
pidRuntime.rcSmoothingDebugAxis = debugAxis;
pidRuntime.rcSmoothingFilterType = filterType;
if ((filterCutoff > 0) && (pidRuntime.rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
if (filterCutoff > 0) {
pidRuntime.setpointDerivativeLpfInitialized = true;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
switch (pidRuntime.rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
pt1FilterInit(&pidRuntime.setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, pidRuntime.dT));
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
biquadFilterInitLPF(&pidRuntime.setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
break;
}
pt3FilterInit(&pidRuntime.setpointDerivativePt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
}
}
}
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
{
if ((filterCutoff > 0) && (pidRuntime.rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
if (filterCutoff > 0) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
switch (pidRuntime.rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
pt1FilterUpdateCutoff(&pidRuntime.setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, pidRuntime.dT));
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
biquadFilterUpdateLPF(&pidRuntime.setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
break;
}
pt3FilterUpdateCutoff(&pidRuntime.setpointDerivativePt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
}
}
}
@ -401,7 +386,12 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#ifdef USE_INTERPOLATED_SP
pidRuntime.ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp;
pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f;
if (pidProfile->ff_smooth_factor) {
pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f;
} else {
// set automatically according to boost amount, limit to 0.5 for auto
pidRuntime.ffSmoothFactor = MAX(0.5f, 1.0f - ((float)pidProfile->ff_boost) * 2.0f / 100.0f);
}
interpolatedSpInit(pidProfile);
#endif

2
src/main/flight/pid_init.h
View file

@ -24,6 +24,6 @@ void pidInit(const pidProfile_t *pidProfile);
void pidInitFilters(const pidProfile_t *pidProfile);
void pidInitConfig(const pidProfile_t *pidProfile);
void pidSetItermAccelerator(float newItermAccelerator);
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis);
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);
void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);

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

@ -66,9 +66,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
.rc_smoothing_input_cutoff = 0, // automatically calculate the cutoff by default
.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_AUTO, // automatically choose type based on feedforward method
.rc_smoothing_auto_factor = 10,
.rc_smoothing_auto_factor = 30,
.srxl2_unit_id = 1,
.srxl2_baud_fast = true,
.sbus_baud_fast = false,