ITerm rotation (#5469)

* ITerm rotation

* address requested changes

* now counting up

* scale errors according to Ki while rotating

* iterm_rotation profile setting

* revert to non scaled version, style related fixes

* Triggering a CI build.

src/main/flight/pid.c

@@ -51,6 +51,7 @@
 #include "sensors/gyro.h"
 #include "sensors/acceleration.h"
 
+
 FAST_RAM uint32_t targetPidLooptime;
 static FAST_RAM bool pidStabilisationEnabled;
 
@@ -132,7 +133,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .crash_limit_yaw = 200,
         .itermLimit = 150,
         .throttle_boost = 0,
-        .throttle_boost_cutoff = 15
+        .throttle_boost_cutoff = 15,
+        .iterm_rotation = false                 
     );
 }
 
@@ -290,6 +292,7 @@ static FAST_RAM float crashLimitYaw;
 static FAST_RAM float itermLimit;
 FAST_RAM float throttleBoost;
 pt1Filter_t throttleLpf;
+static FAST_RAM bool itermRotation;
 
 void pidInitConfig(const pidProfile_t *pidProfile)
 {
@@ -298,6 +301,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
         Ki[axis] = ITERM_SCALE * pidProfile->pid[axis].I;
         Kd[axis] = DTERM_SCALE * pidProfile->pid[axis].D;
     }
+
     dtermSetpointWeight = pidProfile->dtermSetpointWeight / 127.0f;
     if (pidProfile->setpointRelaxRatio == 0) {
         relaxFactor = 0;
@@ -324,6 +328,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     crashLimitYaw = pidProfile->crash_limit_yaw;
     itermLimit = pidProfile->itermLimit;
     throttleBoost = pidProfile->throttle_boost * 0.1f;
+    itermRotation = pidProfile->iterm_rotation == 1;
 }
 
 void pidInit(const pidProfile_t *pidProfile)
@@ -456,6 +461,19 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
     // Dynamic d component, enable 2-DOF PID controller only for rate mode
     const float dynCd = flightModeFlags ? 0.0f : dtermSetpointWeight;
 
+    if (itermRotation) {
+        // rotate old I to the new coordinate system
+        const float gyroToAngle = deltaT * RAD;
+        for (int i = FD_ROLL; i <= FD_YAW; i++) {
+            int i_1 = (i + 1) % 3;
+            int i_2 = (i + 2) % 3;
+            float angle = gyro.gyroADCf[i] * gyroToAngle;
+            float newPID_I_i_1 = axisPID_I[i_1] + axisPID_I[i_2] * angle;
+            axisPID_I[i_2] -= axisPID_I[i_1] * angle;
+            axisPID_I[i_1] = newPID_I_i_1;
+        }
+    }
+
     // ----------PID controller----------
     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
         float currentPidSetpoint = getSetpointRate(axis);

src/main/flight/pid.h

@@ -109,7 +109,7 @@ typedef struct pidProfile_s {
     uint16_t dterm_lowpass2_hz;                // Extra PT1 Filter on D in hz
     uint8_t throttle_boost;                 // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle
     uint8_t throttle_boost_cutoff;          // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
-    
+    uint8_t  iterm_rotation;                    // rotates iterm to translate world errors to local coordinate system
 } pidProfile_t;
 
 #ifndef USE_OSD_SLAVE

src/main/interface/settings.c

@@ -709,6 +709,7 @@ const clivalue_t valueTable[] = {
     { "crash_limit_yaw",            VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_limit_yaw) },
     { "crash_recovery",             VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRASH_RECOVERY }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery) },
 
+    { "iterm_rotation",             VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_rotation) },
     { "iterm_windup",               VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
     { "iterm_limit",                VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) },
     { "pidsum_limit",               VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },