rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Update RC smoothing "auto" settings to consider interpolated feedforward 

Allows default settings for RC smoothing to work seamlessly regardless of the feedforward type selected ("classic" vs. interpolated).

Adds a new "AUTO" setting for the derivative filter type that will select based on whether interpolated feedforward is enabled (use PT1) or not (use BIQUAD). The derivative filter cutoff if set to auto (0) will also use a fixed cutoff calculated from a 100hz base if interpolated feedforward is enabled. Otherwise if classic FF is active then it will default to the previous method of calculating the cutoff based on the RX frame rate.
This commit is contained in:
Bruce Luckcuck 2019-11-28 11:34:52 -05:00
parent cb538ea2ed
commit b2a50819a3
5 changed files with 50 additions and 19 deletions
Show Stats 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,