Modded drone_teleop to store and modify a current reference position