Path Planning and Collision Avoidance Algorithms for Small RPAS