Dadurch das Kommentare und Code teils nicht zusammenpassen ist der Code schwer zu verstehen. Er hilft einem auch kaum weiter. Es wird genau einer der PWM Modes benutzt die das ICR register als obere Grenze für den Timer nutzen, also die ISP Funktion gerade ausschaltet.

Wenn man nur 1 Senvo hat, könnte man eine den Servo an OC1B hängen und mit MODE15 arbeiten (fast PWM mit OCR1A als TOP wert). Die Frequenzmessung wird dadurch aber schon etwas komplizierter.

Eine Alternative wäre es das PWM Signal nicht ganz in Hardware zuerzeugen, sondern Teilweise in Software. Timer 1 kann dann durchlaufen und dann jeweils ein Interrupt ausgelöst werden wenn die nächste Änderung ansteht. Der Code sollte sehr ähnlich sein, wie man ihn nutzt um sehr viel Servos (z.B. 10) anzusteuern. Bei einem Servo könnte es sogar noch als Quasi Hardware PWM gehen.
Wenn die taktfrequenz günstig liegt, könnte es eventuell auch gehen, den timer bis 2^16 laufen zu lassen und dann eine etwas von 20 ms abweichende Periode hinzunehmen.