Algorithm for ARDrone