Improve PWM jitter.
This modification is a bit troubling as it makes interrupt routine busyloop for about 16us, this however is just about 1-2% (depending on channel values) of CPU time so I see this can be tolerated. With this change the servo jitter is virtually nonexistent.