Ich hab auch das Problem, das ich eine Frequenz mit dem ATMEGA 16 erzeugen muß.
Meine Lösung wäre, den Timer 1 im CTC Mode zu betreiben und den OC1A Pin damit zu toggeln.
Dabei sollte am OC1A Pin eine symmetrische Rechteckspannung mit einstellbarer ( OCR1A Register ) möglich sein.

Die Frequenzerzeugung würde dabei komplett in Hardware laufen.
Die Ein und Ausschalterei der Frequenz müsste dabei über die Konfigurationsregister des Timers 1 erfolgen.

Die Formel im Datenblatt lautet (F-TAKT)/(2*N*(1+OCR1A))=F-OUT.
Bitte selber im Datenblatt nachschauen, hab das jetzt aus dem Kopf gemacht.

Durch eine geeigneten Vorteiler Faktor- es sollten möglichst hohe OCR1A Werte entstehen - sollten sich die 40kHz ziemlich genau einstellen lassen.

Das mit dem Hochpaß vor dem Ultraschallsensor würde ich so unterschreiben.
Eine weitere Möglichkeit wäre, die Toggle Routine per Software zu machen und dabei 2 Ports gegenphasig anzusteuern.
Ich kenn jetzt die Impedanz der Ultraschallkapseln nicht, aber wenn die 10...20mA des Controllers reichen, wär das eine gute Methode zur Leistungssteigerung.
Die Abschaltung des Tones wär dann auch ganz einfach, man setzt einfach beide Ports nach 0 und lässt die Routine diese auch nicht mehr ohne Nutzereingriff einschalten ( Ein Flag, das in der Toggle Routine abgefragt wird ).