PID implementation reorganization, initial sim of translational velocity. Altitude adjustment