Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

ITerm rotation (#5469)

Browse source 
* 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.
This commit is contained in:
joelucid 2018-03-25 23:50:21 +02:00 • committed by Michael Keller
parent d6af4c2d6b
commit ee8daac41d
3 changed files with 21 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

20
src/main/flight/pid.c
View file

@ -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);