Servo control with ATMega128 and 4017 decade counter

Since moving to FreeRTOS Renoster's control system lost the use of Timer1 thus the ability to control 4 servos using PWM. After some scouring on the net using a 4017 decade counter seem to be the solution to get that functionality back. AUD0.90 later the counter was purchased and all that was left was code.

The first attempt was based on Larry Barello's great paper RC Pulse Generation on an AVR using a decade counter, unfortunately the implementation got messed up while trying to change it to use Timer 3. After some more scouring a pseudo solution presented itself in the Swordfish compiler forums.

A bit of hacking here and there and this is the end product... The ability to control up to 10 servo's using Timer3.

uint16_t ServoPositions[9]; 
uint16_t *CurrentServo; 
 
uint16_t CalcCounterValue(int Time) 
{ 
   return (uint16_t)(65535 - (Time * 16) + 1); 
} 
 
void InitServo() 
{ 
   DDRE = _BV(PE3); 
   DDRA = _BV(PA1); 
 
   for (int i = 0; i < 9; i++) 
      ServoPositions[i] = 1500; 
   CurrentServo = &ServoPositions[0]; 
 
   PORTA |= _BV(PA1); 
   TCCR3B = (1 << CS30); 
   TCNT3 = CalcCounterValue(*CurrentServo++); 
   ETIMSK = (1 << TOIE3); 
   PORTA &= ~_BV(PA1);  
} 
 
ISR(TIMER3_OVF_vect) 
{ 
   PORTE |= _BV(PE3); 
   TCNT3 = CalcCounterValue(*CurrentServo++); 
   PORTE &= ~_BV(PE3); 
 
   if (CurrentServo > &ServoPositions[8]) 
      CurrentServo = &ServoPositions[0]; 
}

Schematic:

Below is an image of output 0 of the 4017 connected to a scope.