Merge pull request #11571 from breadoven/abo_pt1filter_cleanup

PT1 low pass filter clean up + fixes

Author: breadoven

docs/Settings.md

@@ -5946,6 +5946,7 @@ Selection of pitot hardware. VIRTUAL only works if a GPS is enabled.
 | FAKE |  |
 | MSP |  |
 | DLVR-L10D |  |
+| MS5525 |  |
 
 ---
 

src/main/common/filter.c

@@ -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;
 }
 

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;
@@ -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);
 
 /*

src/main/common/fp_pid.c

@@ -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;
     }
@@ -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;
     }
@@ -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) {
@@ -121,10 +118,10 @@ float navPidApply3(
             pid->integrator = newIntegrator;
         }
     }
-    
+
     if (pidFlags & PID_LIMIT_INTEGRATOR) {
         pid->integrator = constrainf(pid->integrator, outMin, outMax);
-    } 
+    }
 
     return outValConstrained;
 }
@@ -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)
@@ -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->dterm_filter_state, pid->dTermLpfHz);
+    pt1FilterSetCutoff(&pid->error_filter_state, pid->errorLpfHz);
+
     navPidReset(pid);
 }

src/main/flight/imu.c

@@ -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;
 
@@ -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)
@@ -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)
@@ -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;