Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Selectable PTn Filter Order for RC Filtering #787

Draft
wants to merge 1 commit into
base: emuflight-1.0.0-master
Choose a base branch
from
Draft

Selectable PTn Filter Order for RC Filtering #787

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions src/main/cli/settings.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -756,6 +756,7 @@ const clivalue_t valueTable[] = {

#ifdef USE_RC_SMOOTHING_FILTER
{ PARAM_NAME_RC_SMOOTHING, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_mode) },
{ "rc_smoothing_order", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 3 }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_mode) },
{ PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR, 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_rpy) },
{ PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE, 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_throttle) },
{ PARAM_NAME_RC_SMOOTHING_SETPOINT_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_setpoint_cutoff) },
Expand Down
34 changes: 34 additions & 0 deletions src/main/common/filter.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -309,3 +309,37 @@ void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpS
filter->beta = beta;
filter->fpShift = fpShift;
}

const float PTN_SCALE[3] = { 1.0f, 1.553773974f, 1.961459177f };

void ptnFilterInit(ptnFilter_t *filter, uint8_t order, uint16_t f_cut, float dT) {

// AdjCutHz = CutHz /(sqrtf(powf(2, 1/Order) -1))
const float ScaleF[] = { 1.0f, 1.553773974f, 1.961459177f };
float Adj_f_cut;

filter->order = (order > 3) ? 3 : order;
for (int n = 1; n <= filter->order; n++) {
filter->state[n] = 0.0f;
}

Adj_f_cut = f_cut * PTN_SCALE[filter->order - 1];

filter->k = dT / ((1.0f / (2.0f * M_PIf * Adj_f_cut)) + dT);
} // ptnFilterInit

FAST_CODE void ptnFilterUpdate(ptnFilter_t *filter, float f_cut, float dT) {
float Adj_f_cut;
Adj_f_cut = f_cut * PTN_SCALE[filter->order - 1];
filter->k = dT / ((1.0f / (2.0f * M_PIf * Adj_f_cut)) + dT);
}

FAST_CODE float ptnFilterApply(ptnFilter_t *filter, float input) {
filter->state[0] = input;

for (int n = 1; n <= filter->order; n++) {
filter->state[n] += (filter->state[n - 1] - filter->state[n]) * filter->k;
}

return filter->state[filter->order];
} // ptnFilterApply
22 changes: 16 additions & 6 deletions src/main/common/filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,18 @@ typedef struct laggedMovingAverage_s {
bool primed;
} laggedMovingAverage_t;

typedef struct simpleLowpassFilter_s {
int32_t fp;
int32_t beta;
int32_t fpShift;
} simpleLowpassFilter_t;

typedef struct ptnFilter_s {
float state[4];
float k;
uint8_t order;
} ptnFilter_t;

typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD,
Expand Down Expand Up @@ -111,11 +123,9 @@ float pt3FilterApply(pt3Filter_t *filter, float input);
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
float slewFilterApply(slewFilter_t *filter, float input);

typedef struct simpleLowpassFilter_s {
int32_t fp;
int32_t beta;
int32_t fpShift;
} simpleLowpassFilter_t;

int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal);
void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift);

void ptnFilterInit(ptnFilter_t *filter, uint8_t order, uint16_t f_cut, float dT);
void ptnFilterUpdate(ptnFilter_t *filter, float f_cut, float dt);
float ptnFilterApply(ptnFilter_t *filter, float input);
18 changes: 9 additions & 9 deletions src/main/fc/rc.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -350,25 +350,25 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
if (i < THROTTLE) { // Throttle handled by smoothing rcCommand
if (!smoothingData->filterInitialized) {
pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
ptnFilterInit(&smoothingData->filter[i], rxConfig()->rc_smoothing_order, smoothingData->setpointCutoffFrequency, dT);
} else {
pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
ptnFilterUpdate(&smoothingData->filter[i], smoothingData->setpointCutoffFrequency, dT);
}
} else {
if (!smoothingData->filterInitialized) {
pt3FilterInit(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
ptnFilterInit(&smoothingData->filter[i], rxConfig()->rc_smoothing_order, smoothingData->throttleCutoffFrequency, dT);
} else {
pt3FilterUpdateCutoff(&smoothingData->filter[i], pt3FilterGain(smoothingData->throttleCutoffFrequency, dT));
ptnFilterUpdate(&smoothingData->filter[i], smoothingData->throttleCutoffFrequency, dT);
}
}
}

// initialize or update the Level filter
for (int i = FD_ROLL; i < FD_YAW; i++) {
if (!smoothingData->filterInitialized) {
pt3FilterInit(&smoothingData->filterDeflection[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
ptnFilterInit(&smoothingData->filterDeflection[i], rxConfig()->rc_smoothing_order, smoothingData->setpointCutoffFrequency, dT);
} else {
pt3FilterUpdateCutoff(&smoothingData->filterDeflection[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
ptnFilterUpdate(&smoothingData->filterDeflection[i], smoothingData->setpointCutoffFrequency, dT);
}
}
}
Expand Down Expand Up @@ -546,7 +546,7 @@ static FAST_CODE void processRcSmoothingFilter(void)
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
float *dst = i == THROTTLE ? &rcCommand[i] : &setpointRate[i];
if (rcSmoothingData.filterInitialized) {
*dst = pt3FilterApply(&rcSmoothingData.filter[i], rxDataToSmooth[i]);
*dst = ptnFilterApply(&rcSmoothingData.filter[i], rxDataToSmooth[i]);
} else {
// If filter isn't initialized yet, as in smoothing off, use the actual unsmoothed rx channel data
*dst = rxDataToSmooth[i];
Expand All @@ -557,7 +557,7 @@ static FAST_CODE void processRcSmoothingFilter(void)
bool smoothingNeeded = (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && rcSmoothingData.filterInitialized;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
if (smoothingNeeded && axis < FD_YAW) {
rcDeflectionSmoothed[axis] = pt3FilterApply(&rcSmoothingData.filterDeflection[axis], rcDeflection[axis]);
rcDeflectionSmoothed[axis] = ptnFilterApply(&rcSmoothingData.filterDeflection[axis], rcDeflection[axis]);
} else {
rcDeflectionSmoothed[axis] = rcDeflection[axis];
}
Expand Down Expand Up @@ -586,7 +586,7 @@ FAST_CODE void processRcCommand(void)
#endif

float angleRate;

#ifdef USE_GPS_RESCUE
if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) {
// If GPS Rescue is active then override the setpointRate used in the
Expand Down
4 changes: 2 additions & 2 deletions src/main/fc/rc_controls.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,8 +95,8 @@ typedef struct rcSmoothingFilterTraining_s {

typedef struct rcSmoothingFilter_s {
bool filterInitialized;
pt3Filter_t filter[4];
pt3Filter_t filterDeflection[2];
ptnFilter_t filter[4];
ptnFilter_t filterDeflection[2];
uint8_t setpointCutoffSetting;
uint8_t throttleCutoffSetting;
uint16_t setpointCutoffFrequency;
Expand Down
1 change: 1 addition & 0 deletions src/main/pg/rx.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
.airModeActivateThreshold = 25,
.max_aux_channel = DEFAULT_AUX_CHANNEL_COUNT,
.rc_smoothing_mode = 1,
.rc_smoothing_order = 3,
.rc_smoothing_setpoint_cutoff = 0,
.rc_smoothing_feedforward_cutoff = 0,
.rc_smoothing_throttle_cutoff = 0,
Expand Down
1 change: 1 addition & 0 deletions src/main/pg/rx.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ typedef struct rxConfig_s {
uint8_t rssi_src_frame_errors; // true to use frame drop flags in the rx protocol
int8_t rssi_offset; // offset applied to the RSSI value before it is returned
uint8_t rc_smoothing_mode; // Whether filter based rc smoothing is on or off
uint8_t rc_smoothing_order; // choose from pt1-pt4 for rc smoothing
uint8_t rc_smoothing_setpoint_cutoff; // Filter cutoff frequency for the setpoint filter (0 = auto)
uint8_t rc_smoothing_feedforward_cutoff; // Filter cutoff frequency for the feedforward filter (0 = auto)
uint8_t rc_smoothing_throttle_cutoff; // Filter cutoff frequency for the setpoint filter (0 = auto)
Expand Down