Fix bug in sonar servo code