Improve planning repeatability

[jkflying]

Use the longest A* path weighted by cost, since we don't plan all of the way to the goal.

Before, the path chosen could have been any of the green cells, right up to the point of finding the red cell.

(play around with it here)

Now, it will find roughly the longest path until it actually reaches the end.

Happy for comments on what exactly we can do for a quick win here to improve the choice mechanism for the A* when it only plans partially.

[Jaeyoung-Lim]

I think this is great!

An approach I like personally on dealing with partial planning is using intermediate goals on a receding horizon. By selecting the intermediate planner smartly, we can also make the global navigation behavior fairly smart if we add global heuristics on selecting the intermediate goal such as the Next Best View Planner NBVP+goal cost

[jkflying]

SITL test before:
https://logs.px4.io/plot_app?log=323a24ef-8827-4e5e-b6e7-ed24836f3316

After:
https://logs.px4.io/plot_app?log=497302be-9c69-407c-b7e1-dd64dae3eb58

There's definitely an improvement, especially in the occasional odd big 'jump' that doesn't happen anymore.

[LorenzMeier]

Is this something that helps short-time?

[jkflying]

IMO yes, it needs real-vehicle testing and some more time in SITL though, since it changes the core A* path choice.

[baumanta]

I flight tested today, no regression found. Also this PR seems to improve roll and pitch rate
master

PR