PID block now uses the derivative of input value rather than the error.

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;
 }
 
 

[bbartels]

I might not be recalling correctly, but I was under the impression that step-like changes in PID_SETPOINT gave rise to the spikes, not PID_CUR_POINT, so maybe we don't need this?

[dawehr]

This is for when the PID block was just connected. Upon the first iteration, it does not know the previous input, so we cannot properly take the derivative. When dealing with error, we could simply set the last error to be 0, but we can't set the last input to be zero, because if we are currently at 1 meter, then upon the 2nd iteration, that would show up as a 1 meter change in position.