absolute control feature

src/main/flight/pid.c

@@ -147,11 +147,14 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .smart_feedforward = false,
         .iterm_relax = ITERM_RELAX_OFF,
         .iterm_relax_cutoff_low = 3,
-        .iterm_relax_cutoff_high = 15,      
+        .iterm_relax_cutoff_high = 30,
         .acro_trainer_angle_limit = 20,
         .acro_trainer_lookahead_ms = 50,
         .acro_trainer_debug_axis = FD_ROLL,
         .acro_trainer_gain = 75
+        .abs_control_gain = 0,
+        .abs_control_limit = 90,
+        .abs_control_error_limit = 20,
     );
 }
 
@@ -168,13 +171,6 @@ static void pidSetTargetLooptime(uint32_t pidLooptime)
     dT = (float)targetPidLooptime * 0.000001f;
 }
 
-void pidResetITerm(void)
-{
-    for (int axis = 0; axis < 3; axis++) {
-        pidData[axis].I = 0.0f;
-    }
-}
-
 static FAST_RAM float itermAccelerator = 1.0f;
 
 void pidSetItermAccelerator(float newItermAccelerator)
@@ -320,6 +316,18 @@ FAST_RAM_ZERO_INIT float throttleBoost;
 pt1Filter_t throttleLpf;
 static FAST_RAM_ZERO_INIT bool itermRotation;
 static FAST_RAM_ZERO_INIT bool smartFeedforward;
+static FAST_RAM_ZERO_INIT float axisError[3];
+static FAST_RAM_ZERO_INIT float acGain;
+static FAST_RAM_ZERO_INIT float acLimit;
+static FAST_RAM_ZERO_INIT float acErrorLimit;
+
+void pidResetITerm(void)
+{
+    for (int axis = 0; axis < 3; axis++) {
+        pidData[axis].I = 0.0f;
+        axisError[axis] = 0.0f;
+    }
+}
 
 #ifdef USE_ACRO_TRAINER
 static FAST_RAM_ZERO_INIT float acroTrainerAngleLimit;
@@ -364,13 +372,8 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     crashLimitYaw = pidProfile->crash_limit_yaw;
     itermLimit = pidProfile->itermLimit;
     throttleBoost = pidProfile->throttle_boost * 0.1f;
-<<<<<<< HEAD
     itermRotation = pidProfile->iterm_rotation;
     smartFeedforward = pidProfile->smart_feedforward;
-=======
-    smartFeedforward = pidProfile->smart_feedforward;
-    itermRotation = pidProfile->iterm_rotation;
->>>>>>> incorporate style feedback
     itermRelax = pidProfile->iterm_relax;
     itermRelaxCutoffLow = pidProfile->iterm_relax_cutoff_low;
     itermRelaxCutoffHigh = pidProfile->iterm_relax_cutoff_high;
@@ -381,6 +384,10 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     acroTrainerDebugAxis = pidProfile->acro_trainer_debug_axis;
     acroTrainerGain = (float)pidProfile->acro_trainer_gain / 10.0f;
 #endif // USE_ACRO_TRAINER
+
+    acGain = (float)pidProfile->abs_control_gain;
+    acLimit = (float)pidProfile->abs_control_limit;
+    acErrorLimit = (float)pidProfile->abs_control_error_limit;
 }
 
 void pidInit(const pidProfile_t *pidProfile)
@@ -568,17 +575,40 @@ static void detectAndSetCrashRecovery(
     }
 }
 
-static void handleItermRotation()
+static void rotateVector(float v[3], float rotation[3]) 
 {
-    // rotate old I to the new coordinate system
+    // rotate v around rotation vector rotation
-    const float gyroToAngle = dT * RAD;
+    // rotation in radians, all elements must be small
-    for (int i = FD_ROLL; i <= FD_YAW; i++) {
+    for (int i = 0; i < 3; i++) {
         int i_1 = (i + 1) % 3;
         int i_2 = (i + 2) % 3;
-        float angle = gyro.gyroADCf[i] * gyroToAngle;
+        float newV = v[i_1] + v[i_2] * rotation[i];
-        float newPID_I_i_1 = pidData[i_1].I + pidData[i_2].I * angle;
+        v[i_2] -= v[i_1] * rotation[i];
-        pidData[i_2].I -= pidData[i_1].I * angle;
+        v[i_1] = newV;
-        pidData[i_1].I = newPID_I_i_1;
+    }
+}
+
+static void handleRotations() 
+{
+    if (itermRotation || acGain > 0) {
+        const float gyroToAngle = dT * RAD;
+        float rotationRads[3];
+        for (int i = FD_ROLL; i <= FD_YAW; i++) {
+            rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle;
+        }
+        if (acGain > 0) {
+            rotateVector(axisError, rotationRads);
+        }
+        if (itermRotation) {
+            float v[3];
+            for (int i = 0; i < 3; i++) {
+                v[i] = pidData[i].I;
+            }
+            rotateVector(v, rotationRads );
+            for (int i = 0; i < 3; i++) {
+                pidData[i].I = v[i];
+            }
+        }
     }
 }
 
@@ -685,9 +715,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
     }
 
-    if (itermRotation) {
+    handleRotations();
-        handleItermRotation();
-    }
 
     // ----------PID controller----------
     for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
@@ -714,6 +742,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         }
 #endif // USE_YAW_SPIN_RECOVERY
 
+        // mix in a correction for accrued attitude error
+        float acCorrection = 0;
+        if (acGain > 0) {
+            acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
+            currentPidSetpoint += acCorrection;
+        }   
+        
         // -----calculate error rate
         const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
         float errorRate = currentPidSetpoint - gyroRate; // r - y
@@ -753,6 +788,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
             itermErrorRate = errorRate;
         }
         
+        if (acGain > 0) {
+            axisError[axis] = constrainf(axisError[axis] + (itermErrorRate - acCorrection) * dT, -acErrorLimit, acErrorLimit);
+        }
+        
         const float ITerm = pidData[axis].I;
         const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
         const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);

src/main/flight/pid.h

@@ -132,6 +132,9 @@ typedef struct pidProfile_s {
     uint16_t acro_trainer_lookahead_ms;     // The lookahead window in milliseconds used to reduce overshoot
     uint8_t acro_trainer_debug_axis;        // The axis for which record debugging values are captured 0=roll, 1=pitch
     uint8_t acro_trainer_gain;              // The strength of the limiting. Raising may reduce overshoot but also lead to oscillation around the angle limit
+    uint8_t abs_control_gain;              // How strongly should the absolute accumulated error be corrected for
+    uint8_t abs_control_limit;             // Limit to the correction
+    uint8_t abs_control_error_limit;       // Limit to the accumulated error
 } pidProfile_t;
 
 #ifndef USE_OSD_SLAVE

src/main/interface/settings.c

@@ -793,6 +793,11 @@ const clivalue_t valueTable[] = {
     { "horizon_tilt_effect",        VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0,  250 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) },
     { "horizon_tilt_expert_mode",   VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_expert_mode) },
 
+    { "abs_control_gain",           VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_gain) },
+    { "abs_control_limit",          VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 10, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_limit) },
+    { "abs_control_error_limit",    VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 1, 45 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_error_limit) },
+
+
 // PG_TELEMETRY_CONFIG
 #ifdef USE_TELEMETRY
     { "tlm_inverted",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) },