Added functions for controlling the copter to go to a desired position and check if it arrived there (according to a specifiable threshold value)