"Thank you for this optimisation. I will test it this evening.

I would have some question about using this algorithm.
I plan to use it with an atmel board (xmega-a1 or xmega-b1) and atavrsbin1 9dof board (http://www.atmel.com/Images/doc8354.pdf)…"