diff --git a/stm-firmware/pid-controller.c b/stm-firmware/pid-controller.c index 20658e4..ac71a68 100644 --- a/stm-firmware/pid-controller.c +++ b/stm-firmware/pid-controller.c @@ -89,7 +89,8 @@ float pid_sample(struct pid_controller *pid, float deviation) } /* Calculate derivative part */ - pid->derivate = pid->k_deriv_t * (deviation - pid->last_in) + pid->k_inv_deriv_t * pid->derivate; + pid->derivate = pid->k_deriv_t * (deviation - pid->last_in) + + pid->k_inv_deriv_t * pid->derivate; output += pid->derivate; output += pid->integral;