From 8662239978cdd778d01b575e05a1dd64db32f1c0 Mon Sep 17 00:00:00 2001
From: David Wehr <dawehr@iastate.edu>
Date: Thu, 6 Apr 2017 09:08:04 -0500
Subject: [PATCH] PID block now uses the derivative of input value rather than
the error.
---
quad/src/graph_blocks/node_pid.c | 17 ++++++++++++-----
1 file changed, 12 insertions(+), 5 deletions(-)
diff --git a/quad/src/graph_blocks/node_pid.c b/quad/src/graph_blocks/node_pid.c
index df6b61110..eb166b3c9 100644
--- a/quad/src/graph_blocks/node_pid.c
+++ b/quad/src/graph_blocks/node_pid.c
@@ -6,9 +6,10 @@ static double FLT_EPSILON = 0.0001;
struct pid_node_state {
- double prev_error; // Previous error
+ double prev_val; // Previous input value
double acc_error; // Accumulated error
double last_filtered; // Last output from the filtered derivative
+ int just_reset; // Set to 1 after a call to reset
};
// The generic PID diagram. This function takes in pid inputs (CUR_POINT and SETOINT) and calculates the output "pid_correction"
@@ -50,21 +51,27 @@ static void pid_computation(void *state, const double* params, const double *inp
pid_state->acc_error += error;
}
+ if (pid_state->just_reset) {
+ // On first call after a reset, set the previous value to
+ // the current value to prevent a spike in derivative
+ pid_state->prev_val = inputs[PID_CUR_POINT];
+ pid_state->just_reset = 0;
+ }
// Compute each term's contribution
P = params[PID_KP] * error;
I = params[PID_KI] * pid_state->acc_error * inputs[PID_DT];
// Low-pass filter on derivative
- double change_in_error = error - pid_state->prev_error;
+ double change_in_value = inputs[PID_CUR_POINT] - pid_state->prev_val;
double term1 = params[PID_ALPHA] * pid_state->last_filtered;
- double derivative = change_in_error / inputs[PID_DT];
+ double derivative = change_in_value / inputs[PID_DT];
if (inputs[PID_DT] == 0) { // Divide by zero check
derivative = 0;
}
double term2 = params[PID_KD] * (1.0f - params[PID_ALPHA]) * derivative;
D = term1 + term2;
pid_state->last_filtered = D; // Store filtered value for next filter iteration
- pid_state->prev_error = error; // Store the current error into the state
+ pid_state->prev_val = inputs[PID_CUR_POINT]; // Store the current error into the state
outputs[PID_CORRECTION] = P + I + D; // Store the computed correction
}
@@ -74,8 +81,8 @@ static void pid_computation(void *state, const double* params, const double *inp
static void reset_pid(void *state) {
struct pid_node_state* pid_state = (struct pid_node_state*)state;
pid_state->acc_error = 0;
- pid_state->prev_error = 0;
pid_state->last_filtered = 0;
+ pid_state->just_reset = 1;
}
--
GitLab