Ist Dir der Timer 1 nicht zu schade für sowas ? Hast Du Timer 2 noch frei ? wenn ja mach folgendes :
Im Register TCCR2 folgende Bits setzen: CS21, WGM21,COM 20
Damit stellst Du den Prescaller auf 8,schaltest Du CTC Modus am Timer 2 ein und Toggelst Du OC2 Pin bei jedem Compare Match.
Im Register OCR2 schreibst Du 25 rein.

Wenn ich nichts vergessen habe, erzeugst Du am Pin OC2 40 kHz, ganz in Hardware ohne ein Takt für die Erzeugung zu verlieren .
Ich hoffe, daß Du den OC2 noch frei hast ?

Gruß Sebastian