Added, compiled and tested the range finder, seems to be working as expected