Index: PIDControllers.c
===================================================================
diff -u -raa6bdd3bfc3bd8bc1a56670c2bdadc32c99b33c3 -r68e4c6462f8345be55fa35f5b21fca3e266350aa
--- PIDControllers.c	(.../PIDControllers.c)	(revision aa6bdd3bfc3bd8bc1a56670c2bdadc32c99b33c3)
+++ PIDControllers.c	(.../PIDControllers.c)	(revision 68e4c6462f8345be55fa35f5b21fca3e266350aa)
@@ -244,8 +244,11 @@
         // Add d term to control signal
         if ( controller->Kd != 0 )
         {
-                controller->controlSignal += ( controller->Kd * ( controller->errorSignal - controller->errorPrevious ) );
-
+                // Wait for previous error to be set before adding d term
+                if ( controller->errorPrevious != 0 )
+                {
+                    controller->controlSignal += ( controller->Kd * ( controller->errorSignal - controller->errorPrevious ) );
+                }
                 controller->errorPrevious = controller->errorSignal;
         }