From a513b2c675027f9faeec61e4c4fb24630f1fccb3 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 3 Apr 2016 21:49:44 +0200 Subject: [PATCH] [LP-276] Support D Term calculation on measurement only --- flight/libraries/math/pid.c | 16 ++++++++++++---- flight/libraries/math/pid.h | 2 +- flight/modules/Stabilization/innerloop.c | 4 ++-- .../stabilizationsettings.xml | 2 ++ 4 files changed, 17 insertions(+), 7 deletions(-) diff --git a/flight/libraries/math/pid.c b/flight/libraries/math/pid.c index e03e6e97b..6e962c306 100644 --- a/flight/libraries/math/pid.c +++ b/flight/libraries/math/pid.c @@ -76,7 +76,7 @@ float pid_apply(struct pid *pid, const float err, float dT) * This version of apply uses setpoint weighting for the derivative component so the gain * on the gyro derivative can be different than the gain on the setpoint derivative */ -float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT) +float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT, bool meas_based_d_term) { float err = setpoint - measured; @@ -85,9 +85,18 @@ float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f); // Calculate DT1 term, + float diff; + float derr = (-measured); + + if (!meas_based_d_term) { + derr += deriv_gamma * setpoint; + } + + diff = derr - pid->lastErr; + pid->lastErr = derr; + float dterm = 0; - float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr); - pid->lastErr = (deriv_gamma * setpoint - measured); + if (pid->d > 0.0f && dT > 0.0f) { // low pass filter derivative term. below formula is the same as // dterm = (1-alpha)*pid->lastDer + alpha * (...)/dT @@ -95,7 +104,6 @@ float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float dterm = pid->lastDer + dT / (dT + deriv_tau) * ((scaler->d * diff * pid->d / dT) - pid->lastDer); pid->lastDer = dterm; } - return (err * scaler->p * pid->p) + pid->iAccumulator / 1000.0f + dterm; } diff --git a/flight/libraries/math/pid.h b/flight/libraries/math/pid.h index 9b81cebad..b6d9e2ae6 100644 --- a/flight/libraries/math/pid.h +++ b/flight/libraries/math/pid.h @@ -70,7 +70,7 @@ typedef struct pid_scaler_s { // ! Methods to use the pid structures float pid_apply(struct pid *pid, const float err, float dT); -float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT); +float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT, bool meas_based_d_term); void pid_zero(struct pid *pid); void pid_configure(struct pid *pid, float p, float i, float d, float iLim); void pid_configure_derivative(float cutoff, float gamma); diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 3786b62f4..bd2a8f43f 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -290,7 +290,7 @@ static void stabilizationInnerloopTask() StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t] ); pid_scaler scaler = create_pid_scaler(t); - actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT); + actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE); break; case STABILIZATIONSTATUS_INNERLOOP_ACRO: { @@ -305,7 +305,7 @@ static void stabilizationInnerloopTask() pid_scaler ascaler = create_pid_scaler(t); ascaler.i *= boundf(1.0f - (1.5f * fabsf(stickinput[t])), 0.0f, 1.0f); // this prevents Integral from getting too high while controlled manually - float arate = pid_apply_setpoint(&stabSettings.innerPids[t], &ascaler, rate[t], gyro_filtered[t], dT); + float arate = pid_apply_setpoint(&stabSettings.innerPids[t], &ascaler, rate[t], gyro_filtered[t], dT, stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE); float factor = fabsf(stickinput[t]) * stabSettings.acroInsanityFactors[t]; actuatorDesiredAxis[t] = factor * stickinput[t] + (1.0f - factor) * arate; } diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 810fd1afd..b3cddf903 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -48,6 +48,8 @@ + +