Jante,
No idea what was causing the peaks, why don't you try the code here including my new servo library -
I could use a friendly tester.
I would suggest testing outputs instead of the measured signal - thats the hard part getting highly consistent output pulses - the new servo library helps with this.
As this is now off topic I will start a new topic on the library in a day or two when I have a chance to document it a little more, but Jante, you should be able to find your way around it and have a go at testing in the meantime - if you can find the time.
Duane B