Merge remote-tracking branch 'origin/development' into dzikuvx-yaw-handling-cleanup

src/main/build/debug.h

@@ -51,29 +51,19 @@ extern timeUs_t sectionTimes[2][4];
 typedef enum {
     DEBUG_NONE,
     DEBUG_GYRO,
-    DEBUG_NOTCH,
-    DEBUG_NAV_LANDING_DETECTOR,
-    DEBUG_FW_CLIMB_RATE_TO_ALTITUDE,
     DEBUG_AGL,
     DEBUG_FLOW_RAW,
     DEBUG_FLOW,
     DEBUG_SBUS,
     DEBUG_FPORT,
     DEBUG_ALWAYS,
-    DEBUG_STAGE2,
     DEBUG_SAG_COMP_VOLTAGE,
     DEBUG_VIBE,
     DEBUG_CRUISE,
     DEBUG_REM_FLIGHT_TIME,
     DEBUG_SMARTAUDIO,
     DEBUG_ACC,
-    DEBUG_GENERIC,
     DEBUG_ITERM_RELAX,
-    DEBUG_D_BOOST,
-    DEBUG_ANTIGRAVITY,
-    DEBUG_FFT,
-    DEBUG_FFT_TIME,
-    DEBUG_FFT_FREQ,
     DEBUG_ERPM,
     DEBUG_COUNT
 } debugType_e;

src/main/common/maths.c

@@ -23,6 +23,7 @@
 #include "maths.h"
 #include "vector.h"
 #include "quaternion.h"
+#include "platform.h"
 
 // http://lolengine.net/blog/2011/12/21/better-function-approximations
 // Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
@@ -158,7 +159,7 @@ int constrain(int amt, int low, int high)
         return amt;
 }
 
-float constrainf(float amt, float low, float high)
+float FAST_CODE NOINLINE constrainf(float amt, float low, float high)
 {
     if (amt < low)
         return low;

src/main/fc/cli.c

@@ -1612,8 +1612,8 @@ static void cliServo(char *cmdline)
         servo = servoParamsMutable(i);
 
         if (
-            arguments[MIN] < PWM_PULSE_MIN || arguments[MIN] > PWM_PULSE_MAX ||
+            arguments[MIN] < SERVO_OUTPUT_MIN || arguments[MIN] > SERVO_OUTPUT_MAX ||
-            arguments[MAX] < PWM_PULSE_MIN || arguments[MAX] > PWM_PULSE_MAX ||
+            arguments[MAX] < SERVO_OUTPUT_MIN || arguments[MAX] > SERVO_OUTPUT_MAX ||
             arguments[MIDDLE] < arguments[MIN] || arguments[MIDDLE] > arguments[MAX] ||
             arguments[MIN] > arguments[MAX] || arguments[MAX] < arguments[MIN] ||
             arguments[RATE] < -125 || arguments[RATE] > 125

src/main/fc/config.c

@@ -167,27 +167,18 @@ uint32_t getLooptime(void) {
 
 void validateAndFixConfig(void)
 {
-#ifdef USE_GYRO_NOTCH_1
     if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) {
         gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
     }
-#endif
-#ifdef USE_GYRO_NOTCH_2
     if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
         gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
     }
-#endif
-#ifdef USE_DTERM_NOTCH
     if (pidProfile()->dterm_soft_notch_cutoff >= pidProfile()->dterm_soft_notch_hz) {
         pidProfileMutable()->dterm_soft_notch_hz = 0;
     }
-#endif
-
-#ifdef USE_ACC_NOTCH
     if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) {
         accelerometerConfigMutable()->acc_notch_hz = 0;
     }
-#endif
 
     // Disable unused features
     featureClear(FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 | FEATURE_UNUSED_10);

src/main/fc/fc_msp.c

@@ -1101,43 +1101,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz);
         sbufWriteU16(dst, pidProfile()->dterm_lpf_hz);
         sbufWriteU16(dst, pidProfile()->yaw_lpf_hz);
-#ifdef USE_GYRO_NOTCH_1
         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1); //masterConfig.gyro_soft_notch_hz_1
         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1); //BF: masterConfig.gyro_soft_notch_cutoff_1
-#else
-        sbufWriteU16(dst, 0); //masterConfig.gyro_soft_notch_hz_1
-        sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_1
-#endif
 
-#ifdef USE_DTERM_NOTCH
         sbufWriteU16(dst, pidProfile()->dterm_soft_notch_hz); //BF: pidProfile()->dterm_notch_hz
         sbufWriteU16(dst, pidProfile()->dterm_soft_notch_cutoff); //pidProfile()->dterm_notch_cutoff
-#else
-        sbufWriteU16(dst, 0); //BF: pidProfile()->dterm_notch_hz
-        sbufWriteU16(dst, 1); //pidProfile()->dterm_notch_cutoff
-#endif
 
-#ifdef USE_GYRO_NOTCH_2
         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2); //BF: masterConfig.gyro_soft_notch_hz_2
         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2); //BF: masterConfig.gyro_soft_notch_cutoff_2
-#else
-        sbufWriteU16(dst, 0); //BF: masterConfig.gyro_soft_notch_hz_2
-        sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_2
-#endif
 
-#ifdef USE_ACC_NOTCH
         sbufWriteU16(dst, accelerometerConfig()->acc_notch_hz);
         sbufWriteU16(dst, accelerometerConfig()->acc_notch_cutoff);
-#else
-        sbufWriteU16(dst, 0);
-        sbufWriteU16(dst, 1);
-#endif
 
-#ifdef USE_GYRO_BIQUAD_RC_FIR2
         sbufWriteU16(dst, gyroConfig()->gyro_stage2_lowpass_hz);
-#else
-        sbufWriteU16(dst, 0);
-#endif
         break;
 
     case MSP_PID_ADVANCED:
@@ -2017,43 +1993,38 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src);
             pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 255);
             pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
-#ifdef USE_GYRO_NOTCH_1
             if (dataSize >= 9) {
                 gyroConfigMutable()->gyro_soft_notch_hz_1 = constrain(sbufReadU16(src), 0, 500);
                 gyroConfigMutable()->gyro_soft_notch_cutoff_1 = constrain(sbufReadU16(src), 1, 500);
-            } else
+            } else {
                 return MSP_RESULT_ERROR;
-#endif
+            }
-#ifdef USE_DTERM_NOTCH
             if (dataSize >= 13) {
                 pidProfileMutable()->dterm_soft_notch_hz = constrain(sbufReadU16(src), 0, 500);
                 pidProfileMutable()->dterm_soft_notch_cutoff = constrain(sbufReadU16(src), 1, 500);
                 pidInitFilters();
-            } else
+            } else {
                 return MSP_RESULT_ERROR;
-#endif
+            }
-#ifdef USE_GYRO_NOTCH_2
             if (dataSize >= 17) {
                 gyroConfigMutable()->gyro_soft_notch_hz_2 = constrain(sbufReadU16(src), 0, 500);
                 gyroConfigMutable()->gyro_soft_notch_cutoff_2 = constrain(sbufReadU16(src), 1, 500);
-            } else
+            } else {
                 return MSP_RESULT_ERROR;
-#endif
+            }
 
-#ifdef USE_ACC_NOTCH
             if (dataSize >= 21) {
                 accelerometerConfigMutable()->acc_notch_hz = constrain(sbufReadU16(src), 0, 255);
                 accelerometerConfigMutable()->acc_notch_cutoff = constrain(sbufReadU16(src), 1, 255);
-            } else
+            } else {
                 return MSP_RESULT_ERROR;
-#endif
+            }
 
-#ifdef USE_GYRO_BIQUAD_RC_FIR2
             if (dataSize >= 22) {
                 gyroConfigMutable()->gyro_stage2_lowpass_hz = constrain(sbufReadU16(src), 0, 500);
-            } else
+            } else {
                 return MSP_RESULT_ERROR;
-#endif
+            }
         } else
             return MSP_RESULT_ERROR;
         break;

src/main/fc/settings.yaml

@@ -78,10 +78,10 @@ tables:
   - name: i2c_speed
     values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
   - name: debug_modes
-    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW",
+    values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
-      "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE",
+      "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
-      "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX", 
+      "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX", 
-      "D_BOOST", "ANTIGRAVITY", "FFT", "FFT_TIME", "FFT_FREQ", "ERPM"]
+      "ERPM"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -135,6 +135,9 @@ tables:
   - name: dynamicFilterRangeTable
     values: ["HIGH", "MEDIUM", "LOW"]
     enum: dynamicFilterRange_e
+  - name: pidTypeTable
+    values: ["NONE", "PID", "PIFF", "AUTO"]
+    enum: pidType_e
 
 groups:
   - name: PG_GYRO_CONFIG
@@ -164,25 +167,20 @@ groups:
         max: 128
       - name: gyro_notch1_hz
         field: gyro_soft_notch_hz_1
-        condition: USE_GYRO_NOTCH_1
         max: 500
       - name: gyro_notch1_cutoff
         field: gyro_soft_notch_cutoff_1
-        condition: USE_GYRO_NOTCH_1
         min: 1
         max: 500
       - name: gyro_notch2_hz
         field: gyro_soft_notch_hz_2
-        condition: USE_GYRO_NOTCH_2
         max: 500
       - name: gyro_notch2_cutoff
         field: gyro_soft_notch_cutoff_2
-        condition: USE_GYRO_NOTCH_2
         min: 1
         max: 500
       - name: gyro_stage2_lowpass_hz
         field: gyro_stage2_lowpass_hz
-        condition: USE_GYRO_BIQUAD_RC_FIR2
         min: 0
         max: 500
       - name: dyn_notch_width_percent
@@ -236,11 +234,9 @@ groups:
     headers: ["sensors/acceleration.h"]
     members:
       - name: acc_notch_hz
-        condition: USE_ACC_NOTCH
         min: 0
         max: 255
       - name: acc_notch_cutoff
-        condition: USE_ACC_NOTCH
         min: 1
         max: 255
       - name: align_acc
@@ -1073,12 +1069,10 @@ groups:
         max: 1
       - name: dterm_notch_hz
         field: dterm_soft_notch_hz
-        condition: USE_DTERM_NOTCH
         min: 0
         max: 500
       - name: dterm_notch_cutoff
         field: dterm_soft_notch_cutoff
-        condition: USE_DTERM_NOTCH
         min: 1
         max: 500
       - name: pidsum_limit
@@ -1232,6 +1226,9 @@ groups:
         condition: USE_ANTIGRAVITY
         min: 1
         max: 30
+      - name: pid_type
+        field: pidControllerType
+        table: pidTypeTable
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t

src/main/flight/gyroanalyse.c

@@ -157,9 +157,6 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn,
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
             state->downsampledGyroData[axis][state->circularBufferIdx] = sample;
-            if (axis == 0) {
-                DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
-            }
 
             state->oversampledGyroAccumulator[axis] = 0;
         }
@@ -201,12 +198,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
 
     arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint);
 
-    uint32_t startTime = 0;
-    if (debugMode == (DEBUG_FFT_TIME)) {
-        startTime = micros();
-    }
-
-    DEBUG_SET(DEBUG_FFT_TIME, 0, state->updateStep);
     switch (state->updateStep) {
         case STEP_ARM_CFFT_F32:
         {
@@ -224,14 +215,12 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
                 arm_radix8_butterfly_f32(state->fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1);
                 break;
             }
-            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
             break;
         }
         case STEP_BITREVERSAL:
         {
             // 6us
             arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable);
-            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
             state->updateStep++;
             FALLTHROUGH;
         }
@@ -240,14 +229,12 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
             // 14us
             // this does not work in place => fftData AND rfftData needed
             stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData);
-            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
             break;
         }
         case STEP_ARM_CMPLX_MAG_F32:
         {
             // 8us
             arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT);
-            DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime);
             state->updateStep++;
             FALLTHROUGH;
         }
@@ -312,15 +299,7 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
 
             dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]);
 
-            if (state->updateAxis == 0) {
-                DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
-                DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]);
-            }
-            if (state->updateAxis == 1) {
-                DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]);
-            }
             // Debug FFT_Freq carries raw gyro, gyro after first filter set, FFT centre for roll and for pitch
-            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
             break;
         }
         case STEP_UPDATE_FILTERS:
@@ -329,8 +308,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
             // calculate cutoffFreq and notch Q, update notch filter  =1.8+((A2-150)*0.004)
             if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
                 
-                DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis + 5, state->centerFreq[state->updateAxis]);
-
                 if (dualNotch) {
                     biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
                     biquadFilterUpdate(&notchFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
@@ -338,7 +315,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
                     biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], getLooptime(), dynNotchQ, FILTER_NOTCH);
                 }
             }
-            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 
             state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
             state->updateStep++;
@@ -354,8 +330,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
             if (state->circularBufferIdx > 0) {
                 arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx);
             }
-
-            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
         }
     }
 

src/main/flight/mixer.c

@@ -109,6 +109,9 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
 
 PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
 
+typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
+static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn;
+
 static void computeMotorCount(void)
 {
     motorCount = 0;
@@ -153,6 +156,49 @@ void mixerUpdateStateFlags(void)
     }
 }
 
+void nullMotorRateLimiting(const float dT)
+{
+    UNUSED(dT);
+}
+
+void applyMotorRateLimiting(const float dT)
+{
+    static float motorPrevious[MAX_SUPPORTED_MOTORS] = { 0 };
+
+    if (feature(FEATURE_3D)) {
+        // FIXME: Don't apply rate limiting in 3D mode
+        for (int i = 0; i < motorCount; i++) {
+            motorPrevious[i] = motor[i];
+        }
+    }
+    else {
+        // Calculate max motor step
+        const uint16_t motorRange = motorConfig()->maxthrottle - motorConfig()->minthrottle;
+        const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f);
+        const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f);
+
+        for (int i = 0; i < motorCount; i++) {
+            // Apply motor rate limiting
+            motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc);
+
+            // Handle throttle below min_throttle (motor start/stop)
+            if (motorPrevious[i] < motorConfig()->minthrottle) {
+                if (motor[i] < motorConfig()->minthrottle) {
+                    motorPrevious[i] = motor[i];
+                }
+                else {
+                    motorPrevious[i] = motorConfig()->minthrottle;
+                }
+            }
+        }
+    }
+
+    // Update motor values
+    for (int i = 0; i < motorCount; i++) {
+        motor[i] = motorPrevious[i];
+    }
+}
+
 void mixerInit(void)
 {
     computeMotorCount();
@@ -163,6 +209,12 @@ void mixerInit(void)
     }
 
     mixerResetDisarmedMotors();
+
+    if (motorConfig()->motorAccelTimeMs || motorConfig()->motorDecelTimeMs) {
+        motorRateLimitingApplyFn = applyMotorRateLimiting;
+    } else {
+        motorRateLimitingApplyFn = nullMotorRateLimiting;
+    }
 }
 
 void mixerResetDisarmedMotors(void)
@@ -239,44 +291,6 @@ void stopPwmAllMotors(void)
     pwmShutdownPulsesForAllMotors(motorCount);
 }
 
-static void applyMotorRateLimiting(const float dT)
-{
-    static float motorPrevious[MAX_SUPPORTED_MOTORS] = { 0 };
-
-    if (feature(FEATURE_3D)) {
-        // FIXME: Don't apply rate limiting in 3D mode
-        for (int i = 0; i < motorCount; i++) {
-            motorPrevious[i] = motor[i];
-        }
-    }
-    else {
-        // Calculate max motor step
-        const uint16_t motorRange = motorConfig()->maxthrottle - motorConfig()->minthrottle;
-        const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f);
-        const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f);
-
-        for (int i = 0; i < motorCount; i++) {
-            // Apply motor rate limiting
-            motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc);
-
-            // Handle throttle below min_throttle (motor start/stop)
-            if (motorPrevious[i] < motorConfig()->minthrottle) {
-                if (motor[i] < motorConfig()->minthrottle) {
-                    motorPrevious[i] = motor[i];
-                }
-                else {
-                    motorPrevious[i] = motorConfig()->minthrottle;
-                }
-            }
-        }
-    }
-
-    // Update motor values
-    for (int i = 0; i < motorCount; i++) {
-        motor[i] = motorPrevious[i];
-    }
-}
-
 void FAST_CODE NOINLINE mixTable(const float dT)
 {
     int16_t input[3];   // RPY, range [-500:+500]
@@ -408,7 +422,7 @@ void FAST_CODE NOINLINE mixTable(const float dT)
     }
 
     /* Apply motor acceleration/deceleration limit */
-    applyMotorRateLimiting(dT);
+    motorRateLimitingApplyFn(dT);
 }
 
 motorStatus_e getMotorStatus(void)

src/main/flight/pid.c

@@ -81,11 +81,8 @@ typedef struct {
     rateLimitFilter_t axisAccelFilter;
     pt1Filter_t ptermLpfState;
     biquadFilter_t deltaLpfState;
-
     // Dterm notch filtering
-#ifdef USE_DTERM_NOTCH
     biquadFilter_t deltaNotchFilter;
-#endif
     float stickPosition;
 
 #ifdef USE_D_BOOST
@@ -96,17 +93,15 @@ typedef struct {
 #endif
     uint16_t pidSumLimit;
     filterApply4FnPtr ptermFilterApplyFn;
+    bool itermLimitActive;
 } pidState_t;
 
-#ifdef USE_DTERM_NOTCH
 STATIC_FASTRAM filterApplyFnPtr notchFilterApplyFn;
-#endif
-
 STATIC_FASTRAM bool pidFiltersConfigured = false;
-FASTRAM float headingHoldCosZLimit;
+static EXTENDED_FASTRAM float headingHoldCosZLimit;
-FASTRAM int16_t headingHoldTarget;
+static EXTENDED_FASTRAM int16_t headingHoldTarget;
-STATIC_FASTRAM pt1Filter_t headingHoldRateFilter;
+static EXTENDED_FASTRAM pt1Filter_t headingHoldRateFilter;
-STATIC_FASTRAM pt1Filter_t fixedWingTpaFilter;
+static EXTENDED_FASTRAM pt1Filter_t fixedWingTpaFilter;
 
 // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied
 STATIC_FASTRAM bool pidGainsUpdateRequired;
@@ -139,6 +134,16 @@ static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration;
 #endif
 
 static EXTENDED_FASTRAM uint8_t yawLpfHz;
+static EXTENDED_FASTRAM float motorItermWindupPoint;
+static EXTENDED_FASTRAM float antiWindupScaler;
+#ifdef USE_ANTIGRAVITY
+static EXTENDED_FASTRAM float iTermAntigravityGain;
+#endif
+static EXTENDED_FASTRAM uint8_t usedPidControllerType;
+
+typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT);
+static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
+static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 
 PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 11);
 
@@ -244,49 +249,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .antigravityGain = 1.0f,
         .antigravityAccelerator = 1.0f,
         .antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF,
+        .pidControllerType = PID_TYPE_AUTO,
 );
 
-void pidInit(void)
-{
-    pidResetTPAFilter();
-
-    // Calculate max overall tilt (max pitch + max roll combined) as a limit to heading hold
-    headingHoldCosZLimit = cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_ROLL])) *
-                           cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_PITCH]));
-
-    pidGainsUpdateRequired = false;
-
-    itermRelax = pidProfile()->iterm_relax;
-    itermRelaxType = pidProfile()->iterm_relax_type;
-    itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff;
-    
-    yawLpfHz = pidProfile()->yaw_lpf_hz;
-
-#ifdef USE_D_BOOST
-    dBoostFactor = pidProfile()->dBoostFactor;
-    dBoostMaxAtAlleceleration = pidProfile()->dBoostMaxAtAlleceleration;
-#endif
-
-#ifdef USE_ANTIGRAVITY
-    antigravityGain = pidProfile()->antigravityGain;
-    antigravityAccelerator = pidProfile()->antigravityAccelerator;
-#endif
-
-    for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
-        if (axis == FD_YAW) {
-            pidState[axis].pidSumLimit = pidProfile()->pidSumLimitYaw;
-            if (yawLpfHz) {
-                pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) pt1FilterApply4;
-            } else {
-                pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
-            }
-        } else {
-            pidState[axis].pidSumLimit = pidProfile()->pidSumLimit;
-            pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
-        }
-    }
-}
-
 bool pidInitFilters(void)
 {
     const uint32_t refreshRate = getLooptime();
@@ -319,7 +284,6 @@ bool pidInitFilters(void)
         firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs);
     }
 
-#ifdef USE_DTERM_NOTCH
     notchFilterApplyFn = nullFilterApply;
     if (pidProfile()->dterm_soft_notch_hz != 0) {
         notchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
@@ -327,7 +291,6 @@ bool pidInitFilters(void)
             biquadFilterInitNotch(&pidState[axis].deltaNotchFilter, refreshRate, pidProfile()->dterm_soft_notch_hz, pidProfile()->dterm_soft_notch_cutoff);
         }
     }
-#endif
 
     // Init other filters
     for (int axis = 0; axis < 3; ++ axis) {
@@ -355,7 +318,7 @@ bool pidInitFilters(void)
 
 void pidResetTPAFilter(void)
 {
-    if (STATE(FIXED_WING) && currentControlRateProfile->throttle.fixedWingTauMs > 0) {
+    if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) {
         pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, getLooptime() * 1e-6f);
         pt1FilterReset(&fixedWingTpaFilter, motorConfig()->minthrottle);
     }
@@ -453,7 +416,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
     STATIC_FASTRAM uint16_t prevThrottle = 0;
 
     // Check if throttle changed. Different logic for fixed wing vs multirotor
-    if (STATE(FIXED_WING) && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) {
+    if (usedPidControllerType == PID_TYPE_PIFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) {
         uint16_t filteredThrottle = pt1FilterApply3(&fixedWingTpaFilter, rcCommand[THROTTLE], dT);
         if (filteredThrottle != prevThrottle) {
             prevThrottle = filteredThrottle;
@@ -468,7 +431,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
     }
 
 #ifdef USE_ANTIGRAVITY
-    if (!STATE(FIXED_WING)) {
+    if (usedPidControllerType == PID_TYPE_PID) {
         antigravityThrottleHpf = rcCommand[THROTTLE] - pt1FilterApply(&antigravityThrottleLpf, rcCommand[THROTTLE]);
     }
 #endif
@@ -485,12 +448,12 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
         return;
     }
 
-    const float tpaFactor = STATE(FIXED_WING) ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor();
+    const float tpaFactor = usedPidControllerType == PID_TYPE_PIFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor();
 
     // 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++) {
-        if (STATE(FIXED_WING)) {
+        if (usedPidControllerType == PID_TYPE_PIFF) {
             // Airplanes - scale all PIDs according to TPA
             pidState[axis].kP  = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER  * tpaFactor;
             pidState[axis].kI  = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER  * tpaFactor;
@@ -604,6 +567,20 @@ static FAST_CODE NOINLINE float pTermProcess(pidState_t *pidState, float rateErr
     return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
 }
 
+static void applyItermLimiting(pidState_t *pidState) {
+    if (pidState->itermLimitActive) {
+        pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
+    } else {
+        pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
+    }
+}
+
+static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
+    UNUSED(pidState);
+    UNUSED(axis);
+    UNUSED(dT);
+}
+
 static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
 {
     const float rateError = pidState->rateTarget - pidState->gyroRate;
@@ -613,11 +590,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     // Calculate integral
     pidState->errorGyroIf += rateError * pidState->kI * dT;
 
-    if (STATE(ANTI_WINDUP) || isFixedWingItermLimitActive(pidState->stickPosition)) {
+    applyItermLimiting(pidState);
-        pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
-    } else {
-        pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
-    }
 
     if (pidProfile()->fixedWingItermThrowLimit != 0) {
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
@@ -666,7 +639,7 @@ static void FAST_CODE applyItermRelax(const int axis, const float gyroRate, floa
     }
 }
 #ifdef USE_D_BOOST
-static float FAST_CODE applyDBoost(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
+static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
     
     float dBoost = 1.0f;
     
@@ -680,14 +653,6 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, flight_dynamics_index_t
         dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
         dBoost = constrainf(dBoost, 1.0f, dBoostFactor);
 
-        if (axis == FD_ROLL) {
-            DEBUG_SET(DEBUG_D_BOOST, 0, dBoostGyroAcceleration);
-            DEBUG_SET(DEBUG_D_BOOST, 1, dBoostRateAcceleration);
-            DEBUG_SET(DEBUG_D_BOOST, 2, dBoost * 100);
-        } else if (axis == FD_PITCH) {
-            DEBUG_SET(DEBUG_D_BOOST, 3, dBoost * 100);
-        }
-
         pidState->previousRateTarget = pidState->rateTarget;
         pidState->previousRateGyro = pidState->gyroRate;
     } 
@@ -695,43 +660,38 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, flight_dynamics_index_t
     return dBoost;
 }
 #else 
-static float applyDBoost(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
+static float applyDBoost(pidState_t *pidState, float dT) {
     UNUSED(pidState);
-    UNUSED(axis);
     UNUSED(dT);
     return 1.0f;
 }
 #endif
 
-static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
+static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
 {
     const float rateError = pidState->rateTarget - pidState->gyroRate;
     const float newPTerm = pTermProcess(pidState, rateError, dT);
 
     // Calculate new D-term
     float newDTerm;
-    if (pidBank()->pid[axis].D == 0) {
+    if (pidState->kD == 0) {
         // optimisation for when D8 is zero, often used by YAW axis
         newDTerm = 0;
     } else {
         // Calculate delta for Dterm calculation. Apply filters before derivative to minimize effects of dterm kick
         float deltaFiltered = pidProfile()->dterm_setpoint_weight * pidState->rateTarget - pidState->gyroRate;
 
-#ifdef USE_DTERM_NOTCH
         // Apply D-term notch
         deltaFiltered = notchFilterApplyFn(&pidState->deltaNotchFilter, deltaFiltered);
-#endif
 
         // Apply additional lowpass
-        if (pidProfile()->dterm_lpf_hz) {
+        deltaFiltered = dTermLpfFilterApplyFn(&pidState->deltaLpfState, deltaFiltered);
-            deltaFiltered = biquadFilterApply(&pidState->deltaLpfState, deltaFiltered);
-        }
 
         firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
         newDTerm = firFilterApply(&pidState->gyroRateFilter);
 
         // Calculate derivative
-        newDTerm =  newDTerm * (pidState->kD / dT) * applyDBoost(pidState, axis, dT);
+        newDTerm =  newDTerm * (pidState->kD / dT) * applyDBoost(pidState, dT);
 
         // Additionally constrain D
         newDTerm = constrainf(newDTerm, -300.0f, 300.0f);
@@ -741,17 +701,10 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
     const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf;
     const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit);
 
-    // Prevent strong Iterm accumulation during stick inputs
-    const float motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
-    const float antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f);
-
     float itermErrorRate = rateError;
     applyItermRelax(axis, pidState->gyroRate, pidState->rateTarget, &itermErrorRate);
 
 #ifdef USE_ANTIGRAVITY
-    const float iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain);    
-    DEBUG_SET(DEBUG_ANTIGRAVITY, 0, iTermAntigravityGain * 100);
-    DEBUG_SET(DEBUG_ANTIGRAVITY, 1, antigravityThrottleHpf);
     itermErrorRate *= iTermAntigravityGain;
 #endif
 
@@ -759,11 +712,7 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
                              + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);
 
     // Don't grow I-term if motors are at their limit
-    if (STATE(ANTI_WINDUP) || mixerIsOutputSaturated()) {
+    applyItermLimiting(pidState);
-        pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
-    } else {
-        pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
-    }
 
     axisPID[axis] = newOutputLimited;
 
@@ -955,7 +904,20 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
     pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
 }
 
-void FAST_CODE pidController(float dT)
+void FAST_CODE checkItermLimitingActive(pidState_t *pidState)
+{
+    bool shouldActivate;
+    if (usedPidControllerType == PID_TYPE_PIFF) {
+        shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
+    } else 
+    {
+        shouldActivate = mixerIsOutputSaturated();
+    }
+
+    pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; 
+}
+
+void FAST_CODE NOINLINE pidController(float dT)
 {
     if (!pidFiltersConfigured) {
         return;
@@ -1006,30 +968,29 @@ void FAST_CODE pidController(float dT)
         pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees);
     }
 
-    // Apply setpoint rate of change limits
+    // Prevent strong Iterm accumulation during stick inputs
-    for (int axis = 0; axis < 3; axis++) {
+    antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f);
-        pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
+
-    }
+#ifdef USE_ANTIGRAVITY
+    iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain);    
+#endif
 
-    // Step 4: Run gyro-driven control
     for (int axis = 0; axis < 3; axis++) {
-        // Apply PID setpoint controller
+        // Apply setpoint rate of change limits
-        if (STATE(FIXED_WING)) {
+        pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
-            pidApplyFixedWingRateController(&pidState[axis], axis, dT);
+        
-        }
+        // Step 4: Run gyro-driven control
-        else {
+        checkItermLimitingActive(&pidState[axis]);
-            pidApplyMulticopterRateController(&pidState[axis], axis, dT);
+        pidControllerApplyFn(&pidState[axis], axis, dT);
-        }
     }
 }
 
 pidType_e pidIndexGetType(pidIndex_e pidIndex)
 {
+    if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
+        return usedPidControllerType;    
+    }
     if (STATE(FIXED_WING)) {
-        // FW specific
-        if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
-            return PID_TYPE_PIFF;
-        }
         if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) {
             return PID_TYPE_NONE;
         }
@@ -1040,3 +1001,69 @@ pidType_e pidIndexGetType(pidIndex_e pidIndex)
     }
     return PID_TYPE_PID;
 }
+
+void pidInit(void)
+{
+    pidResetTPAFilter();
+
+    // Calculate max overall tilt (max pitch + max roll combined) as a limit to heading hold
+    headingHoldCosZLimit = cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_ROLL])) *
+                           cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_PITCH]));
+
+    pidGainsUpdateRequired = false;
+
+    itermRelax = pidProfile()->iterm_relax;
+    itermRelaxType = pidProfile()->iterm_relax_type;
+    itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff;
+
+    yawLpfHz = pidProfile()->yaw_lpf_hz;
+    motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
+
+#ifdef USE_D_BOOST
+    dBoostFactor = pidProfile()->dBoostFactor;
+    dBoostMaxAtAlleceleration = pidProfile()->dBoostMaxAtAlleceleration;
+#endif
+
+#ifdef USE_ANTIGRAVITY
+    antigravityGain = pidProfile()->antigravityGain;
+    antigravityAccelerator = pidProfile()->antigravityAccelerator;
+#endif
+    
+    for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
+        if (axis == FD_YAW) {
+            pidState[axis].pidSumLimit = pidProfile()->pidSumLimitYaw;
+            if (yawLpfHz) {
+                pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) pt1FilterApply4;
+            } else {
+                pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
+            }
+        } else {
+            pidState[axis].pidSumLimit = pidProfile()->pidSumLimit;
+            pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
+        }
+    }
+
+    if (pidProfile()->pidControllerType == PID_TYPE_AUTO) {
+        if (STATE(FIXED_WING)) {
+            usedPidControllerType = PID_TYPE_PIFF;
+        } else {
+            usedPidControllerType = PID_TYPE_PID;
+        }
+    } else {
+        usedPidControllerType = pidProfile()->pidControllerType;
+    }
+
+    if (pidProfile()->dterm_lpf_hz) {
+        dTermLpfFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
+    } else {
+        dTermLpfFilterApplyFn = nullFilterApply;
+    }
+
+    if (usedPidControllerType == PID_TYPE_PIFF) {
+        pidControllerApplyFn = pidApplyFixedWingRateController;
+    } else if (usedPidControllerType == PID_TYPE_PID) {
+        pidControllerApplyFn = pidApplyMulticopterRateController;
+    } else {
+        pidControllerApplyFn = nullRateController;
+    }
+}

src/main/flight/pid.h

@@ -70,9 +70,10 @@ typedef enum {
 
 // TODO(agh): PIDFF
 typedef enum {
-    PID_TYPE_NONE,  // Not used in the current platform/mixer/configuration
+    PID_TYPE_NONE = 0,  // Not used in the current platform/mixer/configuration
     PID_TYPE_PID,   // Uses P, I and D terms
     PID_TYPE_PIFF,  // Uses P, I and FF, ignoring D
+    PID_TYPE_AUTO,  // Autodetect
 } pidType_e;
 
 typedef struct pid8_s {
@@ -98,6 +99,7 @@ typedef enum {
 } itermRelaxType_e;
 
 typedef struct pidProfile_s {
+    uint8_t pidControllerType;
     pidBank_t bank_fw;
     pidBank_t bank_mc;
 
@@ -153,18 +155,16 @@ typedef struct pidAutotuneConfig_s {
 PG_DECLARE_PROFILE(pidProfile_t, pidProfile);
 PG_DECLARE(pidAutotuneConfig_t, pidAutotuneConfig);
 
-static inline const pidBank_t * pidBank(void) { return STATE(FIXED_WING) ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; }
+static uint8_t usedPidControllerType;
-static inline pidBank_t * pidBankMutable(void) { return STATE(FIXED_WING) ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; }
+
+static inline const pidBank_t * pidBank(void) { return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; }
+static inline pidBank_t * pidBankMutable(void) { return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; }
 
 extern int16_t axisPID[];
 extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[];
 
 void pidInit(void);
-
-#ifdef USE_DTERM_NOTCH
 bool pidInitFilters(void);
-#endif
-
 void pidResetErrorAccumulators(void);
 void pidResetTPAFilter(void);
 

src/main/flight/servos.h

@@ -108,9 +108,8 @@ typedef struct servoMixer_s {
 
 #define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS)
 #define MAX_SERVO_SPEED UINT8_MAX
-#define MAX_SERVO_BOXES 3
+#define SERVO_OUTPUT_MAX 2500
-
+#define SERVO_OUTPUT_MIN 500
-#define SERVO_MIXER_INPUT_WIDTH 1000
 
 PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers);
 

src/main/navigation/navigation.c

@@ -1368,6 +1368,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
     switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
         case NAV_WP_ACTION_WAYPOINT:
             calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
+            posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
+            posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
             return NAV_FSM_EVENT_SUCCESS;       // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
 
         case NAV_WP_ACTION_RTH:
@@ -1392,7 +1394,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
                     return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_REACHED
                 }
                 else {
-                    setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
+                    fpVector3_t tmpWaypoint;
+                    tmpWaypoint.x = posControl.activeWaypoint.pos.x;
+                    tmpWaypoint.y = posControl.activeWaypoint.pos.y;
+                    tmpWaypoint.z = scaleRange(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10, posControl.wpInitialDistance),
+                        posControl.wpInitialDistance, posControl.wpInitialDistance / 10,
+                        posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
+                    setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
                     return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
                 }
                 break;
@@ -2040,15 +2048,15 @@ bool isWaypointMissed(const navWaypointPosition_t * waypoint)
 static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome)
 {
     // We consider waypoint reached if within specified radius
-    const uint32_t wpDistance = calculateDistanceToDestination(pos);
+    posControl.wpDistance = calculateDistanceToDestination(pos);
 
     if (STATE(FIXED_WING) && isWaypointHome) {
         // Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check
-        return (wpDistance <= navConfig()->general.waypoint_radius)
+        return (posControl.wpDistance <= navConfig()->general.waypoint_radius)
-                || (wpDistance <= (navConfig()->fw.loiter_radius * 1.10f));  // 10% margin of desired circular loiter radius
+                || (posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f));  // 10% margin of desired circular loiter radius
     }
     else {
-        return (wpDistance <= navConfig()->general.waypoint_radius);
+        return (posControl.wpDistance <= navConfig()->general.waypoint_radius);
     }
 }
 
@@ -2391,9 +2399,6 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
             // Fixed wing climb rate controller is open-loop. We simply move the known altitude target
             float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
 
-            DEBUG_SET(DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, 0, desiredClimbRate);
-            DEBUG_SET(DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, 1, timeDelta * 1000);
-
             if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
                 posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
                 posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, altitudeToUse - 500, altitudeToUse + 500);    // FIXME: calculate sanity limits in a smarter way

src/main/navigation/navigation_multicopter.c

@@ -660,10 +660,6 @@ bool isMulticopterLandingDetected(void)
 
     bool possibleLandingDetected = isAtMinimalThrust && !verticalMovement && !horizontalMovement;
 
-    DEBUG_SET(DEBUG_NAV_LANDING_DETECTOR, 0, isAtMinimalThrust * 100 + !verticalMovement * 10 + !horizontalMovement * 1);
-    DEBUG_SET(DEBUG_NAV_LANDING_DETECTOR, 1, (landingThrSamples == 0) ? 0 : (rcCommandAdjustedThrottle - (landingThrSum / landingThrSamples)));
-    DEBUG_SET(DEBUG_NAV_LANDING_DETECTOR, 2, (currentTimeUs - landingTimer) / 1000);
-
     // If we have surface sensor (for example sonar) - use it to detect touchdown
     if ((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z >= 0)) {
         // TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety.

src/main/navigation/navigation_private.h

@@ -354,6 +354,9 @@ typedef struct {
 
     navWaypointPosition_t       activeWaypoint;     // Local position and initial bearing, filled on waypoint activation
     int8_t                      activeWaypointIndex;
+    float                       wpInitialAltitude; // Altitude at start of WP
+    float                       wpInitialDistance; // Distance when starting flight to WP
+    float                       wpDistance;        // Distance to active WP
 
     /* Internals & statistics */
     int16_t                     rcAdjustment[4];

src/main/sensors/acceleration.c

@@ -81,10 +81,8 @@ STATIC_FASTRAM void *accSoftLpfFilter[XYZ_AXIS_COUNT];
 STATIC_FASTRAM pt1Filter_t accVibeFloorFilter[XYZ_AXIS_COUNT];
 STATIC_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT];
 
-#ifdef USE_ACC_NOTCH
 STATIC_FASTRAM filterApplyFnPtr accNotchFilterApplyFn;
 STATIC_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
-#endif
 
 PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 3);
 
@@ -578,13 +576,11 @@ void accUpdate(void)
         acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]);
     }
 
-#ifdef USE_ACC_NOTCH
     if (accelerometerConfig()->acc_notch_hz) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             acc.accADCf[axis] = accNotchFilterApplyFn(accNotchFilter[axis], acc.accADCf[axis]);
         }
     }
-#endif
 
 }
 
@@ -665,7 +661,6 @@ void accInitFilters(void)
         pt1FilterInit(&accVibeFilter[axis], ACC_VIBE_FILT_HZ, accDt);
     }
 
-#ifdef USE_ACC_NOTCH
     STATIC_FASTRAM biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT];
     accNotchFilterApplyFn = nullFilterApply;
 
@@ -676,7 +671,6 @@ void accInitFilters(void)
             biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, accelerometerConfig()->acc_notch_hz, accelerometerConfig()->acc_notch_cutoff);
         }
     }
-#endif
 
 }
 

src/main/sensors/gyro.c

@@ -84,20 +84,13 @@ STATIC_FASTRAM int32_t gyroADC[XYZ_AXIS_COUNT];
 STATIC_FASTRAM filterApplyFnPtr softLpfFilterApplyFn;
 STATIC_FASTRAM void *softLpfFilter[XYZ_AXIS_COUNT];
 
-#ifdef USE_GYRO_NOTCH_1
 STATIC_FASTRAM filterApplyFnPtr notchFilter1ApplyFn;
 STATIC_FASTRAM void *notchFilter1[XYZ_AXIS_COUNT];
-#endif
-#ifdef USE_GYRO_NOTCH_2
 STATIC_FASTRAM filterApplyFnPtr notchFilter2ApplyFn;
 STATIC_FASTRAM void *notchFilter2[XYZ_AXIS_COUNT];
-#endif
 
-#if defined(USE_GYRO_BIQUAD_RC_FIR2)
-// gyro biquad RC FIR2 filter
 STATIC_FASTRAM filterApplyFnPtr gyroFilterStage2ApplyFn;
 STATIC_FASTRAM void *stage2Filter[XYZ_AXIS_COUNT];
-#endif
 
 #ifdef USE_DYNAMIC_FILTERS
 
@@ -337,16 +330,13 @@ void gyroInitFilters(void)
 {
     STATIC_FASTRAM filter_t gyroFilterLPF[XYZ_AXIS_COUNT];
     softLpfFilterApplyFn = nullFilterApply;
-#ifdef USE_GYRO_NOTCH_1
+
     STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
     notchFilter1ApplyFn = nullFilterApply;
-#endif
+    
-#ifdef USE_GYRO_NOTCH_2
     STATIC_FASTRAM biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
     notchFilter2ApplyFn = nullFilterApply;
-#endif
+    
-
-#ifdef USE_GYRO_BIQUAD_RC_FIR2
     STATIC_FASTRAM biquadFilter_t gyroFilterStage2[XYZ_AXIS_COUNT];
     gyroFilterStage2ApplyFn = nullFilterApply;
 
@@ -357,7 +347,6 @@ void gyroInitFilters(void)
             biquadRCFIR2FilterInit(stage2Filter[axis], gyroConfig()->gyro_stage2_lowpass_hz, getLooptime());
         }
     }
-#endif
 
     if (gyroConfig()->gyro_soft_lpf_hz) {
         
@@ -380,7 +369,6 @@ void gyroInitFilters(void)
         }
     }
 
-#ifdef USE_GYRO_NOTCH_1
     if (gyroConfig()->gyro_soft_notch_hz_1) {
         notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
         for (int axis = 0; axis < 3; axis++) {
@@ -388,9 +376,7 @@ void gyroInitFilters(void)
             biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
         }
     }
-#endif
 
-#ifdef USE_GYRO_NOTCH_2
     if (gyroConfig()->gyro_soft_notch_hz_2) {
         notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
         for (int axis = 0; axis < 3; axis++) {
@@ -398,7 +384,6 @@ void gyroInitFilters(void)
             biquadFilterInitNotch(notchFilter2[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
         }
     }
-#endif
 }
 
 void gyroStartCalibration(void)
@@ -479,44 +464,13 @@ void FAST_CODE NOINLINE gyroUpdate()
 
         DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
 
-#ifdef USE_DYNAMIC_FILTERS
-        if (isDynamicFilterActive()) {
-            if (axis == FD_ROLL) {
-                DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
-                DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
-            }
-        }
-#endif
-
-        if (axis < 2) {
-            DEBUG_SET(DEBUG_STAGE2, axis, lrintf(gyroADCf));
-        }
-
-#ifdef USE_GYRO_BIQUAD_RC_FIR2
         gyroADCf = gyroFilterStage2ApplyFn(stage2Filter[axis], gyroADCf);
-#endif
-
-        if (axis < 2) {
-            DEBUG_SET(DEBUG_STAGE2, axis + 2, lrintf(gyroADCf));
-        }
-
         gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
-
-        DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));
-
-#ifdef USE_GYRO_NOTCH_1
         gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
-#endif
-#ifdef USE_GYRO_NOTCH_2
         gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
-#endif
 
 #ifdef USE_DYNAMIC_FILTERS
         if (isDynamicFilterActive()) {
-            if (axis == FD_ROLL) {
-                DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
-                DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf));
-            }
             gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
             gyroADCf = notchFilterDynApplyFn((filter_t *)&notchFilterDyn[axis], gyroADCf);
             gyroADCf = notchFilterDynApplyFn2((filter_t *)&notchFilterDyn2[axis], gyroADCf);

src/main/target/OMNIBUS/target.h

@@ -154,3 +154,5 @@
 #define TARGET_IO_PORTB         0xffff
 #define TARGET_IO_PORTC         (BIT(13)|BIT(14)|BIT(15))
 #define TARGET_IO_PORTF         (BIT(0)|BIT(1)|BIT(4))
+
+#undef USE_PWM_SERVO_DRIVER

src/main/target/common.h

@@ -54,7 +54,6 @@
 #define USE_TELEMETRY_LTM
 #define USE_TELEMETRY_FRSKY
 
-#define USE_GYRO_BIQUAD_RC_FIR2
 #define USE_MR_BRAKING_MODE
 
 #if defined(STM_FAST_TARGET)
@@ -121,10 +120,6 @@
 #define USE_AUTOTUNE_FIXED_WING
 #define USE_LOG
 #define USE_STATS
-#define USE_GYRO_NOTCH_1
-#define USE_GYRO_NOTCH_2
-#define USE_DTERM_NOTCH
-#define USE_ACC_NOTCH
 #define USE_CMS
 #define CMS_MENU_OSD
 #define USE_GPS_PROTO_NMEA