Add more configurable Dterm approaches (for testing purposes)

src/main/config/config.c

@@ -178,7 +178,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
 
     pidProfile->gyro_soft_lpf = 1;   // filtering ON by default
     pidProfile->dterm_cut_hz = 0;
-    pidProfile->yaw_pterm_cut_hz = 0;
+    pidProfile->delta_from_gyro_error = 0;
 
     pidProfile->P_f[ROLL] = 1.5f;     // new PID with preliminary defaults test carefully
     pidProfile->I_f[ROLL] = 0.3f;

src/main/flight/pid.c

@@ -84,24 +84,29 @@ void pidResetErrorGyro(void)
     errorGyroIf[YAW] = 0.0f;
 }
 
-void setPidDeltaSamples(void) {
+void setPidDeltaSamples(uint8_t filter) {
-    if (targetLooptime < 1000) {
+    if (!filter) {
-        deltaTotalSamples = 8;
+        if (targetLooptime < 1000) {
+            deltaTotalSamples = 8;
+        } else {
+           deltaTotalSamples = 4;
+        }
     } else {
-        deltaTotalSamples = 4;
+        deltaTotalSamples = 1;
     }
 }
 
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
 static filterStatePt1_t DTermState[3];
-static filterStatePt1_t yawPTermState;
+//static filterStatePt1_t yawPTermState;
 
 static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 {
     float RateError, AngleRate, gyroRate;
     float ITerm,PTerm,DTerm;
+    static float lastGyroRate[3];
     static float lastError[3];
     static float previousDelta[3][8];
     float delta, deltaSum;
@@ -109,7 +114,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     float horizonLevelStrength = 1;
     static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
 
-    if (!deltaTotalSamples) setPidDeltaSamples();
+    if (!deltaTotalSamples) setPidDeltaSamples(pidProfile->dterm_cut_hz);
 
     if (FLIGHT_MODE(HORIZON_MODE)) {
         // Figure out the raw stick positions
@@ -167,10 +172,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         // -----calculate P component
         PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
 
-        if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
-            PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
-        }
-
         // -----calculate I component.
         errorGyroIf[axis] = constrainf(errorGyroIf[axis] + 0.5f * (lastError[axis] + RateError) * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
 
@@ -189,19 +190,26 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         ITerm = errorGyroIf[axis];
 
         //-----calculate D-term
-        delta = RateError - lastError[axis];
+        if (!pidProfile->delta_from_gyro_error) {
-        lastError[axis] = RateError;
+            delta = RateError - lastError[axis];
+            lastError[axis] = RateError;
+        } else {
+            delta = -(gyroRate - lastGyroRate[axis]);  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
+            lastGyroRate[axis] = gyroRate;
+        }
 
         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
         // would be scaled by different dt each time. Division by dT fixes that.
         delta *= (1.0f / dT);
 
+        deltaSum = 0;
         if (pidProfile->dterm_cut_hz) {
             // Dterm low pass
             deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
-        } else {
+        }
-            // Apply moving average
+
-            deltaSum = 0;
+        // Apply moving average
+        if (deltaTotalSamples > 1) {
             for (deltaCount = deltaTotalSamples-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
             previousDelta[axis][0] = delta;
             for (deltaCount = 0; deltaCount < deltaTotalSamples; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
@@ -237,12 +245,13 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
     static int32_t previousDelta[3][8];
     int32_t PTerm, ITerm, DTerm;
     static int32_t lastError[3] = { 0, 0, 0 };
+    static int32_t lastGyroRate[3] = { 0, 0, 0 };
     static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
-    int32_t AngleRateTmp, RateError;
+    int32_t AngleRateTmp, RateError, gyroRate;
 
     int8_t horizonLevelStrength = 100;
 
-    if (!deltaTotalSamples) setPidDeltaSamples();
+    if (!deltaTotalSamples) setPidDeltaSamples(pidProfile->dterm_cut_hz);
 
     if (FLIGHT_MODE(HORIZON_MODE)) {
         // Figure out the raw stick positions
@@ -290,15 +299,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
         // -----calculate scaled error.AngleRates
         // multiplication of rcCommand corresponds to changing the sticks scaling here
-        RateError = AngleRateTmp - (gyroADC[axis] / 4);
+        gyroRate = gyroADC[axis] / 4;
+        RateError = AngleRateTmp - gyroRate;
 
         // -----calculate P component
         PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
 
-        if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
-            PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
-        }
-
         // -----calculate I component
         // there should be no division before accumulating the error to integrator, because the precision would be reduced.
         // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
@@ -323,24 +329,30 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         }
 
         //-----calculate D-term
-        delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
+        if (!pidProfile->delta_from_gyro_error) {   // quick and dirty solution for testing
-        lastError[axis] = RateError;
+            delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
+            lastError[axis] = RateError;
+        } else {
+            delta = -(gyroRate - lastGyroRate[axis]);  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
+            lastGyroRate[axis] = gyroRate;
+        }
 
         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
         // would be scaled by different dt each time. Division by dT fixes that.
         delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
 
+        // Apply moving average
+        deltaSum = 0;
         if (pidProfile->dterm_cut_hz) {
-            // Dterm delta low pass
+            // Dterm low pass
             deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
-        } else {
+        }
-            // Apply moving average
+        if (deltaTotalSamples > 1) {
-            deltaSum = 0;
             for (deltaCount = deltaTotalSamples-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
             previousDelta[axis][0] = delta;
             for (deltaCount = 0; deltaCount < deltaTotalSamples; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
-            deltaSum = (deltaSum / deltaTotalSamples) * 3; // get old scaling by multiplying with 3
         }
+        deltaSum = (deltaSum / deltaTotalSamples) * 3; // get old scaling by multiplying with 3
 
         DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
 

src/main/flight/pid.h

@@ -59,7 +59,7 @@ typedef struct pidProfile_s {
 
     uint16_t yaw_p_limit;                   // set P term limit (fixed value was 300)
     uint8_t dterm_cut_hz;                   // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
-    uint8_t yaw_pterm_cut_hz;               // Used for filering Pterm noise on noisy frames
+    uint8_t delta_from_gyro_error;          // Used for filering Pterm noise on noisy frames
     uint8_t gyro_soft_lpf;                  // Gyro FIR filter
 
 #ifdef GTUNE

src/main/io/i2c_bst.c

@@ -996,7 +996,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
 			bstWrite16(masterConfig.gyro_lpf);
 			bstWrite8(0);//masterConfig.profile[0].pidProfile.gyro_cut_hz);
 			bstWrite8(masterConfig.profile[0].pidProfile.dterm_cut_hz);
-			bstWrite8(masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz);
+			bstWrite8(masterConfig.profile[0].pidProfile.delta_from_gyro_error);
 			break;
 		default:
 			// we do not know how to handle the (valid) message, indicate error BST
@@ -1461,7 +1461,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
 			masterConfig.gyro_lpf = bstRead16();
 			bstRead8();//masterConfig.profile[0].pidProfile.gyro_cut_hz = bstRead8();
 			masterConfig.profile[0].pidProfile.dterm_cut_hz = bstRead8();
-			masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz = bstRead8();
+			masterConfig.profile[0].pidProfile.delta_from_gyro_error = bstRead8();
 			break;
 
 	    default:

src/main/io/serial_cli.c

@@ -657,7 +657,7 @@ const clivalue_t valueTable[] = {
 
     { "gyro_soft_lpf",              VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_OFF_ON } },
     { "dterm_cut_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } },
-    { "yaw_pterm_cut_hz",           VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz, .config.minmax = {0, 200 } },
+    { "delta_from_gyro_error",      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.delta_from_gyro_error, .config.minmax = {0, 1} },
 
 #ifdef BLACKBOX
     { "blackbox_rate_num",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_num, .config.minmax = { 1,  32 } },