Refactor yaw precomp

src/main/blackbox/blackbox.c

@@ -1640,8 +1640,8 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE("yaw_precomp", "%d,%d,%d",               currentPidProfile->yaw_precomp_cutoff,
                                                                             currentPidProfile->yaw_cyclic_ff_gain,
                                                                             currentPidProfile->yaw_collective_ff_gain);
-        BLACKBOX_PRINT_HEADER_LINE("yaw_precomp_impulse", "%d,%d",          currentPidProfile->yaw_collective_dynamic_gain,
-                                                                            currentPidProfile->yaw_collective_dynamic_decay);
+        BLACKBOX_PRINT_HEADER_LINE("yaw_inertia_precomp", "%d,%d",          currentPidProfile->yaw_inertia_precomp_gain,
+                                                                            currentPidProfile->yaw_inertia_precomp_cutoff);
         BLACKBOX_PRINT_HEADER_LINE("yaw_tta", "%d,%d",                      currentPidProfile->governor.tta_gain,
                                                                             currentPidProfile->governor.tta_limit);
         BLACKBOX_PRINT_HEADER_LINE("hsi_gain", "%d,%d",                     currentPidProfile->pid[PID_ROLL].O,

src/main/cli/settings.c

@@ -1109,8 +1109,9 @@ const clivalue_t valueTable[] = {
 
     { "yaw_cyclic_ff_gain",           VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_cyclic_ff_gain) },
     { "yaw_collective_ff_gain",       VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_collective_ff_gain) },
-    { "yaw_collective_dynamic_gain",  VAR_INT8  | PROFILE_VALUE, .config.minmax = { -125, 125 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_collective_dynamic_gain) },
-    { "yaw_collective_dynamic_decay", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_collective_dynamic_decay) },
+
+    { "yaw_inertia_precomp_gain",      VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_inertia_precomp_gain) },
+    { "yaw_inertia_precomp_cutoff",    VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_inertia_precomp_cutoff) },
 
     { "pitch_collective_ff_gain",   VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, pitch_collective_ff_gain) },
 

src/main/fc/rc_adjustments.c

@@ -283,10 +283,10 @@ static int getAdjustmentValue(adjustmentFunc_e adjFunc)
             value = currentPidProfile->yaw_collective_ff_gain;
             break;
         case ADJUSTMENT_YAW_COLLECTIVE_DYN:
-            value = currentPidProfile->yaw_collective_dynamic_gain;
+            value = 0;
             break;
         case ADJUSTMENT_YAW_COLLECTIVE_DECAY:
-            value = currentPidProfile->yaw_collective_dynamic_decay;
+            value = 0;
             break;
         case ADJUSTMENT_PITCH_COLLECTIVE_FF:
             value = currentPidProfile->pitch_collective_ff_gain;
@@ -390,6 +390,12 @@ static int getAdjustmentValue(adjustmentFunc_e adjFunc)
         case ADJUSTMENT_ACC_TRIM_ROLL:
             value = accelerometerConfig()->accelerometerTrims.values.roll;
             break;
+        case ADJUSTMENT_INERTIA_PRECOMP_GAIN:
+            value = currentPidProfile->yaw_inertia_precomp_gain;
+            break;
+        case ADJUSTMENT_INERTIA_PRECOMP_CUTOFF:
+            value = currentPidProfile->yaw_inertia_precomp_cutoff;
+            break;
         case ADJUSTMENT_FUNCTION_COUNT:
             break;
     }
@@ -494,10 +500,8 @@ static void setAdjustmentValue(adjustmentFunc_e adjFunc, int value)
             currentPidProfile->yaw_collective_ff_gain = value;
             break;
         case ADJUSTMENT_YAW_COLLECTIVE_DYN:
-            currentPidProfile->yaw_collective_dynamic_gain = value;
             break;
         case ADJUSTMENT_YAW_COLLECTIVE_DECAY:
-            currentPidProfile->yaw_collective_dynamic_decay = value;
             break;
         case ADJUSTMENT_PITCH_COLLECTIVE_FF:
             currentPidProfile->pitch_collective_ff_gain = value;
@@ -601,6 +605,12 @@ static void setAdjustmentValue(adjustmentFunc_e adjFunc, int value)
         case ADJUSTMENT_ACC_TRIM_ROLL:
             accelerometerConfigMutable()->accelerometerTrims.values.roll = value;
             break;
+        case ADJUSTMENT_INERTIA_PRECOMP_GAIN:
+            currentPidProfile->yaw_inertia_precomp_gain = value;
+            break;
+        case ADJUSTMENT_INERTIA_PRECOMP_CUTOFF:
+            currentPidProfile->yaw_inertia_precomp_cutoff = value;
+            break;
         case ADJUSTMENT_FUNCTION_COUNT:
             break;
     }

src/main/fc/rc_adjustments.h

@@ -127,6 +127,10 @@ typedef enum {
     ADJUSTMENT_ACC_TRIM_PITCH           = 64,
     ADJUSTMENT_ACC_TRIM_ROLL            = 65,
 
+    // Yaw Inertia precomp
+    ADJUSTMENT_INERTIA_PRECOMP_GAIN     = 66,
+    ADJUSTMENT_INERTIA_PRECOMP_CUTOFF   = 67,
+
     ADJUSTMENT_FUNCTION_COUNT
 } adjustmentFunc_e;
 

src/main/flight/mixer.c

@@ -351,10 +351,13 @@ static void mixerUpdateMotorizedTail(void)
     // Motorized tail control
     if (mixerIsTailMode(TAIL_MODE_MOTORIZED)) {
         // Yaw input value - positive is against torque
-        const float yaw = mixer.input[MIXER_IN_STABILIZED_YAW] * mixerRotationSign();
+        float yaw = mixer.input[MIXER_IN_STABILIZED_YAW] * mixerRotationSign();
 
         // Add center trim
-        float throttle = yaw + mixer.tailCenterTrim;
+        yaw += mixer.tailCenterTrim;
+
+        // Square root law
+        float throttle = sqrtf(fmaxf(yaw, 0));
 
         // Apply minimum throttle
         throttle = fmaxf(throttle, mixer.tailMotorIdle);
@@ -373,10 +376,13 @@ static void mixerUpdateMotorizedTail(void)
     // Bidirectional tail motor
     else if (mixerIsTailMode(TAIL_MODE_BIDIRECTIONAL)) {
         // Yaw input value - positive is against torque
-        const float yaw = mixer.input[MIXER_IN_STABILIZED_YAW] * mixerRotationSign();
+        float yaw = mixer.input[MIXER_IN_STABILIZED_YAW] * mixerRotationSign();
 
         // Add center trim
-        float throttle = yaw + mixer.tailCenterTrim;
+        yaw += mixer.tailCenterTrim;
+
+        // Use square root law
+        float throttle = copysignf(sqrtf(fabsf(yaw)), yaw);
 
         // Apply minimum throttle
         if (throttle > -mixer.tailMotorIdle && throttle < mixer.tailMotorIdle)

src/main/flight/pid.c

@@ -217,17 +217,15 @@ void INIT_CODE pidInitProfile(const pidProfile_t *pidProfile)
     pid.yawCCWStopGain = pidProfile->yaw_ccw_stop_gain / 100.0f;
 
     // Collective/cyclic deflection lowpass filters
-    lowpassFilterInit(&pid.precomp.collDeflectionFilter, pidProfile->yaw_precomp_filter_type, pidProfile->yaw_precomp_cutoff, pid.freq, 0);
-    lowpassFilterInit(&pid.precomp.pitchDeflectionFilter, pidProfile->yaw_precomp_filter_type, pidProfile->yaw_precomp_cutoff, pid.freq, 0);
-    lowpassFilterInit(&pid.precomp.rollDeflectionFilter, pidProfile->yaw_precomp_filter_type, pidProfile->yaw_precomp_cutoff, pid.freq, 0);
+    lowpassFilterInit(&pid.precomp.yawPrecompFilter, pidProfile->yaw_precomp_filter_type, pidProfile->yaw_precomp_cutoff, pid.freq, 0);
 
-    // Collective dynamic filter
-    pt1FilterInit(&pid.precomp.collDynamicFilter, 100.0f / constrainf(pidProfile->yaw_collective_dynamic_decay, 1, 250), pid.freq);
+    // RPM change filter
+    difFilterInit(&pid.precomp.yawInertiaFilter, pidProfile->yaw_inertia_precomp_cutoff / 10.0f, pid.freq);
 
     // Tail/yaw precomp
-    pid.precomp.yawCyclicFFGain = pidProfile->yaw_cyclic_ff_gain / 100.0f;
     pid.precomp.yawCollectiveFFGain = pidProfile->yaw_collective_ff_gain / 100.0f;
-    pid.precomp.yawCollectiveDynamicGain = pidProfile->yaw_collective_dynamic_gain / 100.0f;
+    pid.precomp.yawCyclicFFGain = pidProfile->yaw_cyclic_ff_gain / 100.0f;
+    pid.precomp.yawInertiaGain = pidProfile->yaw_inertia_precomp_gain / 100.0f;
 
     // Pitch precomp
     pid.precomp.pitchCollectiveFFGain = pidProfile->pitch_collective_ff_gain / 500.0f;
@@ -385,48 +383,74 @@ static void pidApplyCollective(void)
     pid.collective = collective / 1000;
 }
 
+static inline float dragCoef(float x)
+{
+  /**
+   * Alternatives
+   *   - 7th order approx: (x^7 + 3x^2 + x) / 5
+   *   - 5th order approx: (x^5 + 3x^2 + x) / 5
+   *   - 2nd order approx: x^2
+   *   - 1st order approx: x
+   *
+   * The 7th order is closest to simulation results, but would cause excessive
+   * yaw deflection at high collective angles (>14deg).
+   *
+   * The second order approx is accurate up to 12deg, and doesn't cause issues
+   * with excessive yaw or saturation.
+   */
+  return x * x;
+}
+
 static void pidApplyPrecomp(void)
 {
     // Yaw precompensation direction and ratio
     const float masterGain = mixerRotationSign() * getSpoolUpRatio();
 
     // Get actual control deflections (from previous cycle)
-    const float collectiveDeflection = filterApply(&pid.precomp.collDeflectionFilter, mixerGetInput(MIXER_IN_STABILIZED_COLLECTIVE));
-    const float pitchDeflection = filterApply(&pid.precomp.pitchDeflectionFilter, mixerGetInput(MIXER_IN_STABILIZED_PITCH));
-    const float rollDeflection = filterApply(&pid.precomp.rollDeflectionFilter, mixerGetInput(MIXER_IN_STABILIZED_ROLL));
+    const float collectiveDeflection = getCollectiveDeflection();
+    const float cyclicDeflection = getCyclicDeflection();
 
-    // Calculate cyclic deflection from the filtered controls
-    const float cyclicDeflection = sqrtf(sq(pitchDeflection) + sq(rollDeflection));
 
-    // Collective High Pass Filter (this is possible with PT1)
-    const float collectiveLF = pt1FilterApply(&pid.precomp.collDynamicFilter, collectiveDeflection);
-    const float collectiveHF = collectiveDeflection - collectiveLF;
+  //// Main rotor intertia precomp
+
+    // Normalised effective rotor speed
+    //const float rotorSpeed = getHeadSpeedf() / 3000;
+    const float rotorSpeed = (getHeadSpeedf() + mixerRotationSign() * pidGetSetpoint(FD_YAW) / 6) / 3000;
+
+    // Rotorspeed derivative
+    const float speedChange = difFilterApply(&pid.precomp.yawInertiaFilter, rotorSpeed);
+
+    // Momentum change precomp
+    const float torquePrecomp = speedChange * pid.precomp.yawInertiaGain;
 
 
   //// Collective-to-Yaw Precomp
 
-    // Collective components
-    const float yawCollectiveFF = fabsf(collectiveDeflection) * pid.precomp.yawCollectiveFFGain;
-    const float yawCollectiveHF = fabsf(collectiveHF) * pid.precomp.yawCollectiveDynamicGain;
+    // Equivalent Average main rotor deflection
+    const float mainDeflection =
+      fabsf(collectiveDeflection) * pid.precomp.yawCollectiveFFGain +
+      fabsf(cyclicDeflection) * pid.precomp.yawCyclicFFGain;
+
+    // Drag estimate
+    const float mainDrag = dragCoef(mainDeflection);
 
-    // Cyclic component
-    float yawCyclicFF = fabsf(cyclicDeflection) * pid.precomp.yawCyclicFFGain;
+    // Apply filter
+    const float mainPrecomp = filterApply(&pid.precomp.yawPrecompFilter, mainDrag);
 
-    // Calculate total precompensation
-    float yawPrecomp = (yawCollectiveFF + yawCollectiveHF + yawCyclicFF) * masterGain;
+    // Total precomp with direction
+    const float totalPrecomp = (mainPrecomp + torquePrecomp) * masterGain;
 
     // Add to YAW feedforward
-    pid.data[FD_YAW].F += yawPrecomp;
-    pid.data[FD_YAW].pidSum += yawPrecomp;
-
-    DEBUG(YAW_PRECOMP, 0, collectiveDeflection * 1000);
-    DEBUG(YAW_PRECOMP, 1, collectiveLF * 1000);
-    DEBUG(YAW_PRECOMP, 2, collectiveHF * 1000);
-    DEBUG(YAW_PRECOMP, 3, cyclicDeflection * 1000);
-    DEBUG(YAW_PRECOMP, 4, yawCollectiveFF * 1000);
-    DEBUG(YAW_PRECOMP, 5, yawCollectiveHF * 1000);
-    DEBUG(YAW_PRECOMP, 6, yawCyclicFF * 1000);
-    DEBUG(YAW_PRECOMP, 7, yawPrecomp * 1000);
+    pid.data[FD_YAW].F += totalPrecomp;
+    pid.data[FD_YAW].pidSum += totalPrecomp;
+
+    DEBUG(YAW_PRECOMP, 0, totalPrecomp * 1000);
+    DEBUG(YAW_PRECOMP, 1, mainPrecomp * 1000);
+    DEBUG(YAW_PRECOMP, 2, mainDeflection * 1000);
+    DEBUG(YAW_PRECOMP, 3, collectiveDeflection * 1000);
+    DEBUG(YAW_PRECOMP, 4, cyclicDeflection * 1000);
+    DEBUG(YAW_PRECOMP, 6, speedChange * 1000);
+    DEBUG(YAW_PRECOMP, 7, torquePrecomp * 1000);
 
 
   //// Collective-to-Pitch precomp

src/main/flight/pid.h

@@ -79,15 +79,12 @@ typedef struct {
 
 typedef struct {
 
-    filter_t collDeflectionFilter;
-    filter_t pitchDeflectionFilter;
-    filter_t rollDeflectionFilter;
+    filter_t yawPrecompFilter;
+    difFilter_t yawInertiaFilter;
 
-    pt1Filter_t collDynamicFilter;
-
-    float yawCyclicFFGain;
     float yawCollectiveFFGain;
-    float yawCollectiveDynamicGain;
+    float yawCyclicFFGain;
+    float yawInertiaGain;
 
     float pitchCollectiveFFGain;
 

src/main/msp/msp.c

@@ -1820,8 +1820,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
         sbufWriteU8(dst, currentPidProfile->yaw_precomp_cutoff);
         sbufWriteU8(dst, currentPidProfile->yaw_cyclic_ff_gain);
         sbufWriteU8(dst, currentPidProfile->yaw_collective_ff_gain);
-        sbufWriteU8(dst, currentPidProfile->yaw_collective_dynamic_gain);
-        sbufWriteU8(dst, currentPidProfile->yaw_collective_dynamic_decay);
+        sbufWriteU8(dst, 0); // was currentPidProfile->yaw_collective_dynamic_gain
+        sbufWriteU8(dst, 0); // was currentPidProfile->yaw_collective_dynamic_decay
         sbufWriteU8(dst, currentPidProfile->pitch_collective_ff_gain);
         /* Angle mode */
         sbufWriteU8(dst, currentPidProfile->angle.level_strength);
@@ -1842,6 +1842,9 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
         sbufWriteU8(dst, currentPidProfile->bterm_cutoff[0]);
         sbufWriteU8(dst, currentPidProfile->bterm_cutoff[1]);
         sbufWriteU8(dst, currentPidProfile->bterm_cutoff[2]);
+        /* Inertia precomps */
+        sbufWriteU8(dst, currentPidProfile->yaw_inertia_precomp_gain);
+        sbufWriteU8(dst, currentPidProfile->yaw_inertia_precomp_cutoff);
         break;
 
     case MSP_RESCUE_PROFILE:
@@ -2621,8 +2624,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
         currentPidProfile->yaw_precomp_cutoff = sbufReadU8(src);
         currentPidProfile->yaw_cyclic_ff_gain = sbufReadU8(src);
         currentPidProfile->yaw_collective_ff_gain = sbufReadU8(src);
-        currentPidProfile->yaw_collective_dynamic_gain = sbufReadU8(src);
-        currentPidProfile->yaw_collective_dynamic_decay = sbufReadU8(src);
+        sbufReadU8(src); // was currentPidProfile->yaw_collective_dynamic_gain
+        sbufReadU8(src); // was currentPidProfile->yaw_collective_dynamic_decay
         currentPidProfile->pitch_collective_ff_gain = sbufReadU8(src);
         /* Angle mode */
         currentPidProfile->angle.level_strength = sbufReadU8(src);
@@ -2649,6 +2652,11 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
             currentPidProfile->bterm_cutoff[1] = sbufReadU8(src);
             currentPidProfile->bterm_cutoff[2] = sbufReadU8(src);
         }
+        /* Inertia precomps */
+        if (sbufBytesRemaining(src) >= 2) {
+            currentPidProfile->yaw_inertia_precomp_gain = sbufReadU8(src);
+            currentPidProfile->yaw_inertia_precomp_cutoff = sbufReadU8(src);
+        }
         /* Load new values */
         pidInitProfile(currentPidProfile);
         break;

src/main/pg/governor.c

@@ -42,7 +42,7 @@ PG_RESET_TEMPLATE(governorConfig_t, governorConfig,
     .gov_pwr_filter = 5,
     .gov_rpm_filter = 10,
     .gov_tta_filter = 0,
-    .gov_ff_filter = 10,
+    .gov_ff_filter = 5,
     .gov_spoolup_min_throttle = 5,
 );
 

src/main/pg/pid.c

@@ -81,8 +81,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .yaw_precomp_filter_type = LPF_1ST_ORDER,
         .yaw_cyclic_ff_gain = 0,
         .yaw_collective_ff_gain = 30,
-        .yaw_collective_dynamic_gain = 0,
-        .yaw_collective_dynamic_decay = 25,
+        .yaw_inertia_precomp_gain = 0,
+        .yaw_inertia_precomp_cutoff = 25,
         .pitch_collective_ff_gain = 0,
         .cyclic_cross_coupling_gain = 50,
         .cyclic_cross_coupling_ratio = 0,

src/main/pg/pid.h

@@ -166,8 +166,8 @@ typedef struct pidProfile_s {
 
     uint8_t             yaw_cyclic_ff_gain;
     uint8_t             yaw_collective_ff_gain;
-    uint8_t             yaw_collective_dynamic_gain;
-    uint8_t             yaw_collective_dynamic_decay;
+    uint8_t             yaw_inertia_precomp_gain;
+    uint8_t             yaw_inertia_precomp_cutoff;
 
     uint8_t             pitch_collective_ff_gain;