Skip to content

Commit

Permalink
Initial dynamic PID implementation
Browse files Browse the repository at this point in the history 
New Defaults and some rework in dynamic PID

Cli Fixes

Copy / Paste Protection

Change Stick threshold

Remove differentiator

Change Default PIDs
  • Loading branch information
@borisbstyle
borisbstyle committed May 3, 2016
1 parent ad756bc commit a4456ce
Show file tree
Hide file tree
Showing 7 changed files with 48 additions and 70 deletions.
4 changes: 2 additions & 2 deletions src/main/blackbox/blackbox.c
View file
Expand Up @@ -1280,8 +1280,8 @@ static bool blackboxWriteSysinfo()
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count);
break;
case 36:
blackboxPrintfHeaderLine("dterm_differentiator:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_differentiator);
blackboxPrintfHeaderLine("dynamic_pterm:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pterm);
break;
case 37:
blackboxPrintfHeaderLine("rollPitchItermResetRate:%d",
Expand Down
17 changes: 9 additions & 8 deletions src/main/config/config.c
View file
Expand Up @@ -148,10 +148,10 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->pidController = 1;

pidProfile->P8[ROLL] = 45;
pidProfile->I8[ROLL] = 30;
pidProfile->I8[ROLL] = 35;
pidProfile->D8[ROLL] = 18;
pidProfile->P8[PITCH] = 45;
pidProfile->I8[PITCH] = 30;
pidProfile->I8[PITCH] = 35;
pidProfile->D8[PITCH] = 18;
pidProfile->P8[YAW] = 90;
pidProfile->I8[YAW] = 40;
Expand All @@ -177,11 +177,12 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->D8[PIDVEL] = 75;

pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->yaw_lpf_hz = 100;
pidProfile->yaw_lpf_hz = 70;
pidProfile->rollPitchItermResetRate = 200;
pidProfile->rollPitchItermResetAlways = 0;
pidProfile->yawItermResetRate = 50;
pidProfile->dterm_lpf_hz = 70; // filtering ON by default
pidProfile->dynamic_pterm = 1;

pidProfile->H_sensitivity = 75; // TODO - Cleanup during next EEPROM changes

Expand Down Expand Up @@ -307,9 +308,9 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->rcExpo8 = 60;
controlRateConfig->thrMid8 = 50;
controlRateConfig->thrExpo8 = 0;
controlRateConfig->dynThrPID = 0;
controlRateConfig->dynThrPID = 20;
controlRateConfig->rcYawExpo8 = 20;
controlRateConfig->tpa_breakpoint = 1500;
controlRateConfig->tpa_breakpoint = 1650;

for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
controlRateConfig->rates[axis] = 50;
Expand Down Expand Up @@ -401,10 +402,10 @@ static void resetConf(void)
masterConfig.dcm_kp = 2500; // 1.0 * 10000
masterConfig.dcm_ki = 0; // 0.003 * 10000
masterConfig.gyro_lpf = 0; // 256HZ default
masterConfig.gyro_sync_denom = 8;
masterConfig.gyro_soft_lpf_hz = 80;
masterConfig.gyro_sync_denom = 4;
masterConfig.gyro_soft_lpf_hz = 100;

masterConfig.pid_process_denom = 1;
masterConfig.pid_process_denom = 2;

masterConfig.debug_mode = 0;

Expand Down
64 changes: 27 additions & 37 deletions src/main/flight/pid.c
View file
Expand Up @@ -93,6 +93,16 @@ float calculateExpoPlus(int axis, rxConfig_t *rxConfig) {
return propFactor;
}

uint16_t getDynamicKp(int axis, pidProfile_t *pidProfile) {
uint16_t dynamicKp;

uint32_t dynamicFactor = constrain(ABS(rcCommand[axis] << 8) / DYNAMIC_PTERM_STICK_THRESHOLD, 0, 1 << 7);

dynamicKp = ((pidProfile->P8[axis] << 8) + (pidProfile->P8[axis] * dynamicFactor)) >> 8;

return dynamicKp;
}

void pidResetErrorAngle(void)
{
errorAngleI[ROLL] = 0;
Expand Down Expand Up @@ -136,7 +146,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
{
float RateError, AngleRate, gyroRate;
float ITerm,PTerm,DTerm;
static float lastRate[3][PID_LAST_RATE_COUNT];
static float lastRate[3];
float delta;
int axis;
float horizonLevelStrength = 1;
Expand Down Expand Up @@ -200,11 +210,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRate - gyroRate;

uint16_t kP = (pidProfile->dynamic_pterm) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];

// -----calculate P component
if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
PTerm = (luxPTermScale * pidProfile->P8[axis] * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig));
PTerm = (luxPTermScale * kP * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig));
} else {
PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor;
PTerm = luxPTermScale * RateError * kP * tpaFactor;
}

// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
Expand All @@ -220,7 +232,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
}

if (axis == YAW) {
if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW);
if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD);
}

if (antiWindupProtection || motorLimitReached) {
Expand All @@ -238,20 +250,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
DTerm = 0;
} else {
if (pidProfile->dterm_differentiator) {
// Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters)
// by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
// N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4
delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8;
for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) {
lastRate[axis][i] = lastRate[axis][i-1];
}
} else {
// When DTerm low pass filter disabled apply moving average to reduce noise
delta = -(gyroRate - lastRate[axis][0]);
}

lastRate[axis][0] = gyroRate;
delta = -(gyroRate - lastRate[axis]);
lastRate[axis] = gyroRate;

// Divide delta by targetLooptime to get differential (ie dr/dt)
delta *= (1.0f / getdT());
Expand Down Expand Up @@ -288,7 +288,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co

int axis;
int32_t PTerm, ITerm, DTerm, delta;
static int32_t lastRate[3][PID_LAST_RATE_COUNT];
static int32_t lastRate[3];
int32_t AngleRateTmp, RateError, gyroRate;

int8_t horizonLevelStrength = 100;
Expand Down Expand Up @@ -342,11 +342,13 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
gyroRate = gyroADC[axis] / 4;
RateError = AngleRateTmp - gyroRate;

uint16_t kP = (pidProfile->dynamic_pterm) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];

// -----calculate P component
if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
PTerm = (pidProfile->P8[axis] * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7;
PTerm = (kP * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7;
} else {
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7;
}

// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
Expand All @@ -366,11 +368,11 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);

if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) {
if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD);
if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -ITERM_RESET_THRESHOLD << 13, (int32_t) + ITERM_RESET_THRESHOLD << 13);
}

if (axis == YAW) {
if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW);
if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -ITERM_RESET_THRESHOLD << 13, (int32_t) + ITERM_RESET_THRESHOLD << 13);
}

if (antiWindupProtection || motorLimitReached) {
Expand All @@ -386,20 +388,8 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
DTerm = 0;
} else {
if (pidProfile->dterm_differentiator) {
// Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters)
// by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
// N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4
delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8;
for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) {
lastRate[axis][i] = lastRate[axis][i-1];
}
} else {
// When DTerm low pass filter disabled apply moving average to reduce noise
delta = -(gyroRate - lastRate[axis][0]);
}

lastRate[axis][0] = gyroRate;
delta = -(gyroRate - lastRate[axis]);
lastRate[axis] = gyroRate;

// Divide delta by targetLooptime to get differential (ie dr/dt)
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5;
Expand Down
7 changes: 3 additions & 4 deletions src/main/flight/pid.h
View file
Expand Up @@ -22,9 +22,8 @@
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_MAX 400 // Maximum value for yaw P limiter

#define PID_LAST_RATE_COUNT 7
#define ITERM_RESET_THRESHOLD 20
#define ITERM_RESET_THRESHOLD_YAW 10
#define ITERM_RESET_THRESHOLD 15
#define DYNAMIC_PTERM_STICK_THRESHOLD 400

typedef enum {
PIDROLL,
Expand Down Expand Up @@ -81,7 +80,7 @@ typedef struct pidProfile_s {
uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates
uint16_t yaw_p_limit;
uint8_t dterm_average_count; // Configurable delta count for dterm
uint8_t dterm_differentiator;
uint8_t dynamic_pterm;

#ifdef GTUNE
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune
Expand Down
10 changes: 5 additions & 5 deletions src/main/io/serial_cli.c
View file
Expand Up @@ -653,7 +653,7 @@ const clivalue_t valueTable[] = {

{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_lowpass_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } },
{ "gyro_lowpass", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },
Expand Down Expand Up @@ -721,12 +721,12 @@ const clivalue_t valueTable[] = {

{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } },
{ "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dterm_differentiator", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_differentiator, .config.lookup = { TABLE_OFF_ON } },
{ "iterm_always_reset", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } },
{ "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dynamic_pterm", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pterm, .config.lookup = { TABLE_OFF_ON } },
{ "iterm_always_reset", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } },
{ "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } },
{ "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } },
{ "yaw_lowpass_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } },

{ "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } },
Expand Down
2 changes: 1 addition & 1 deletion src/main/io/serial_msp.c
View file
Expand Up @@ -152,7 +152,7 @@ void setGyroSamplingSpeed(uint16_t looptime) {
masterConfig.pid_process_denom = 1;
if (currentProfile->pidProfile.pidController == 2) masterConfig.pid_process_denom = 2;
if (looptime < 250) {
masterConfig.pid_process_denom = 3;
masterConfig.pid_process_denom = 4;
} else if (looptime < 375) {
if (currentProfile->pidProfile.pidController == 2) {
masterConfig.pid_process_denom = 3;
Expand Down
14 changes: 1 addition & 13 deletions src/main/mw.c
View file
Expand Up @@ -770,17 +770,6 @@ uint8_t setPidUpdateCountDown(void) {
}
}

// Check for oneshot125 protection. With fast looptimes oneshot125 pulse duration gets more near the pid looptime
bool shouldUpdateMotorsAfterPIDLoop(void) {
if (targetPidLooptime > 375 ) {
return true;
} else if ((masterConfig.use_multiShot || masterConfig.use_oneshot42) && feature(FEATURE_ONESHOT125)) {
return true;
} else {
return false;
}
}

// Function for loop trigger
void taskMainPidLoopCheck(void) {
static uint32_t previousTime;
Expand All @@ -801,7 +790,6 @@ void taskMainPidLoopCheck(void) {
static uint8_t pidUpdateCountdown;

if (runTaskMainSubprocesses) {
if (!shouldUpdateMotorsAfterPIDLoop()) taskMotorUpdate();
subTasksMainPidLoop();
runTaskMainSubprocesses = false;
}
Expand All @@ -813,7 +801,7 @@ void taskMainPidLoopCheck(void) {
} else {
pidUpdateCountdown = setPidUpdateCountDown();
taskMainPidLoop();
if (shouldUpdateMotorsAfterPIDLoop()) taskMotorUpdate();
taskMotorUpdate();
runTaskMainSubprocesses = true;
}

Expand Down

0 comments on commit a4456ce

Please sign in to comment.