From 2955cf93cb019b71ae1a13d2fd4446727543704d Mon Sep 17 00:00:00 2001
From: David Wehr <dawehr@iastate.edu>
Date: Wed, 12 Apr 2017 13:14:22 -0500
Subject: [PATCH] Correctly integrating error in PID block to work with dynamic
sampling time.
---
quad/src/graph_blocks/node_pid.c | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/quad/src/graph_blocks/node_pid.c b/quad/src/graph_blocks/node_pid.c
index e776b7ed2..74ee28ab9 100644
--- a/quad/src/graph_blocks/node_pid.c
+++ b/quad/src/graph_blocks/node_pid.c
@@ -48,7 +48,7 @@ static void pid_computation(void *state, const double* params, const double *inp
if (fabs(params[PID_KI]) <= FLT_EPSILON) {
pid_state->acc_error = 0;
} else {
- pid_state->acc_error += error;
+ pid_state->acc_error += (error * inputs[PID_DT]);
}
if (pid_state->just_reset) {
@@ -60,7 +60,7 @@ static void pid_computation(void *state, const double* params, const double *inp
// Compute each term's contribution
P = params[PID_KP] * error;
- I = params[PID_KI] * pid_state->acc_error * inputs[PID_DT];
+ I = params[PID_KI] * pid_state->acc_error;
// Low-pass filter on derivative
double change_in_value = inputs[PID_CUR_POINT] - pid_state->prev_val;
double term1 = params[PID_ALPHA] * pid_state->last_filtered;
--
GitLab