Ich hab das letzthin bei einem ATMEGA 328PB so gemacht.
Ein 16Bit Timer wurde mit FAST PWM so eingestellt, das er beim erreichen vom ICP Wert das TCNT Register auf 0 gesetzt hat.
Der Wert für das OCR1A Register bestimmt dann die länge des Servoimpulses.
Der Ausgang des OCR muss dabei im DDR Register auch als Ausgang definiert werden.
Das ganze läuft rein In Hardware und deshalb super stabil. Andere Servoimpulslängen werden lediglich durch Beschreiben des OCRxA Registers eingestellt.

Wenn's wegen der Ports nicht in Hardware geht, kann man es auch über einen Comparematch Interrupt lösen und darin den entsprechenden Ausgang umschalten.
Im Interrupt wird dann der neue Wert für den nächsten Comparematch belegt - Also für den nächsten Impulsbeginn bzw. für die nächste Impulspause.
Sind aber schon mehrere andere Interrupts aktiv kann das zu einem Jitter bei der Impulsausgabe führen.

Als Prescaler würde ich dabei 8 wählen, dann bekommt man bei 16MHz eine Auflösung von 0,5µs und das ist für ein Servo schon sehr gut.