diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 36997ebe3..2c1e5ce0a 100644 --- a/src/main/flight/pid.c +++ b/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); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c09c9c606..c121ddf48 100644 --- a/src/main/flight/pid.h +++ b/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 diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index aedddf657..b2253aa7c 100644 --- a/src/main/interface/settings.c +++ b/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) },