Tweeked the codes abit to make it work better.

still having issue on the first connecting of the drone.