Optional SUPER EXPO for yaw // Optional always Iterm reset // Rework Iterm reset

src/main/blackbox/blackbox.c

@@ -1280,8 +1280,8 @@ static bool blackboxWriteSysinfo()
                                      masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count);
             break;
         case 36:
-            blackboxPrintfHeaderLine("dynamic_dterm_threshold:%d",
+            blackboxPrintfHeaderLine("dterm_differentiator:%d",
-                                     masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_dterm_threshold);
+                                     masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_differentiator);
             break;
         case 37:
             blackboxPrintfHeaderLine("rollPitchItermResetRate:%d",

src/main/config/config.c

@@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 132;
+static const uint8_t EEPROM_CONF_VERSION = 133;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -178,9 +178,9 @@ static void resetPidProfile(pidProfile_t *pidProfile)
 
     pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
     pidProfile->yaw_lpf_hz = 70.0f;
-    pidProfile->dterm_average_count = 0;
     pidProfile->dterm_differentiator = 1;
     pidProfile->rollPitchItermResetRate = 200;
+    pidProfile->rollPitchItermResetAlways = 0;
     pidProfile->yawItermResetRate = 50;
     pidProfile->dterm_lpf_hz = 70.0f;    // filtering ON by default
     pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
@@ -454,6 +454,8 @@ static void resetConf(void)
     masterConfig.rxConfig.fpvCamAngleDegrees = 0;
     masterConfig.rxConfig.max_aux_channel = 6;
     masterConfig.rxConfig.superExpoFactor = 30;
+    masterConfig.rxConfig.superExpoFactorYaw = 30;
+    masterConfig.rxConfig.superExpoYawMode = 0;
 
     resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);
 

src/main/flight/pid.c

@@ -80,11 +80,17 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) {
 }
 
 float calculateExpoPlus(int axis, rxConfig_t *rxConfig) {
-	float propFactor;
+    float propFactor;
+    float superExpoFactor;
 
-    propFactor = 1.0f - ((rxConfig->superExpoFactor / 100.0f) * (ABS(rcCommand[axis]) / 500.0f));
+    if (axis == YAW && !rxConfig->superExpoYawMode) {
+        propFactor = 1.0f;
+    } else {
+        superExpoFactor = (axis == YAW) ? rxConfig->superExpoFactorYaw : rxConfig->superExpoFactor;
+        propFactor = 1.0f - ((superExpoFactor / 100.0f) * (ABS(rcCommand[axis]) / 500.0f));
+    }
 
-	return propFactor;
+    return propFactor;
 }
 
 void pidResetErrorAngle(void)
@@ -195,7 +201,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         RateError = AngleRate - gyroRate;
 
         // -----calculate P component
-        if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) {
+        if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
             PTerm = (luxPTermScale * pidProfile->P8[axis] * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig));
         } else {
             PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor;
@@ -209,12 +215,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         // -----calculate I component.
         errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * pidProfile->I8[axis], -250.0f, 250.0f);
 
-        if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) {
+        if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) {
-            if (ABS(gyroRate / 4.1f) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0;
+            if (ABS(gyroRate / 4.1f) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD);
         }
 
         if (axis == YAW) {
-            if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0;
+            if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW);
         }
 
         if (antiWindupProtection || motorLimitReached) {
@@ -337,7 +343,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
         RateError = AngleRateTmp - gyroRate;
 
         // -----calculate P component
-        if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) {
+        if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
             PTerm = (pidProfile->P8[axis] * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7;
         } else {
             PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
@@ -359,12 +365,12 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
         // I coefficient (I8) moved before integration to make limiting independent from PID settings
         errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
 
-        if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) {
+        if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) {
-            if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = 0;
+            if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD);
         }
 
         if (axis == YAW) {
-            if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = 0;
+            if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW);
         }
 
         if (antiWindupProtection || motorLimitReached) {

src/main/flight/pid.h

@@ -25,6 +25,8 @@
 #define PID_LAST_RATE_COUNT 7
 #define PID_DTERM_FIR_MAX_LENGTH 7
 #define PID_MAX_DIFFERENTIATOR (PID_DTERM_FIR_MAX_LENGTH-2)
+#define ITERM_RESET_THRESHOLD 20
+#define ITERM_RESET_THRESHOLD_YAW 10
 
 typedef enum {
     PIDROLL,
@@ -57,6 +59,12 @@ typedef enum {
     RESET_ITERM_AND_REDUCE_PID
 } pidErrorResetOption_e;
 
+typedef enum {
+    SUPEREXPO_YAW_OFF = 0,
+    SUPEREXPO_YAW_ON,
+    SUPEREXPO_YAW_ALWAYS
+} pidSuperExpoYaw_e;
+
 #define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
 
 typedef struct pidProfile_s {
@@ -70,8 +78,9 @@ typedef struct pidProfile_s {
 
     float dterm_lpf_hz;                     // Delta Filter in hz
     float yaw_lpf_hz;                       // Additional yaw filter when yaw axis too noisy
-    uint16_t rollPitchItermResetRate;        // Experimental threshold for resetting iterm for pitch and roll on certain rates
+    uint16_t rollPitchItermResetRate;       // Experimental threshold for resetting iterm for pitch and roll on certain rates
-    uint16_t yawItermResetRate;              // Experimental threshold for resetting iterm for yaw on certain rates
+    uint8_t rollPitchItermResetAlways;      // Reset Iterm also without SUPER EXPO
+    uint16_t yawItermResetRate;             // Experimental threshold for resetting iterm for yaw on certain rates
     uint8_t deltaMethod;                    // Alternative delta Calculation
     uint16_t yaw_p_limit;
     uint8_t dterm_average_count;            // Configurable delta count for dterm

src/main/io/serial_cli.c

@@ -440,6 +440,10 @@ static const char * const lookupTableDebug[] = {
     "AIRMODE"
 };
 
+static const char * const lookupTableSuperExpoYaw[] = {
+    "OFF", "ON", "ALWAYS"
+};
+
 typedef struct lookupTableEntry_s {
     const char * const *values;
     const uint8_t valueCount;
@@ -466,6 +470,7 @@ typedef enum {
     TABLE_MAG_HARDWARE,
     TABLE_DELTA_METHOD,
 	TABLE_DEBUG,
+    TABLE_SUPEREXPO_YAW,
 } lookupTableIndex_e;
 
 static const lookupTableEntry_t lookupTables[] = {
@@ -488,7 +493,8 @@ static const lookupTableEntry_t lookupTables[] = {
     { lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) },
     { lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) },
     { lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) },
-    { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }
+    { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) },
+    { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) }
 };
 
 #define VALUE_TYPE_OFFSET 0
@@ -688,6 +694,8 @@ const clivalue_t valueTable[] = {
     { "tpa_rate",                   VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0,  CONTROL_RATE_CONFIG_TPA_MAX} },
     { "tpa_breakpoint",             VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN,  PWM_RANGE_MAX} },
     { "super_expo_factor",          VAR_UINT8  | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactor, .config.minmax = {1, 100 } },
+    { "super_expo_factor_yaw",      VAR_UINT8  | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactorYaw, .config.minmax = {1, 100 } },
+    { "super_expo_yaw",             VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.rxConfig.superExpoYawMode, .config.lookup = { TABLE_SUPEREXPO_YAW } },
 
     { "failsafe_delay",             VAR_UINT8  | MASTER_VALUE,  &masterConfig.failsafeConfig.failsafe_delay, .config.minmax = { 0,  200 } },
     { "failsafe_off_delay",         VAR_UINT8  | MASTER_VALUE,  &masterConfig.failsafeConfig.failsafe_off_delay, .config.minmax = { 0,  200 } },
@@ -722,6 +730,7 @@ const clivalue_t valueTable[] = {
     { "pid_delta_method",           VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
     { "dterm_lowpass_hz",           VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
     { "dterm_differentiator",       VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_differentiator, .config.lookup = { TABLE_OFF_ON } },
+    { "iterm_always_reset",         VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } },
     { "iterm_reset_degrees",        VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } },
     { "yaw_iterm_reset_degrees",    VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } },
     { "yaw_lowpass_hz",             VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },

src/main/rx/rx.h

@@ -125,6 +125,8 @@ typedef struct rxConfig_s {
     uint8_t fpvCamAngleDegrees;             // Camera angle to be scaled into rc commands
     uint8_t max_aux_channel;
     uint8_t superExpoFactor;                 // Super Expo Factor
+    uint8_t superExpoFactorYaw;              // Super Expo Factor Yaw
+    uint8_t superExpoYawMode;                // Seperate Super expo for yaw
 
     uint16_t rx_min_usec;
     uint16_t rx_max_usec;