Merge pull request #7483 from joelucid/airmode_noise_fix

Airmode noise fix

src/main/cli/settings.c

@@ -1012,6 +1012,9 @@ const clivalue_t valueTable[] = {
 #ifdef USE_THRUST_LINEARIZATION
     { "thrust_linear",              VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, thrustLinearization) },
 #endif
+#ifdef USE_AIRMODE_LPF
+    { "transient_throttle_limit",   VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, transient_throttle_limit) },
+#endif
 
 // PG_TELEMETRY_CONFIG
 #ifdef USE_TELEMETRY

src/main/flight/mixer.c

@@ -926,7 +926,13 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
     }
 #endif
 
+#ifdef USE_AIRMODE_LPF
+    const float unadjustedThrottle = throttle;
+    throttle += pidGetAirmodeThrottleOffset();
+    float airmodeThrottleChange = 0;
+#endif
     loggingThrottle = throttle;
+
     motorMixRange = motorMixMax - motorMixMin;
     if (motorMixRange > 1.0f) {
         for (int i = 0; i < motorCount; i++) {
@@ -939,9 +945,16 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
     } else {
         if (airmodeEnabled || throttle > 0.5f) {  // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle
             throttle = constrainf(throttle, -motorMixMin, 1.0f - motorMixMax);
+#ifdef USE_AIRMODE_LPF
+            airmodeThrottleChange = constrainf(unadjustedThrottle, -motorMixMin, 1.0f - motorMixMax) - unadjustedThrottle;
+#endif
         }
     }
 
+#ifdef USE_AIRMODE_LPF
+    pidUpdateAirmodeLpf(airmodeThrottleChange);
+#endif
+
     if (featureIsEnabled(FEATURE_MOTOR_STOP)
         && ARMING_FLAG(ARMED)
         && !featureIsEnabled(FEATURE_3D)

src/main/flight/pid.c

@@ -117,6 +117,10 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 #define ACRO_TRAINER_SETPOINT_LIMIT       1000.0f // Limit the correcting setpoint
 #endif // USE_ACRO_TRAINER
 
+#ifdef USE_AIRMODE_LPF
+static FAST_RAM_ZERO_INIT float airmodeThrottleOffsetLimit;
+#endif
+
 #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15  // The anti gravity throttle highpass filter cutoff
 
 #define CRASH_RECOVERY_DETECTION_DELAY_US 1000000  // 1 second delay before crash recovery detection is active after entering a self-level mode
@@ -196,6 +200,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .d_min_advance = 20,
         .motor_output_limit = 100,
         .auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY,
+        .transient_throttle_limit = 0,
     );
 #ifdef USE_DYN_LPF
     pidProfile->dterm_lowpass_hz = 150;
@@ -287,6 +292,11 @@ static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
 static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType;
 #endif // USE_RC_SMOOTHING_FILTER
 
+#ifdef USE_AIRMODE_LPF
+static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf1;
+static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf2;
+#endif
+
 static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
 
 void pidInitFilters(const pidProfile_t *pidProfile)
@@ -415,6 +425,12 @@ void pidInitFilters(const pidProfile_t *pidProfile)
         pt1FilterInit(&dMinLowpass[axis], pt1FilterGain(D_MIN_LOWPASS_HZ, dT));
      }
 #endif
+#if defined(USE_AIRMODE_LPF)
+    if (pidProfile->transient_throttle_limit) {
+        pt1FilterInit(&airmodeThrottleLpf1, pt1FilterGain(7.0f, dT));
+        pt1FilterInit(&airmodeThrottleLpf2, pt1FilterGain(20.0f, dT));
+    }
+#endif
 
     pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
 }
@@ -674,6 +690,9 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     dMinSetpointGain = pidProfile->d_min_gain * D_MIN_SETPOINT_GAIN_FACTOR * pidProfile->d_min_advance * pidFrequency / (100 * D_MIN_LOWPASS_HZ);
     // lowpass included inversely in gain since stronger lowpass decreases peak effect
 #endif
+#if defined(USE_AIRMODE_LPF)
+    airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
+#endif
 }
 
 void pidInit(const pidProfile_t *pidProfile)
@@ -1117,6 +1136,31 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
 }
 #endif
 
+#ifdef USE_AIRMODE_LPF
+void pidUpdateAirmodeLpf(float currentOffset)
+{
+    if (airmodeThrottleOffsetLimit == 0.0f) {
+        return;
+    }
+
+    float offsetHpf = currentOffset * 2.5f;
+    offsetHpf = offsetHpf - pt1FilterApply(&airmodeThrottleLpf2, offsetHpf);
+
+    // During high frequency oscillation 2 * currentOffset averages to the offset required to avoid mirroring of the waveform
+    pt1FilterApply(&airmodeThrottleLpf1, offsetHpf);
+    // Bring offset up immediately so the filter only applies to the decline
+    if (currentOffset * airmodeThrottleLpf1.state >= 0 && fabsf(currentOffset) > airmodeThrottleLpf1.state) {
+        airmodeThrottleLpf1.state = currentOffset;
+    }
+    airmodeThrottleLpf1.state = constrainf(airmodeThrottleLpf1.state, -airmodeThrottleOffsetLimit, airmodeThrottleOffsetLimit);
+}
+
+float pidGetAirmodeThrottleOffset()
+{
+    return airmodeThrottleLpf1.state;
+}
+#endif
+
 #ifdef USE_LAUNCH_CONTROL
 #define LAUNCH_CONTROL_MAX_RATE 100.0f
 #define LAUNCH_CONTROL_MIN_RATE 5.0f

src/main/flight/pid.h

@@ -165,6 +165,7 @@ typedef struct pidProfile_s {
     uint8_t d_min_advance;                  // Percentage multiplier for setpoint input to boost algorithm
     uint8_t motor_output_limit;             // Upper limit of the motor output (percent)
     int8_t auto_profile_cell_count;         // Cell count for this profile to be used with if auto PID profile switching is used
+    uint8_t transient_throttle_limit;       // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@@ -220,6 +221,10 @@ bool pidAntiGravityEnabled(void);
 float pidApplyThrustLinearization(float motorValue);
 float pidCompensateThrustLinearization(float throttle);
 #endif
+#ifdef USE_AIRMODE_LPF
+void pidUpdateAirmodeLpf(float currentOffset);
+float pidGetAirmodeThrottleOffset();
+#endif
 
 #ifdef UNIT_TEST
 #include "sensors/acceleration.h"

src/main/target/common_pre.h

@@ -258,6 +258,7 @@
 #endif // FLASH_SIZE > 128
 
 #if (FLASH_SIZE > 256)
+#define USE_AIRMODE_LPF
 #define USE_DASHBOARD
 #define USE_GPS
 #define USE_GPS_NMEA