From a513b2c675027f9faeec61e4c4fb24630f1fccb3 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 3 Apr 2016 21:49:44 +0200 Subject: [PATCH 1/3] [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 @@ + + From f83c9e2c7379cad8a33bd59d2487fe7433653bef Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 8 Apr 2016 22:18:01 +0200 Subject: [PATCH 2/3] [LP-276] some perf improvements (mainly for f1 targets) (authored by Lalanne Laurent ) --- flight/modules/Stabilization/innerloop.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index bd2a8f43f..629e87c20 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -68,7 +68,7 @@ static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 }; static PiOSDeltatimeConfig timeval; static float speedScaleFactor = 1.0f; static bool frame_is_multirotor; - +static bool measuredDterm_enabled; // Private functions static void stabilizationInnerloopTask(); static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); @@ -98,7 +98,8 @@ void stabilizationInnerloopInit() // schedule dead calls every FAILSAFE_TIMEOUT_MS to have the watchdog cleared PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER); - frame_is_multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); + frame_is_multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); + measuredDterm_enabled = (stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE); } static float get_pid_scale_source_value() @@ -250,6 +251,7 @@ static void stabilizationInnerloopTask() StabilizationStatusOuterLoopGet(&outerLoop); bool allowPiroComp = true; + for (t = 0; t < AXES; t++) { bool reinit = (StabilizationStatusInnerLoopToArray(enabled)[t] != previous_mode[t]); previous_mode[t] = StabilizationStatusInnerLoopToArray(enabled)[t]; @@ -290,7 +292,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, stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE); + actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled); break; case STABILIZATIONSTATUS_INNERLOOP_ACRO: { @@ -305,7 +307,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, stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE); + float arate = pid_apply_setpoint(&stabSettings.innerPids[t], &ascaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled); float factor = fabsf(stickinput[t]) * stabSettings.acroInsanityFactors[t]; actuatorDesiredAxis[t] = factor * stickinput[t] + (1.0f - factor) * arate; } From 8c860591ff75168857e8377441181e64a168facd Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 8 Apr 2016 23:10:48 +0200 Subject: [PATCH 3/3] [LP-276] change default --- shared/uavobjectdefinition/stabilizationsettings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index b3cddf903..8ab21b25c 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -48,7 +48,7 @@ - +