Reimplemented the PID loop according to industry conventions and with D in the feedback path