diff --git a/Makefile b/Makefile
index 68da7fd..57328c9 100644
--- a/Makefile
+++ b/Makefile
@@ -7,7 +7,7 @@ APP = simpleskycam
 OBJECTS := $(OBJECTS) gui.oo main.oo \
 		   video.oo videoframe.oo \
 		   videodev.oo videodev-v4l2.oo videodev-dumpfile.oo \
-		   convert.oo filter.oo detect.oo histogram.oo \
+		   convert.oo filter.oo detect.oo histogram.oo pid.oo \
 		   json.oo configuration.oo ser.oo dng.oo debayer.oo
 DISTNAME=simpleskycam-$(VERSION)
 DEPENDFILE=.depend
diff --git a/pid.cc b/pid.cc
new file mode 100644
index 0000000..13a1f62
--- /dev/null
+++ b/pid.cc
@@ -0,0 +1,66 @@
+/*
+  PID regulator.
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <sys/time.h>
+#include "pid.h"
+
+PID::PID(double min, double max, double kp, double kd, double ki) {
+  Min = min;
+  Max = max;
+  Kp = kp;
+  Kd = kd;
+  Ki = ki;
+  Integral = 0;
+  PreviousError = 0;
+  PreviousTime = getTime();
+}
+
+PID::~PID() {
+}
+
+int64_t PID::getTime(void) {
+
+  struct timeval current_time;
+  int64_t t;
+  
+  gettimeofday (&current_time, NULL);
+  t = current_time.tv_sec * 1000000L + current_time.tv_usec;
+  return t;
+}
+
+double PID::Update(double target, double value, int64_t time) {
+  double error, out, dt;
+  double P, I, D;
+  double Derivative;
+
+  // error
+  error = target - value;
+
+  // proportional part
+  P = Kp * error;
+
+  // integral part
+  dt = time - PreviousTime;
+  Integral += error * dt;
+  I = Ki * Integral;
+
+  // derivative part
+  Derivative = (error - PreviousError) / dt;
+  D = Kd * Derivative;
+
+  // calculate output of regulator
+  out = P + I + D;
+  
+  // Min/Max
+  if (out > Max) out = Max;
+  if (out < Min) out = Min;
+
+  // save previous error value and time
+  PreviousError = error;
+  PreviousTime = time;
+
+  return out;
+}
diff --git a/pid.h b/pid.h
new file mode 100644
index 0000000..f10048d
--- /dev/null
+++ b/pid.h
@@ -0,0 +1,23 @@
+#ifndef _PID_H_
+#define _PID_H_
+
+class PID {
+
+ private:
+  double Min, Max;
+  double Kp; // proportional factor
+  double Ki; // integral factor
+  double Kd; // derivative factor
+  double Integral;
+  double PreviousError;
+  int64_t PreviousTime;
+
+ public:
+  PID(double min, double max, double kp, double kd, double ki);
+  ~PID();
+
+  double Update(double target, double value, int64_t time);
+  static int64_t getTime(void);
+};
+
+#endif