diff --git a/stm-firmware/include/reflow-controller/pid-controller.h b/stm-firmware/include/reflow-controller/pid-controller.h
index 1e64741..c2914cc 100644
--- a/stm-firmware/include/reflow-controller/pid-controller.h
+++ b/stm-firmware/include/reflow-controller/pid-controller.h
@@ -25,15 +25,19 @@ struct pid_controller {
 	float k_deriv;
 	float k_int;
 	float k_p;
+	float k_int_t;
+	float k_deriv_t;
 	float output_sat_max;
 	float output_sat_min;
 	float integral_max;
+	float sample_period;
 	volatile float control_output;
 	volatile float last_in;
 	volatile float integral;
+	volatile float derivate;
 };
 
-void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p, float output_sat_min, float output_sat_max, float integral_max);
+void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p, float output_sat_min, float output_sat_max, float integral_max, float sample_period);
 
 void pid_zero(struct pid_controller *pid);
 
diff --git a/stm-firmware/main.c b/stm-firmware/main.c
index 0ed1135..e56db8a 100644
--- a/stm-firmware/main.c
+++ b/stm-firmware/main.c
@@ -221,7 +221,7 @@ int main()
 	shell_handle = shell_init(write_shell_callback);
 	shell_print_motd(shell_handle);
 
-	pid_init(&pid, 0.1, 0.1, 4.0, 0.0, 100.0, 40.0);
+	pid_init(&pid, 0.1, 0.1, 4.0, 0.0, 100.0, 40.0, 0.25);
 	pid_zero(&pid);
 
 	while (1) {
diff --git a/stm-firmware/pid-controller.c b/stm-firmware/pid-controller.c
index ef97235..c73d591 100644
--- a/stm-firmware/pid-controller.c
+++ b/stm-firmware/pid-controller.c
@@ -20,18 +20,22 @@
 
 #include <reflow-controller/pid-controller.h>
 
-void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p, float output_sat_min, float output_sat_max, float integral_max)
+void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p, float output_sat_min, float output_sat_max, float integral_max, float sample_period)
 {
 	if (!pid)
 		return;
 
+	pid->sample_period = sample_period;
 	pid->k_p = k_p;
 	pid->k_int = k_int;
 	pid->k_deriv = k_deriv;
+	pid->k_int_t = pid->k_int * pid->sample_period / 2.0f;
+	pid->k_deriv_t = pid->k_deriv * 2.0f / pid->sample_period;
 	pid->output_sat_max = output_sat_max;
 	pid->output_sat_min = output_sat_min;
-	pid->control_output = 0;
 	pid->integral_max = integral_max;
+
+	pid_zero(pid);
 }
 
 void pid_zero(struct pid_controller *pid)
@@ -43,12 +47,13 @@ void pid_zero(struct pid_controller *pid)
 
 static void calculate_integral(struct pid_controller *pid, float deviation)
 {
-	pid->integral += deviation * pid->k_int;
+	pid->integral = pid->integral + pid->k_int_t * (deviation + pid->last_in);
 
+	/* Saturate integral term to spoecified maximum */
 	if (pid->integral > pid->integral_max) {
 		pid->integral = pid->integral_max;
-	} else if (pid->integral < -pid->integral_max) {
-		pid->integral = -pid->integral_max;
+	} else if (pid->integral < -pid->integral_max){
+		pid->integral = - pid->integral_max;
 	}
 }
 
@@ -66,17 +71,20 @@ float pid_sample(struct pid_controller *pid, float deviation)
 		calculate_integral(pid, deviation);
 	}
 
+	/* Calculate derivative part */
+	pid->derivate = pid->k_deriv_t * (deviation - pid->last_in) - pid->derivate;
+
+	output += pid->derivate;
 	output += pid->integral;
-	output -= (deviation - pid->last_in) * pid->k_deriv;
 
-	pid->last_in = deviation;
-
-	if (output > pid->output_sat_max)
-		output = pid->output_sat_max;
+	/* Saturate output */
 	if (output < pid->output_sat_min)
 		output = pid->output_sat_min;
+	else if (output > pid->output_sat_max)
+		output = pid->output_sat_max;
 
 	pid->control_output = output;
+	pid->last_in = deviation;
 
 	return output;
 }