Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

iDecayV2 #320

Merged
merged 3 commits into from
Jul 16, 2020
Merged

iDecayV2 #320

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
57c903b
iDecayV2
Quick-Flash Jul 14, 2020
e9dcf9a
Merge branch 'master' into iDecayV2
Quick-Flash Jul 16, 2020
e6cfdc7
Update cms_menu_imu.c
nerdCopter Jul 16, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
12 changes: 6 additions & 6 deletions src/main/cms/cms_menu_imu.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@ static uint8_t pidProfileIndex;
static char pidProfileIndexString[] = " p";
static uint8_t feathered_pids;
static uint8_t i_decay;
static uint8_t i_decay_cutoff;
static uint16_t errorBoost;
static uint8_t errorBoostLimit;
static uint16_t errorBoostYaw;
Expand Down Expand Up @@ -132,6 +133,7 @@ static long cmsx_PidAdvancedRead(void)
const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
feathered_pids = pidProfile->feathered_pids;
i_decay = pidProfile->i_decay;
i_decay = pidProfile->i_decay_cutoff;
errorBoost = pidProfile->errorBoost;
errorBoostLimit = pidProfile->errorBoostLimit;
errorBoostYaw = pidProfile->errorBoostYaw;
Expand Down Expand Up @@ -159,6 +161,8 @@ static long cmsx_PidAdvancedWriteback(const OSD_Entry *self)
pidProfile->errorBoostYaw = errorBoostYaw;
pidProfile->errorBoostLimitYaw = errorBoostLimitYaw;
pidProfile->i_decay = i_decay;
pidProfile->i_decay_cutoff = i_decay_cutoff;

pidInitConfig(currentPidProfile);

return 0;
Expand All @@ -176,6 +180,8 @@ static OSD_Entry cmsx_menuPidAdvancedEntries[] =
{ "BOOST LIMIT YAW", OME_UINT8, NULL, &(OSD_UINT8_t){ &errorBoostLimitYaw, 0, 250, 1}, 0 },

{ "I DECAY", OME_UINT8, NULL, &(OSD_UINT8_t){ &i_decay, 1, 10, 1 }, 0 },
{ "I DECAY CUTOFF", OME_UINT8, NULL, &(OSD_UINT8_t){ &i_decay_cutoff, 1, 250, 1 }, 0 },

{ "SAVE&EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVE, 0},
{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }
Expand Down Expand Up @@ -222,12 +228,6 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
pidProfile->pid[i].I = tempPid[i][1];
pidProfile->pid[i].D = tempPid[i][2];
}
pidProfile->feathered_pids = feathered_pids;
nerdCopter marked this conversation as resolved.
Show resolved Hide resolved
pidProfile->errorBoost = errorBoost;
pidProfile->errorBoostLimit = errorBoostLimit;
pidProfile->errorBoostYaw = errorBoostYaw;
pidProfile->errorBoostLimitYaw = errorBoostLimitYaw;
pidProfile->i_decay = i_decay;
pidInitConfig(currentPidProfile);

return 0;
Expand Down
15 changes: 10 additions & 5 deletions src/main/flight/pid.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -148,6 +148,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.setPointDTransition[YAW] = 130,
.feathered_pids = 100,
.i_decay = 4,
.i_decay_cutoff = 200, // value of 0 mimicks old i_decay behaviour
.errorBoost = 15,
.errorBoostYaw = 40,
.errorBoostLimit = 20,
Expand Down Expand Up @@ -689,14 +690,14 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
#endif // USE_YAW_SPIN_RECOVERY

previousPidSetpoint[axis] = currentPidSetpoint;
const float gyroRate = gyro.gyroADCf[axis];

// -----calculate error rate
errorRate = currentPidSetpoint - gyro.gyroADCf[axis]; // r - y
errorRate = currentPidSetpoint - gyroRate; // r - y

// EmuFlight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab)

const float gyroRate = gyro.gyroADCf[axis];
float errorBoostAxis;
float errorLimitAxis;

Expand Down Expand Up @@ -733,12 +734,16 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// -----calculate I component
//float iterm = constrainf(pidData[axis].I + (pidCoefficient[axis].Ki * errorRate) * dynCi, -itermLimit, itermLimit);
float iterm = temporaryIterm[axis];
float iDecayMultiplier = iDecay;
float ITermNew = pidCoefficient[axis].Ki * (boostedErrorRate + errorRate) * dynCi;
if (ITermNew != 0.0f)
{
if (SIGN(iterm) != SIGN(ITermNew))
{
const float newVal = ITermNew * iDecay;
// at low iterm iDecayMultiplier will be 1 and at high iterm it will be equivilant to iDecay
iDecayMultiplier = 1.0f + (iDecay - 1.0f) * constrainf(iterm / pidProfile->i_decay_cutoff, 0.0f, 1.0f);
const float newVal = ITermNew * iDecayMultiplier;

if (fabs(iterm) > fabs(newVal))
{
ITermNew = newVal;
Expand All @@ -758,8 +763,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
{
//filter Kd properly, no setpoint filtering
const float pureError = errorRate - previousError[axis];
const float pureMeasurement = -(gyro.gyroADCf[axis] - previousMeasurement[axis]);
previousMeasurement[axis] = gyro.gyroADCf[axis];
const float pureMeasurement = -(gyroRate - previousMeasurement[axis]);
previousMeasurement[axis] = gyroRate;
previousError[axis] = errorRate;
float dDelta = ((feathered_pids * pureMeasurement) + ((1 - feathered_pids) * pureError)) * pidFrequency; //calculating the dterm determine how much is calculated using measurement vs error
//filter the dterm
Expand Down
1 change: 1 addition & 0 deletions src/main/flight/pid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,7 @@ typedef struct pidProfile_s {
// EmuFlight PID controller parameters
uint8_t feathered_pids; // determine how feathered your pids are
uint8_t i_decay; // i-term decay (increases how quickly iterm shrinks in value)
uint8_t i_decay_cutoff; // iterm values above which i_decay has full effect
uint16_t errorBoost; // the weight of the setpoint boost
uint16_t errorBoostYaw; // the weight of the setpoint boost for yaw
uint8_t errorBoostLimit; // percentage of the error that the emu boost can boost
Expand Down
1 change: 1 addition & 0 deletions src/main/interface/settings.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -866,6 +866,7 @@ const clivalue_t valueTable[] = {

{ "feathered_pids", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, feathered_pids) },
{ "i_decay", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 10 }, PG_PID_PROFILE, offsetof(pidProfile_t, i_decay) },
{ "i_decay_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, i_decay_cutoff) },
{ "emu_boost", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, errorBoost) },
{ "emu_boost_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, errorBoostYaw) },
{ "emu_boost_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, errorBoostLimit) },
Expand Down