CSE reimplementation - take two

[pand5461]

CSE now runs much faster (4 ticks at 200 IPU in my tests), does not require additional lexicon as input. Optional parameters: mu, initial guess for solver, accuracy of convergence. Returns new position, velocity and last guess of X to use on next iteration. Compared to the stock positionat(), difference is equal to displacement per tick. Don't know why it is so. Some assembly required to make it work with the original code, so I add it as a new file.

This must speed up every PEG cycle by 0.2 sec at 500 IPU compared to the original CSE.

[Noiredd]

Thank you @pand5461 for sharing this implementation, and apologies it took so long for me to review and test this.
The new implementation looks good so I'm merging this now. In a future commit I will rename this file and introduce a setting so that users can pick which version of CSE they want to use. If this implementation proves robust at lower IPU settings, it will become default - I will flight test it in the coming weeks.

[Noiredd]

I made a severe blunder when merging this and did not notice the error in line 70, nor the misaligned parameter list (not blaming you of course, @pand5461, you mentioned some engineering would be necessary to integrate this). The fix is already up in commit 1cedc48 though.

[pand5461]

Oh, sorry for line 70. It somehow remained from redesignating (r1, v1, r2, v2) to (r0, v0, r1, v1).