s=15 Kp=0.28 Ki=0.01 Kd=4 t=1.25 m=1000 g=1/m p=0 goto1+:Approach>0
r=:RFD e=r-s d=e-p i+=e/(d-g) i/=2 o=Kp*e+Ki*i+Kd*d p=e r*=r<m
z/=:Approach*r*(d*d>g+e*e>1) :FcuForward=o/t :FcuBackward=-o*t goto2
:Approach=0 :FcuForward=0 :FcuBackward=0 goto1

// PID approach uses a proportional-integal-derivative controller to
// output FCU values appropriate to the distance and current speed.
// It has several tunables:
// s (setpoint)      the distance we want to get to
// Kp (proportional) how much we factor in our current distance
// Ki (integral)     factor of how long we've been at our distance
// Kd (derivative)   factor of prediction of where we'll be in future
// t (thrust diff)   account for disparity in forward/rev thrust power
// Credit to Whitestrake