Lead-Lag Compensator (#12730)

* Implement Lead-Lag-Compensator

* Refactoring / Clean up

* Remove trailing whitespaces

src/main/cli/settings.c

@@ -1142,7 +1142,7 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_HORIZON_LIMIT_STICKS,   VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) },
     { PARAM_NAME_HORIZON_LIMIT_DEGREES,  VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 10,  250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_limit_degrees) },
     { PARAM_NAME_HORIZON_IGNORE_STICKS,  VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_ignore_sticks) },
-    { PARAM_NAME_HORIZON_DELAY_MS,       VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 10,  5000 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_delay_ms) }, 
+    { PARAM_NAME_HORIZON_DELAY_MS,       VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 10,  5000 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_delay_ms) },
 
 #if defined(USE_ABSOLUTE_CONTROL)
     { PARAM_NAME_ABS_CONTROL_GAIN,  VAR_UINT8 | PROFILE_VALUE,  .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_gain) },

src/main/common/filter.c

@@ -19,7 +19,6 @@
  */
 
 #include <stdbool.h>
-#include <stdint.h>
 #include <string.h>
 #include <math.h>
 
@@ -131,30 +130,7 @@ FAST_CODE float pt3FilterApply(pt3Filter_t *filter, float input)
 }
 
 
-// Slew filter with limit
+// Biquad filter
-
-void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold)
-{
-    filter->state = 0.0f;
-    filter->slewLimit = slewLimit;
-    filter->threshold = threshold;
-}
-
-FAST_CODE float slewFilterApply(slewFilter_t *filter, float input)
-{
-    if (filter->state >= filter->threshold) {
-        if (input >= filter->state - filter->slewLimit) {
-            filter->state = input;
-        }
-    } else if (filter->state <= -filter->threshold) {
-        if (input <= filter->state + filter->slewLimit) {
-            filter->state = input;
-        }
-    } else {
-        filter->state = input;
-    }
-    return filter->state;
-}
 
 // get notch filter Q given center frequency (f0) and lower cutoff frequency (f1)
 // Q = f0 / (f2 - f1) ; f2 = f0^2 / f1
@@ -268,6 +244,76 @@ FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input)
     return result;
 }
 
+
+// Phase Compensator (Lead-Lag-Compensator)
+
+void phaseCompInit(phaseComp_t *filter, const float centerFreqHz, const float centerPhaseDeg, const uint32_t looptimeUs)
+{
+    phaseCompUpdate(filter, centerFreqHz, centerPhaseDeg, looptimeUs);
+
+    filter->x1 = 0.0f;
+    filter->y1 = 0.0f;
+}
+
+FAST_CODE void phaseCompUpdate(phaseComp_t *filter, const float centerFreqHz, const float centerPhaseDeg, const uint32_t looptimeUs)
+{
+    const float omega = 2.0f * M_PIf * centerFreqHz * looptimeUs * 1e-6f;
+    const float sn = sin_approx(centerPhaseDeg * RAD);
+    const float gain = (1 + sn) / (1 - sn);
+    const float alpha = (12 - sq(omega)) / (6 * omega * sqrtf(gain));  // approximate prewarping (series expansion)
+
+    filter->b0 = 1 + alpha * gain;
+    filter->b1 = 2 - filter->b0;
+    filter->a1 = 1 - alpha;
+
+    const float a0 = 1 / (1 + alpha);
+
+    filter->b0 *= a0;
+    filter->b1 *= a0;
+    filter->a1 *= a0;
+}
+
+FAST_CODE float phaseCompApply(phaseComp_t *filter, const float input)
+{
+    // compute result
+    const float result = filter->b0 * input + filter->b1 * filter->x1 - filter->a1 * filter->y1;
+
+    // shift input to x1 and result to y1
+    filter->x1 = input;
+    filter->y1 = result;
+
+    return result;
+}
+
+
+// Slew filter with limit
+
+void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold)
+{
+    filter->state = 0.0f;
+    filter->slewLimit = slewLimit;
+    filter->threshold = threshold;
+}
+
+FAST_CODE float slewFilterApply(slewFilter_t *filter, float input)
+{
+    if (filter->state >= filter->threshold) {
+        if (input >= filter->state - filter->slewLimit) {
+            filter->state = input;
+        }
+    } else if (filter->state <= -filter->threshold) {
+        if (input <= filter->state + filter->slewLimit) {
+            filter->state = input;
+        }
+    } else {
+        filter->state = input;
+    }
+    return filter->state;
+}
+
+
+// Moving average
+
 void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf)
 {
     filter->movingWindowIndex = 0;
@@ -290,11 +336,19 @@ FAST_CODE float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float i
     }
 
     const uint16_t denom = filter->primed ? filter->windowSize : filter->movingWindowIndex;
-    return filter->movingSum  / denom;
+    return filter->movingSum / denom;
 }
 
+
 // Simple fixed-point lowpass filter based on integer math
 
+void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift)
+{
+    filter->fp = 0;
+    filter->beta = beta;
+    filter->fpShift = fpShift;
+}
+
 int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal)
 {
     filter->fp = (filter->fp << filter->beta) - filter->fp;
@@ -304,11 +358,13 @@ int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal)
     return result;
 }
 
-void simpleLPFilterInit(simpleLowpassFilter_t *filter, int32_t beta, int32_t fpShift)
+
+// Mean accumulator
+
+void meanAccumulatorInit(meanAccumulator_t *filter)
 {
-    filter->fp = 0;
+    filter->accumulator = 0;
-    filter->beta = beta;
+    filter->count = 0;
-    filter->fpShift = fpShift;
 }
 
 void meanAccumulatorAdd(meanAccumulator_t *filter, const int8_t newVal)
@@ -326,9 +382,3 @@ int8_t meanAccumulatorCalc(meanAccumulator_t *filter, const int8_t defaultValue)
     }
     return defaultValue;
 }
-
-void meanAccumulatorInit(meanAccumulator_t *filter)
-{
-    filter->accumulator = 0;
-    filter->count = 0;
-}

src/main/common/filter.h

@@ -19,10 +19,26 @@
  */
 
 #pragma once
+
 #include <stdbool.h>
+#include <stdint.h>
 
 struct filter_s;
 typedef struct filter_s filter_t;
+typedef float (*filterApplyFnPtr)(filter_t *filter, float input);
+
+typedef enum {
+    FILTER_PT1 = 0,
+    FILTER_BIQUAD,
+    FILTER_PT2,
+    FILTER_PT3,
+} lowpassFilterType_e;
+
+typedef enum {
+    FILTER_LPF,    // 2nd order Butterworth section
+    FILTER_NOTCH,
+    FILTER_BPF,
+} biquadFilterType_e;
 
 typedef struct pt1Filter_s {
     float state;
@@ -42,12 +58,6 @@ typedef struct pt3Filter_s {
     float k;
 } pt3Filter_t;
 
-typedef struct slewFilter_s {
-    float state;
-    float slewLimit;
-    float threshold;
-} slewFilter_t;
-
 /* this holds the data required to update samples thru a filter */
 typedef struct biquadFilter_s {
     float b0, b1, b2, a1, a2;
@@ -55,6 +65,17 @@ typedef struct biquadFilter_s {
     float weight;
 } biquadFilter_t;
 
+typedef struct phaseComp_s {
+    float b0, b1, a1;
+    float x1, y1;
+} phaseComp_t;
+
+typedef struct slewFilter_s {
+    float state;
+    float slewLimit;
+    float threshold;
+} slewFilter_t;
+
 typedef struct laggedMovingAverage_s {
     uint16_t movingWindowIndex;
     uint16_t windowSize;
@@ -63,36 +84,19 @@ typedef struct laggedMovingAverage_s {
     bool primed;
 } laggedMovingAverage_t;
 
-typedef enum {
+typedef struct simpleLowpassFilter_s {
-    FILTER_PT1 = 0,
+    int32_t fp;
-    FILTER_BIQUAD,
+    int32_t beta;
-    FILTER_PT2,
+    int32_t fpShift;
-    FILTER_PT3,
+} simpleLowpassFilter_t;
-} lowpassFilterType_e;
 
-typedef enum {
+typedef struct meanAccumulator_s {
-    FILTER_LPF,    // 2nd order Butterworth section
+    int32_t accumulator;
-    FILTER_NOTCH,
+    int32_t count;
-    FILTER_BPF,
+} meanAccumulator_t;
-} biquadFilterType_e;
-
-typedef float (*filterApplyFnPtr)(filter_t *filter, float input);
 
 float nullFilterApply(filter_t *filter, float input);
 
-void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
-void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
-void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
-void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
-
-float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
-float biquadFilterApplyDF1Weighted(biquadFilter_t *filter, float input);
-float biquadFilterApply(biquadFilter_t *filter, float input);
-float filterGetNotchQ(float centerFreq, float cutoffFreq);
-
-void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf);
-float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input);
-
 float pt1FilterGain(float f_cut, float dT);
 void pt1FilterInit(pt1Filter_t *filter, float k);
 void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
@@ -108,23 +112,29 @@ void pt3FilterInit(pt3Filter_t *filter, float k);
 void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k);
 float pt3FilterApply(pt3Filter_t *filter, float input);
 
+float filterGetNotchQ(float centerFreq, float cutoffFreq);
+
+void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
+void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
+void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
+void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
+float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
+float biquadFilterApplyDF1Weighted(biquadFilter_t *filter, float input);
+float biquadFilterApply(biquadFilter_t *filter, float input);
+
+void phaseCompInit(phaseComp_t *filter, const float centerFreq, const float centerPhase, const uint32_t looptimeUs);
+void phaseCompUpdate(phaseComp_t *filter, const float centerFreq, const float centerPhase, const uint32_t looptimeUs);
+float phaseCompApply(phaseComp_t *filter, const float input);
+
 void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
 float slewFilterApply(slewFilter_t *filter, float input);
 
-typedef struct simpleLowpassFilter_s {
+void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf);
-    int32_t fp;
+float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input);
-    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);
+int32_t simpleLPFilterUpdate(simpleLowpassFilter_t *filter, int32_t newVal);
 
-typedef struct meanAccumulator_s {
+void meanAccumulatorInit(meanAccumulator_t *filter);
-    int32_t accumulator;
-    int32_t count;
-} meanAccumulator_t;
-
 void meanAccumulatorAdd(meanAccumulator_t *filter, const int8_t newVal);
 int8_t meanAccumulatorCalc(meanAccumulator_t *filter, const int8_t defaultValue);
-void meanAccumulatorInit(meanAccumulator_t *filter);

src/main/flight/dyn_notch_filter.c

@@ -123,7 +123,7 @@ typedef struct dynNotch_s {
 
     int maxCenterFreq;
     float centerFreq[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
-    
+
     timeUs_t looptimeUs;
     biquadFilter_t notch[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
 
@@ -261,11 +261,11 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
     DEBUG_SET(DEBUG_FFT_TIME, 0, state.step);
 
     switch (state.step) {
-    
+
         case STEP_WINDOW: // 4.1us (3-6us) @ F722
         {
             sdftWinSq(&sdft[state.axis], sdftData);
-            
+
             // Get total vibrational power in dyn notch range for noise floor estimate in STEP_CALC_FREQUENCIES
             sdftNoiseThreshold = 0.0f;
             for (int bin = sdftStartBin; bin <= sdftEndBin; bin++) {
@@ -359,7 +359,7 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
                     // Convert bin to frequency: freq = bin * binResoultion (bin 0 is 0Hz)
                     const float centerFreq = constrainf(meanBin * sdftResolutionHz, dynNotch.minHz, dynNotch.maxHz);
 
-                    // PT1 style smoothing moves notch center freqs rapidly towards big peaks and slowly away, up to 10x faster 
+                    // PT1 style smoothing moves notch center freqs rapidly towards big peaks and slowly away, up to 10x faster
                     const float cutoffMult = constrainf(peaks[p].value / sdftNoiseThreshold, 1.0f, 10.0f);
                     const float gain = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ * cutoffMult, pt1LooptimeS); // dynamic PT1 k value
 
@@ -403,7 +403,7 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
     state.step = (state.step + 1) % STEP_COUNT;
 }
 
-FAST_CODE float dynNotchFilter(const int axis, float value) 
+FAST_CODE float dynNotchFilter(const int axis, float value)
 {
     for (int p = 0; p < dynNotch.count; p++) {
         value = biquadFilterApplyDF1(&dynNotch.notch[axis][p], value);

src/main/flight/pid.c

@@ -91,7 +91,7 @@ pt1Filter_t throttleLpf;
 PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 3);
 
 #if !defined(DEFAULT_PID_PROCESS_DENOM)
-#if defined(STM32F411xE) 
+#if defined(STM32F411xE)
 #define DEFAULT_PID_PROCESS_DENOM       2
 #else
 #define DEFAULT_PID_PROCESS_DENOM       1
@@ -298,7 +298,7 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
     static float previousThrottle = 0.0f;
     const float throttleInv = 1.0f - throttle;
     float throttleDerivative = fabsf(throttle - previousThrottle) * pidRuntime.pidFrequency;
-    DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(throttleDerivative * 100)); 
+    DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(throttleDerivative * 100));
     throttleDerivative *= throttleInv * throttleInv;
     // generally focus on the low throttle period
     if (throttle > previousThrottle) {
@@ -310,7 +310,7 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
     // lower cutoff suppresses peaks relative to troughs and prolongs the effects
     // PT2 smoothing of throttle derivative.
     // 6 is a typical value for the peak boost factor with default cutoff of 6Hz
-    DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(throttleDerivative * 100)); 
+    DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(throttleDerivative * 100));
     pidRuntime.antiGravityThrottleD = throttleDerivative;
 }
 
@@ -911,7 +911,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
             if (levelMode == LEVEL_MODE_RP) {
                 // if earth referencing is requested, attenuate yaw axis setpoint when pitched or rolled
                 // and send yawSetpoint to Angle code to modulate pitch and roll
-                // code cost is 107 cycles when earthRef enabled, 20 otherwise, nearly all in cos_approx 
+                // code cost is 107 cycles when earthRef enabled, 20 otherwise, nearly all in cos_approx
                 if (pidRuntime.angleEarthRef) {
                     pidRuntime.angleYawSetpoint = currentPidSetpoint;
                     float maxAngleTargetAbs = pidRuntime.angleEarthRef * fmaxf( fabsf(pidRuntime.angleTarget[FD_ROLL]), fabsf(pidRuntime.angleTarget[FD_PITCH]) );

src/main/flight/pid_init.c

@@ -432,4 +432,3 @@ void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
         memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
     }
 }
-

src/main/sensors/gyro.c

@@ -108,7 +108,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
     gyroConfig->gyroMovementCalibrationThreshold = 48;
     gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
     gyroConfig->gyro_lpf1_type = FILTER_PT1;
-    gyroConfig->gyro_lpf1_static_hz = GYRO_LPF1_DYN_MIN_HZ_DEFAULT;  
+    gyroConfig->gyro_lpf1_static_hz = GYRO_LPF1_DYN_MIN_HZ_DEFAULT;
         // NOTE: dynamic lpf is enabled by default so this setting is actually
         // overridden and the static lowpass 1 is disabled. We can't set this
         // value to 0 otherwise Configurator versions 10.4 and earlier will also