Put PID control in udpateTwist, made it optional, and added it only when the quadrotor is flying/hovering