diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 2e9e4574e71..82eb0e3af61 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -42,64 +42,52 @@ float nullFilterApply4(void *filter, float input, float f_cut, float dt) return input; } -// PT1 Low Pass filter +/* PT1 Low Pass filter + * f_cut = cutoff frequency. Use pt1FilterSetTimeConstant to directly set RC time constant tau if required. + */ static float FAST_CODE pt1ComputeRC(const float f_cut) { return 1.0f / (2.0f * M_PIf * f_cut); } -// f_cut = cutoff frequency -void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT) +void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT) { - filter->state = 0.0f; - filter->RC = tau; + filter->RC = pt1ComputeRC(f_cut); filter->dT = dT; filter->alpha = filter->dT / (filter->RC + filter->dT); + filter->state = 0.0f; } -void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT) +float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input) // use with pt1FilterInit if dT and f_cut are constants { - pt1FilterInitRC(filter, pt1ComputeRC(f_cut), dT); + return filter->state = filter->state + filter->alpha * (input - filter->state); } -void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau) { - filter->RC = tau; -} - -float pt1FilterGetLastOutput(pt1Filter_t *filter) { - return filter->state; -} - -void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut) +float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT) { - filter->RC = pt1ComputeRC(f_cut); + // Pre calculate and store RC only if f_cut non zero + if (f_cut && !filter->RC) { + filter->RC = pt1ComputeRC(f_cut); + } + + filter->dT = dT; // cache latest dT for possible use in pt1FilterApply filter->alpha = filter->dT / (filter->RC + filter->dT); -} -float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input) -{ - filter->state = filter->state + filter->alpha * (input - filter->state); - return filter->state; + return filter->state = filter->state + filter->alpha * (input - filter->state); } -float pt1FilterApply3(pt1Filter_t *filter, float input, float dT) -{ - filter->dT = dT; - filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state); - return filter->state; +void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau) { + filter->RC = tau; } -float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT) +void pt1FilterSetCutoff(pt1Filter_t *filter, float f_cut) { - // Pre calculate and store RC - if (!filter->RC) { - filter->RC = pt1ComputeRC(f_cut); - } - - filter->dT = dT; // cache latest dT for possible use in pt1FilterApply + filter->RC = pt1ComputeRC(f_cut); filter->alpha = filter->dT / (filter->RC + filter->dT); - filter->state = filter->state + filter->alpha * (input - filter->state); +} + +float pt1FilterGetLastOutput(pt1Filter_t *filter) { return filter->state; } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 9be86b495ce..e7d45c66b08 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -47,8 +47,8 @@ typedef struct biquadFilter_s { float x1, x2, y1, y2; } biquadFilter_t; -typedef union { - biquadFilter_t biquad; +typedef union { + biquadFilter_t biquad; pt1Filter_t pt1; pt2Filter_t pt2; pt3Filter_t pt3; @@ -97,13 +97,11 @@ float nullFilterApply(void *filter, float input); float nullFilterApply4(void *filter, float input, float f_cut, float dt); void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT); -void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT); -void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau); -void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut); -float pt1FilterGetLastOutput(pt1Filter_t *filter); float pt1FilterApply(pt1Filter_t *filter, float input); -float pt1FilterApply3(pt1Filter_t *filter, float input, float dT); float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt); +void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau); +void pt1FilterSetCutoff(pt1Filter_t *filter, float f_cut); +float pt1FilterGetLastOutput(pt1Filter_t *filter); void pt1FilterReset(pt1Filter_t *filter, float input); /* diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 354ffeff371..8974a03163e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -150,7 +150,7 @@ static EXTENDED_FASTRAM float antigravityAccelerator; #endif #define D_BOOST_GYRO_LPF_HZ 80 // Biquad lowpass input cutoff to peak D around propwash frequencies -#define D_BOOST_LPF_HZ 7 // PT1 lowpass cutoff to smooth the boost effect +#define D_BOOST_LPF_HZ 7.0f // PT1 lowpass cutoff to smooth the boost effect #ifdef USE_D_BOOST static EXTENDED_FASTRAM float dBoostMin; @@ -384,7 +384,8 @@ bool pidInitFilters(void) void pidResetTPAFilter(void) { if (usedPidControllerType == PID_TYPE_PIFF && currentControlProfile->throttle.fixedWingTauMs > 0) { - pt1FilterInitRC(&fixedWingTpaFilter, MS2S(currentControlProfile->throttle.fixedWingTauMs), US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ))); + pt1FilterInit(&fixedWingTpaFilter, 1.0f, HZ2S(TASK_AUX_RATE_HZ)); + pt1FilterSetTimeConstant(&fixedWingTpaFilter, MS2S(currentControlProfile->throttle.fixedWingTauMs)); pt1FilterReset(&fixedWingTpaFilter, getThrottleIdleValue()); } } @@ -549,7 +550,7 @@ void updatePIDCoefficients(void) for (int axis = 0; axis < 3; axis++) { pidState[axis].stickPosition = constrain(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE, -500, 500) / 500.0f; } - + float tpaFactor=1.0f; float iTermFactor=1.0f; // Separate factor for I-term scaling if(usedPidControllerType == PID_TYPE_PIFF){ // Fixed wing TPA calculation @@ -569,13 +570,13 @@ void updatePIDCoefficients(void) } tpaFactorprev = tpaFactor; - + // If nothing changed - don't waste time recalculating coefficients if (!pidGainsUpdateRequired) { return; } - + // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change for (int axis = 0; axis < 3; axis++) { @@ -1378,12 +1379,8 @@ void pidInit(void) pidState[axis].axis = axis; pidState[axis].pidSumLimit = getPidSumLimit(axis); - if (axis == FD_YAW) { - if (yawLpfHz) { - pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) pt1FilterApply4; - } else { - pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4; - } + if (axis == FD_YAW && yawLpfHz) { + pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) pt1FilterApply4; } else { pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index ff2e85031b7..1c1c9b13ce5 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -33,7 +33,7 @@ #define AXIS_ACCEL_MIN_LIMIT 50 -#define HEADING_HOLD_ERROR_LPF_FREQ 2 +#define HEADING_HOLD_ERROR_LPF_FREQ 2.0f /* FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13 diff --git a/src/main/flight/power_limits.c b/src/main/flight/power_limits.c index 1770fd71a7f..273c8ebce1f 100644 --- a/src/main/flight/power_limits.c +++ b/src/main/flight/power_limits.c @@ -86,8 +86,7 @@ void powerLimiterInit(void) { burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentTime * 1e6; burstCurrentReserveFalldown = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentFalldownTime * 1e6; - pt1FilterInit(¤tThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0); - pt1FilterInitRC(¤tThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0); + pt1FilterSetTimeConstant(¤tThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST); #ifdef USE_ADC // Only enforce burst >= continuous if burst is enabled (non-zero) @@ -102,8 +101,7 @@ void powerLimiterInit(void) { burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerTime * 1e6; burstPowerReserveFalldown = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerFalldownTime * 1e6; - pt1FilterInit(&powerThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0); - pt1FilterInitRC(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0); + pt1FilterSetTimeConstant(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST); #endif } @@ -150,6 +148,7 @@ void powerLimiterApply(int16_t *throttleCommand) { static timeUs_t lastCallTimestamp = 0; timeUs_t currentTimeUs = micros(); timeDelta_t callTimeDelta = cmpTimeUs(currentTimeUs, lastCallTimestamp); + const float attnFilterCutoff = powerLimitsConfig()->attnFilterCutoff; int16_t throttleBase; int16_t currentThrottleCommand; @@ -172,9 +171,9 @@ void powerLimiterApply(int16_t *throttleCommand) { float currentThrAttnProportional = MAX(0, overCurrent) * powerLimitsConfig()->piP * 1e-3f; - uint16_t currentThrAttn = lrintf(pt1FilterApply3(¤tThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, callTimeDelta * 1e-6f)); + uint16_t currentThrAttn = lrintf(pt1FilterApply4(¤tThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, attnFilterCutoff, US2S(callTimeDelta))); - throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply3(¤tThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6f)) : *throttleCommand; + throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply4(¤tThrLimitingBaseFilter, *throttleCommand, 0.0f, US2S(callTimeDelta))) : *throttleCommand; uint16_t currentThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - currentThrAttn); if (activeCurrentLimit && currentThrAttned < *throttleCommand) { @@ -201,9 +200,9 @@ void powerLimiterApply(int16_t *throttleCommand) { float powerThrAttnProportional = MAX(0, overPower) * powerLimitsConfig()->piP / voltage * 1e-1f; - uint16_t powerThrAttn = lrintf(pt1FilterApply3(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, callTimeDelta * 1e-6f)); + uint16_t powerThrAttn = lrintf(pt1FilterApply4(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, attnFilterCutoff, US2S(callTimeDelta))); - throttleBase = wasLimitingPower ? lrintf(pt1FilterApply3(&powerThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand; + throttleBase = wasLimitingPower ? lrintf(pt1FilterApply4(&powerThrLimitingBaseFilter, *throttleCommand, 0.0f, US2S(callTimeDelta))) : *throttleCommand; uint16_t powerThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - powerThrAttn); if (activePowerLimit && powerThrAttned < *throttleCommand) { diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 9f6eb4851a7..9f17780d517 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -206,7 +206,7 @@ int getServoCount(void) void loadCustomServoMixer(void) { - + //move the rate filter to new servo rules int movefilterCount = 0; static servoMixerSwitch_t servoMixerSwitchHelper[MAX_SERVO_RULES_SWITCH_CARRY]; // helper to keep track of servoSpeedLimitFilter of servo rules @@ -603,7 +603,7 @@ void processServoAutotrimMode(void) } } -#define SERVO_AUTOTRIM_FILTER_CUTOFF 1 // LPF cutoff frequency +#define SERVO_AUTOTRIM_FILTER_CUTOFF 1.0f // LPF cutoff frequency #define SERVO_AUTOTRIM_CENTER_MIN 1300 #define SERVO_AUTOTRIM_CENTER_MAX 1700 #define SERVO_AUTOTRIM_UPDATE_SIZE 5 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 55bca838f34..a8d2352cf88 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -125,7 +125,7 @@ #define VIDEO_BUFFER_CHARS_HDZERO 900 #define VIDEO_BUFFER_CHARS_DJIWTF 1320 -#define GFORCE_FILTER_TC 0.2 +#define GFORCE_FILTER_T_CUT_HZ 0.8f #define OSD_STATS_SINGLE_PAGE_MIN_ROWS 18 #define IS_HI(X) (rxGetChannelValue(X) > 1750) @@ -1313,7 +1313,7 @@ uint16_t osdGetRemainingGlideTime(void) { const timeMs_t curTimeMs = millis(); static timeMs_t glideTimeUpdatedMs; - value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs)); + value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0.0f, 0.5f, MS2S(curTimeMs - glideTimeUpdatedMs)); glideTimeUpdatedMs = curTimeMs; if (value < 0) { @@ -2071,7 +2071,7 @@ static bool osdDrawSingleElement(uint8_t item) const timeMs_t currentTimeMs = millis(); static timeMs_t gsUpdatedTimeMs; float glideSlope = horizontalSpeed / sinkRate; - glideSlope = pt1FilterApply4(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200, 0.5, MS2S(currentTimeMs - gsUpdatedTimeMs)); + glideSlope = pt1FilterApply4(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200.0f, 0.5f, MS2S(currentTimeMs - gsUpdatedTimeMs)); gsUpdatedTimeMs = currentTimeMs; buff[0] = SYM_GLIDESLOPE; @@ -3113,7 +3113,7 @@ static bool osdDrawSingleElement(uint8_t item) if (getEstimatedActualVelocity(Z) > 0) { if (vEfficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { // Centiamps (kept for osdFormatCentiNumber) / m/s - Will appear as A / m/s in OSD - value = pt1FilterApply4(&veFilterState, (float)getAmperage() / (getEstimatedActualVelocity(Z) / 100.0f), 1, US2S(vEfficiencyTimeDelta)); + value = pt1FilterApply4(&veFilterState, (float)getAmperage() / (getEstimatedActualVelocity(Z) / 100.0f), 1.0f, US2S(vEfficiencyTimeDelta)); vEfficiencyUpdated = currentTimeUs; } else { @@ -3592,8 +3592,7 @@ static bool osdDrawSingleElement(uint8_t item) #endif ) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { - value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, - 1, US2S(efficiencyTimeDelta)); + value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1.0f, US2S(efficiencyTimeDelta)); efficiencyUpdated = currentTimeUs; } else { @@ -3667,8 +3666,7 @@ static bool osdDrawSingleElement(uint8_t item) #endif ) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { - value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, - 1, US2S(efficiencyTimeDelta)); + value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, 1.0f, US2S(efficiencyTimeDelta)); efficiencyUpdated = currentTimeUs; } else { @@ -5785,26 +5783,22 @@ static void osdShowArmed(void) } } -static void osdFilterData(timeUs_t currentTimeUs) { +static void osdFilterData(timeUs_t currentTimeUs) +{ static timeUs_t lastRefresh = 0; float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh)); GForce = fast_fsqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS; - for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS; + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) { + GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS; + } if (lastRefresh) { - GForce = pt1FilterApply3(&GForceFilter, GForce, refresh_dT); - for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) pt1FilterApply3(GForceFilterAxis + axis, GForceAxis[axis], refresh_dT); - } else { - pt1FilterInitRC(&GForceFilter, GFORCE_FILTER_TC, 0); - pt1FilterReset(&GForceFilter, GForce); - + GForce = pt1FilterApply4(&GForceFilter, GForce, GFORCE_FILTER_T_CUT_HZ, refresh_dT); for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) { - pt1FilterInitRC(GForceFilterAxis + axis, GFORCE_FILTER_TC, 0); - pt1FilterReset(GForceFilterAxis + axis, GForceAxis[axis]); + GForceAxis[axis] = pt1FilterApply4(&GForceFilterAxis[axis], GForceAxis[axis], GFORCE_FILTER_T_CUT_HZ, refresh_dT); } } - lastRefresh = currentTimeUs; } diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index a8adf663d57..bac80fd7430 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -785,8 +785,7 @@ static void osdDJIEfficiencyMahPerKM(char *buff) #endif ) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { - value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, - 1, US2S(efficiencyTimeDelta)); + value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1.0f, US2S(efficiencyTimeDelta)); efficiencyUpdated = currentTimeUs; } else { diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 7ec861e934c..6bdd5d9e7b3 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -313,7 +313,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) const float baroDtSec = US2S(baroDtUs); posEstimator.baro.updateDt = baroDtSec; - posEstimator.baro.alt = pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, baroDtSec); + posEstimator.baro.alt = pt1FilterApply4(&posEstimator.baro.avgFilter, posEstimator.baro.alt, INAV_BARO_AVERAGE_HZ, baroDtSec); // baro altitude rate static float baroAltPrevious = 0; @@ -970,9 +970,6 @@ void initializePositionEstimator(void) posEstimator.est.pos.v[axis] = 0; posEstimator.est.vel.v[axis] = 0; } - - pt1FilterInit(&posEstimator.baro.avgFilter, INAV_BARO_AVERAGE_HZ, 0.0f); - pt1FilterInit(&posEstimator.surface.avgFilter, INAV_SURFACE_AVERAGE_HZ, 0.0f); } /** diff --git a/src/main/navigation/navigation_pos_estimator_agl.c b/src/main/navigation/navigation_pos_estimator_agl.c index 38788e20a77..e102a7ead6e 100644 --- a/src/main/navigation/navigation_pos_estimator_agl.c +++ b/src/main/navigation/navigation_pos_estimator_agl.c @@ -80,7 +80,7 @@ void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfa // Update average sonar altitude if range is good if (surfaceMeasurementWithinRange) { - pt1FilterApply3(&posEstimator.surface.avgFilter, newSurfaceAlt, surfaceDt); + pt1FilterApply4(&posEstimator.surface.avgFilter, newSurfaceAlt, INAV_SURFACE_AVERAGE_HZ, surfaceDt); } } } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index a1f07e470c0..6f076659caf 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -28,7 +28,7 @@ #include "navigation/navigation.h" #define MIN_POSITION_UPDATE_RATE_HZ 5 // Minimum position update rate at which XYZ controllers would be applied -#define NAV_THROTTLE_CUTOFF_FREQENCY_HZ 4 // low-pass filter on throttle output +#define NAV_THROTTLE_CUTOFF_FREQENCY_HZ 4.0f // low-pass filter on throttle output #define NAV_FW_CONTROL_MONITORING_RATE 2 #define NAV_DTERM_CUT_HZ 10.0f #define NAV_VEL_Z_DERIVATIVE_CUT_HZ 5.0f diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 79706c8dba7..2b27ad3d314 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -707,8 +707,8 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta) } if (impedanceFilterState.state) { - pt1FilterSetTimeConstant(&impedanceFilterState, impedanceSampleCount > IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH ? 1.2 : 0.5); - pt1FilterApply3(&impedanceFilterState, impedanceSample, US2S(timeDelta)); + pt1FilterSetTimeConstant(&impedanceFilterState, impedanceSampleCount > IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH ? 1.2f : 0.5f); + pt1FilterApply4(&impedanceFilterState, impedanceSample, 0.0f, US2S(timeDelta)); } else { pt1FilterReset(&impedanceFilterState, impedanceSample); } @@ -721,8 +721,8 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta) } uint16_t sagCompensatedVBatSample = MIN(batteryFullVoltage, vbat + (int32_t)powerSupplyImpedance * amperage / 1000); - pt1FilterSetTimeConstant(&sagCompVBatFilterState, sagCompensatedVBatSample < pt1FilterGetLastOutput(&sagCompVBatFilterState) ? 40 : 500); - sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, US2S(timeDelta))); + pt1FilterSetTimeConstant(&sagCompVBatFilterState, sagCompensatedVBatSample < pt1FilterGetLastOutput(&sagCompVBatFilterState) ? 40.0f : 500.0f); + sagCompensatedVBat = lrintf(pt1FilterApply4(&sagCompVBatFilterState, sagCompensatedVBatSample, 0.0f, US2S(timeDelta))); } DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 0, powerSupplyImpedance); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 8544b45dac1..20d60b3955b 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -332,7 +332,7 @@ bool gyroInit(void) // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value gyro.targetLooptime = gyroDev[0].sampleRateIntervalUs; - + gyroInitFilters(); #ifdef USE_DYNAMIC_FILTERS @@ -505,7 +505,7 @@ void FAST_CODE NOINLINE gyroFilter(void) } /** - * Secondary dynamic notch filter. + * Secondary dynamic notch filter. * In some cases, noise amplitude is high enough not to be filtered by the primary filter. * This happens on the first frequency with the biggest aplitude */ @@ -534,7 +534,7 @@ void FAST_CODE NOINLINE gyroFilter(void) ); secondaryDynamicGyroNotchFiltersUpdate( - &secondaryDynamicGyroNotchState, + &secondaryDynamicGyroNotchState, gyroAnalyseState.filterUpdateAxis, gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis] ); @@ -612,7 +612,7 @@ int16_t gyroRateDps(int axis) void gyroUpdateDynamicLpf(float cutoffFreq) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - pt1FilterUpdateCutoff(&gyroLpf2State[axis].pt1, cutoffFreq); + pt1FilterSetCutoff(&gyroLpf2State[axis].pt1, cutoffFreq); } } diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 3618923614a..cee05b2b477 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -241,9 +241,6 @@ STATIC_PROTOTHREAD(pitotThread) // Init filter pitot.lastMeasurementUs = micros(); - if (pitotmeterConfig()->pitot_lpf_milli_hz > 0) { - pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f); - } while(1) { #ifdef USE_SIMULATOR @@ -296,8 +293,9 @@ STATIC_PROTOTHREAD(pitotThread) // NOTE ::filter pressure - apply filter when NOT calibrating for zero !!! currentTimeUs = micros(); - if (pitotmeterConfig()->pitot_lpf_milli_hz > 0) { - pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs)); + const uint16_t pitot_lpf_milli_hz = pitotmeterConfig()->pitot_lpf_milli_hz; + if (pitot_lpf_milli_hz) { + pitot.pressure = pt1FilterApply4(&pitot.lpfState, pitotPressureTmp, pitot_lpf_milli_hz, US2S(currentTimeUs - pitot.lastMeasurementUs)); } else { pitot.pressure = pitotPressureTmp; }