Skip to content

PT1 low pass filter clean up + fixes #11571

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

Open
breadoven wants to merge 7 commits into
base: maintenance-10.x
Choose a base branch
from
Open

PT1 low pass filter clean up + fixes #11571

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
7e7e17e
pt1 filter clean up + fixes
breadoven May 19, 2026
fc1664f
Merge branch 'maintenance-9.x' into abo_pt1filter_cleanup
breadoven May 21, 2026
935623a
fixes
breadoven May 21, 2026
dcc404e
replace pt1FilterApply4 with pt1FilterApply3
breadoven May 22, 2026
fe4751a
use arrays for imu filters
breadoven May 23, 2026
8927515
Merge branch 'maintenance-10.x' into abo_pt1filter_cleanup
breadoven May 29, 2026
db266cc
Update Settings.md
breadoven May 29, 2026
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 docs/Settings.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5946,6 +5946,7 @@ Selection of pitot hardware. VIRTUAL only works if a GPS is enabled.
| FAKE | |
| MSP | |
| DLVR-L10D | |
| MS5525 | |

---

Expand Down
58 changes: 20 additions & 38 deletions src/main/common/filter.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,72 +34,54 @@ float nullFilterApply(void *filter, float input)
return input;
}

float nullFilterApply4(void *filter, float input, float f_cut, float dt)
float nullFilterApply3(void *filter, float input, float dt)
{
UNUSED(filter);
UNUSED(f_cut);
UNUSED(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)
static float 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);
}

void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau) {
filter->RC = tau;
}

float pt1FilterGetLastOutput(pt1Filter_t *filter) {
return filter->state;
return filter->state = filter->state + filter->alpha * (input - filter->state);
}

void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut)
float FAST_CODE NOINLINE pt1FilterApply3(pt1Filter_t *filter, float input, float dT)
{
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;
}

Expand Down
17 changes: 7 additions & 10 deletions src/main/common/filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -88,22 +88,19 @@ typedef struct alphaBetaGammaFilter_s {
} alphaBetaGammaFilter_t;

typedef float (*filterApplyFnPtr)(void *filter, float input);
typedef float (*filterApply4FnPtr)(void *filter, float input, float f_cut, float dt);
typedef float (*filterApply3FnPtr)(void *filter, float input, float dt);

#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/

float nullFilterApply(void *filter, float input);
float nullFilterApply4(void *filter, float input, float f_cut, float dt);

float nullFilterApply3(void *filter, float input, 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);

/*
Expand Down
22 changes: 12 additions & 10 deletions src/main/common/fp_pid.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,9 +46,9 @@ float navPidApply3(
) {
float newProportional, newDerivative, newFeedForward;
float error = 0.0f;

if (pid->errorLpfHz > 0.0f) {
error = pt1FilterApply4(&pid->error_filter_state, setpoint - measurement, pid->errorLpfHz, dt);
error = pt1FilterApply3(&pid->error_filter_state, setpoint - measurement, dt);
} else {
error = setpoint - measurement;
}
Expand All @@ -73,7 +73,7 @@ float navPidApply3(
}

if (pid->dTermLpfHz > 0.0f) {
newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt);
newDerivative = pid->param.kD * pt1FilterApply3(&pid->dterm_filter_state, newDerivative, dt);
} else {
newDerivative = pid->param.kD * newDerivative;
}
Expand Down Expand Up @@ -105,10 +105,7 @@ float navPidApply3(
pid->output_constrained = outValConstrained;

/* Update I-term */
if (
!(pidFlags & PID_ZERO_INTEGRATOR) &&
!(pidFlags & PID_FREEZE_INTEGRATOR)
) {
if (!(pidFlags & PID_ZERO_INTEGRATOR) && !(pidFlags & PID_FREEZE_INTEGRATOR)) {
const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + (backCalc * pid->param.kT * dt);

if (pidFlags & PID_SHRINK_INTEGRATOR) {
Expand All @@ -121,10 +118,10 @@ float navPidApply3(
pid->integrator = newIntegrator;
}
}

if (pidFlags & PID_LIMIT_INTEGRATOR) {
pid->integrator = constrainf(pid->integrator, outMin, outMax);
}
}

return outValConstrained;
}
Expand All @@ -143,8 +140,9 @@ void navPidReset(pidController_t *pid)
pid->integrator = 0.0f;
pid->last_input = 0.0f;
pid->feedForward = 0.0f;
pt1FilterReset(&pid->dterm_filter_state, 0.0f);
pid->output_constrained = 0.0f;

pt1FilterReset(&pid->dterm_filter_state, 0.0f);
}

void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz, float _errorLpfHz)
Expand All @@ -169,5 +167,9 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kF
}
pid->dTermLpfHz = _dTermLpfHz;
pid->errorLpfHz = _errorLpfHz;

pt1FilterSetCutoff(&pid->error_filter_state, pid->errorLpfHz);
pt1FilterSetCutoff(&pid->dterm_filter_state, pid->dTermLpfHz);

navPidReset(pid);
}
59 changes: 23 additions & 36 deletions src/main/flight/imu.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,21 +99,16 @@ FASTRAM float rMat[3][3];

STATIC_FASTRAM imuRuntimeConfig_t imuRuntimeConfig;

STATIC_FASTRAM pt1Filter_t rotRateFilterX;
STATIC_FASTRAM pt1Filter_t rotRateFilterY;
STATIC_FASTRAM pt1Filter_t rotRateFilterZ;
STATIC_FASTRAM pt1Filter_t rotRateFilter[XYZ_AXIS_COUNT];
FASTRAM fpVector3_t imuMeasuredRotationBFFiltered = {.v = {0.0f, 0.0f, 0.0f}};

STATIC_FASTRAM pt1Filter_t accelFilterX;
STATIC_FASTRAM pt1Filter_t accelFilterY;
STATIC_FASTRAM pt1Filter_t accelFilterZ;
STATIC_FASTRAM pt1Filter_t accelFilter[XYZ_AXIS_COUNT];
FASTRAM fpVector3_t imuMeasuredAccelBFFiltered = {.v = {0.0f, 0.0f, 0.0f}};

STATIC_FASTRAM pt1Filter_t HeadVecEFFilterX;
STATIC_FASTRAM pt1Filter_t HeadVecEFFilterY;
STATIC_FASTRAM pt1Filter_t HeadVecEFFilterZ;
STATIC_FASTRAM pt1Filter_t HeadVecEFFilter[XYZ_AXIS_COUNT];
FASTRAM fpVector3_t HeadVecEFFiltered = {.v = {0.0f, 0.0f, 0.0f}};

static pt1Filter_t GPS3DspeedFilter;

FASTRAM bool gpsHeadingInitialized;

Expand Down Expand Up @@ -202,18 +197,19 @@ void imuInit(void)
quaternionInitUnit(&orientation);
imuComputeRotationMatrix();

// Initialize rotation rate filter
pt1FilterReset(&rotRateFilterX, 0);
pt1FilterReset(&rotRateFilterY, 0);
pt1FilterReset(&rotRateFilterZ, 0);
// Initialize accel filter
pt1FilterReset(&accelFilterX, 0);
pt1FilterReset(&accelFilterY, 0);
pt1FilterReset(&accelFilterZ, 0);
// Initialize Heading vector filter
pt1FilterReset(&HeadVecEFFilterX, 0);
pt1FilterReset(&HeadVecEFFilterY, 0);
pt1FilterReset(&HeadVecEFFilterZ, 0);
// Initialize rotation rate, accel and Heading vector filters
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterSetCutoff(&rotRateFilter[axis], IMU_ROTATION_LPF);
pt1FilterReset(&rotRateFilter[axis], 0.0f);

pt1FilterSetCutoff(&accelFilter[axis], IMU_ROTATION_LPF);
pt1FilterReset(&accelFilter[axis], 0.0f);

pt1FilterSetCutoff(&HeadVecEFFilter[axis], IMU_ROTATION_LPF);
pt1FilterReset(&HeadVecEFFilter[axis], 0.0f);
}

pt1FilterSetCutoff(&GPS3DspeedFilter, IMU_ROTATION_LPF);
}

void imuSetMagneticDeclination(float declinationDeg)
Expand Down Expand Up @@ -684,19 +680,11 @@ static float imuCalculateAccelerometerWeightRateIgnore(const float acc_ignore_sl

static void imuCalculateFilters(float dT)
{
//flitering
imuMeasuredRotationBFFiltered.x = pt1FilterApply4(&rotRateFilterX, imuMeasuredRotationBF.x, IMU_ROTATION_LPF, dT);
imuMeasuredRotationBFFiltered.y = pt1FilterApply4(&rotRateFilterY, imuMeasuredRotationBF.y, IMU_ROTATION_LPF, dT);
imuMeasuredRotationBFFiltered.z = pt1FilterApply4(&rotRateFilterZ, imuMeasuredRotationBF.z, IMU_ROTATION_LPF, dT);

imuMeasuredAccelBFFiltered.x = pt1FilterApply4(&accelFilterX, imuMeasuredAccelBF.x, IMU_ROTATION_LPF, dT);
imuMeasuredAccelBFFiltered.y = pt1FilterApply4(&accelFilterY, imuMeasuredAccelBF.y, IMU_ROTATION_LPF, dT);
imuMeasuredAccelBFFiltered.z = pt1FilterApply4(&accelFilterZ, imuMeasuredAccelBF.z, IMU_ROTATION_LPF, dT);

HeadVecEFFiltered.x = pt1FilterApply4(&HeadVecEFFilterX, rMat[0][0], IMU_ROTATION_LPF, dT);
HeadVecEFFiltered.y = pt1FilterApply4(&HeadVecEFFilterY, rMat[1][0], IMU_ROTATION_LPF, dT);
HeadVecEFFiltered.z = pt1FilterApply4(&HeadVecEFFilterZ, rMat[2][0], IMU_ROTATION_LPF, dT);

for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
imuMeasuredRotationBFFiltered.v[axis] = pt1FilterApply3(&rotRateFilter[axis], imuMeasuredRotationBF.v[axis], dT);
imuMeasuredAccelBFFiltered.v[axis] = pt1FilterApply3(&accelFilter[axis], imuMeasuredAccelBF.v[axis], dT);
HeadVecEFFiltered.v[axis] = pt1FilterApply3(&HeadVecEFFilter[axis], rMat[axis][0], dT);
}
}

static void imuCalculateGPSacceleration(fpVector3_t *vEstAccelEF,fpVector3_t *vEstcentrifugalAccelBF, float *acc_ignore_slope_multipiler)
Expand Down Expand Up @@ -732,12 +720,11 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF
if (isGPSTrustworthy()) {
//first speed choice is gps
static bool lastGPSHeartbeat;
static pt1Filter_t GPS3DspeedFilter;
static float GPS3DspeedFiltered = 0.0f;
if (gpsSol.flags.gpsHeartbeat != lastGPSHeartbeat) {
lastGPSHeartbeat = gpsSol.flags.gpsHeartbeat;
float GPS3Dspeed = calc_length_pythagorean_3D(gpsSol.velNED[X], gpsSol.velNED[Y], gpsSol.velNED[Z]);
GPS3DspeedFiltered = pt1FilterApply4(&GPS3DspeedFilter, GPS3Dspeed, IMU_ROTATION_LPF, dT);
GPS3DspeedFiltered = pt1FilterApply3(&GPS3DspeedFilter, GPS3Dspeed, dT);
}
currentspeed = GPS3DspeedFiltered;
*acc_ignore_slope_multipiler = 4.0f;
Expand Down
Loading
Loading