Drone and UAV applications show an increasing demand for terrain-following and obstacle avoidance algorithms. In this project, we developed a terrain-following and obstacle avoidance algorithm for a UAV using a series of rangefinder sensors. The algorithm is able to follow a terrain profile while avoiding obstacles in real-time. The controller used is a Ardupilot based controller