3 axis auto stabilized platform

The resolution is pretty low using the 8 bit counters and that may explain why the servos are not smooth.

Any reason you can't use a simple delay instead of the timers for driving the servos similar to reply #33 above? You have at least 14 milliseconds to do any calculations between pulsing the servos and because your data is changing every two frames you can spread this over 28 milliseconds if you need to.