modify sonar3.cpp