Du wirst aus dem Motorprogramm raus das Sonar auch abfragen müssen, oder?
Bau dir mal irgendeine Meldung in die entsprechenden Unterprogramme ein,die du auf die Konsole ausgeben lässt. Dann siehst du, ob die betreffenden Routinen überhaupt aufgerufen werden.