Have you looked at these: http://discuss.ardupilot.org/t/basic-obstacle-stop-system-build-log-and-firmware/14454 http://ardupilot.org/dev/docs/code-overview-object-avoidance.html