Thanks for the work, I found the time today and merged your angle and position direction code with some minor tweaks.
It seems to be working pretty well at the moment and we merged it into the public version of the firmware at the same time:
This is the community version of the merged and tweaked part of the code:
demo: 2A new feature: shows the angle, direction, position of the target. - YouTube