diff --git a/flight/modules/PathFollower/pidcontrolne.cpp b/flight/modules/PathFollower/pidcontrolne.cpp
index 6a8968f24..5b4a61f11 100644
--- a/flight/modules/PathFollower/pidcontrolne.cpp
+++ b/flight/modules/PathFollower/pidcontrolne.cpp
@@ -7,7 +7,8 @@
  * @{
  *
  * @file       PIDControlNE.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
+ * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
+ *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  * @brief      Executes PID control loops for NE directions
  *
  * @see        The GNU Public License (GPL) Version 3
@@ -79,26 +80,38 @@ void PIDControlNE::Activate()
 
 void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)
 {
-    // pid_configure(&PID, kp, ki, kd, ilimit);
-    float Ti = kp / ki;
-    float Td = kd / kp;
-    float Tt = (Ti + Td) / 2.0f;
-    float kt = 1.0f / Tt;
-    float u0 = 0.0f;
-    float N  = 10.0f;
-    float Tf = Td / N;
+    float Td; // Derivative time constant
+    float Ti; // Integral time constant
+    float kt; // Feedback gain for integral windup avoidance
+    float N = 10.0f; // N is the factor used to determine the
+                     // time constant for derivative filtering
+                     // Why 10? Maybe should be configurable?
+    float Tf; // Low pass filter time constant for derivative filtering
 
-    if (ki < 1e-6f) {
-        // Avoid Ti being infinite
-        Ti = 1e6f;
-        // Tt antiwindup time constant - we don't need antiwindup with no I term
-        Tt = 1e6f;
-        kt = 0.0f;
+    float u0 = 0.0f;
+    
+    // Define Td, handling zero kp term (for I or ID controller)
+    if (kp < 1e-6f) {
+        Td = 1e6f;
+    } else {
+        Td = kd / kp;
     }
 
+    // Define Ti, Tt and kt, handling zero ki term (for P or PD controller)
+    if (ki < 1e-6f) { // Avoid Ti being infinite
+        kt = 0.0f;
+    } else {
+        Ti = kp / ki;
+        kt = 1.0f / Ti;
+    }
+
+    // Define Tf, according to controller type
     if (kd < 1e-6f) {
-        // PI Controller
-        Tf = Ti / N;
+        // PI Controller or P Controller
+        Tf = 0; // Originally this: (Ti / N) - which creates a D term from
+                // the integral time constant!
+    } else {
+        Tf = Td / N;
     }
 
     if (beta > 1.0f) {
@@ -107,6 +120,8 @@ void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, fl
         beta = 0.4f;
     }
 
+
+
     pid2_configure(&PIDvel[0], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
     pid2_configure(&PIDvel[1], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
     deltaTime    = dT;