Finalized SFO2 Laser Rangefinder code