Ich hab erst kürzlich so ein Programmteil in ASM für C gemacht.
Wenn ich heute nach Hause komme kann ich dir den Quellcode mal zuschicken wenn Du das möchtest. Der ist aber für CodeVision AVR!

Im Prinzip läuft dabei der Timer1 frei.
Die Servoimpulse werden auf einen Interrupteingang des AVR gelegt.
Am Anfang steht die Interruptauslösung auf "steigende Flanke".
Im Interrupt 0 wird dann der aktuelle Zählerwert in eine Variable eingelesen und die Interrupteinstellung auf fallende Flanke geändert (MCUCR).
Bei der fallenden Flanke wird der Zählerwert des Timers 1 erneut ausgelesen und der Startwert davon abgezogen und die Interrupteinstellung wieder auf steigende Flanke für den nächsten Impuls gesetzt. Du kannst nach einer fertigen Messung noch ein Flag setzen (Bit Variable) das ein neuer Servoimpuls erkannt wurde (Zur Weiterverarbeitung im Hauptprogramm).
Dadurch das der Timer 1 frei läuft können alle Interrupteingänge + der ICP Pin für die Servoimpulsauswertung verwendet werden. Du kannst somit bis zu 4 Servokanäle überwachen (ATMEGA 16) ohne dabei den Prozessor übermäßig zu belasten.

Als Teilerfaktor für den Timer 1 würd ich 8 verwenden bei einem 8MHz Quarz, dann werden deine Servoimpulse im 1uS Bereich aufgelöst. Ein durchschnittlicher Servoimpuls (1,5ms) wird somit als 1500 dargestellt.
Der gesamte Servoweg 1ms bis 2ms hat somit 1000 Schritte und sollte somit auch für anspruchsvollere Aufgaben geeignet sein.

Wenns nicht reicht - Taktfrequenz erhöhen (16 MHz) oder Teilerfaktor des Timers 1 auf 1 setzen (würd ich bei 4MHz Taktfrequenz machen).