XMega: implement digital servo control