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