diff --git a/quad/src/graph_blocks/node_pid.c b/quad/src/graph_blocks/node_pid.c index e776b7ed28e9295c9f30be9adb104ae3e9598647..74ee28ab93f7817bdd9e59c61136b6b67d03c525 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;