diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 742219fb50..330199f511 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -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) }, diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 8e887aeb56..294eb8a661 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -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 diff --git a/src/main/common/filter.h b/src/main/common/filter.h index fe00bee8a5..8a88aab2c1 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -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, @@ -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); diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 6cbaed5191..d64f00af32 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -350,15 +350,15 @@ 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); } } } @@ -366,9 +366,9 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi // 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); } } } @@ -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]; @@ -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]; } @@ -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 diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 0c6f276859..dd695aaf6b 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -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; diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c index 0cf557d13c..d71ea1d22c 100644 --- a/src/main/pg/rx.c +++ b/src/main/pg/rx.c @@ -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, diff --git a/src/main/pg/rx.h b/src/main/pg/rx.h index 1be7ecdb75..7c2ca131d7 100644 --- a/src/main/pg/rx.h +++ b/src/main/pg/rx.h @@ -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)