CI: mission WP reached - satisfy based on mavros topics instead of di…

[lamping7]

…stance check

Motivation: Checking distance is nice feedback, but it is not good criteria for a test because of the varying acceptance radii of FW WPs. Which is based on the switch or turn distance: where _L1_distance = _L1_ratio * ground_speed and _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period.

Also added: It now logs the distance to the WP initially and every 10 seconds (which may be nice to see for future, possibly longer, missions).

Locally tested: console log - https://pastebin.com/nfTJapu0

[lamping7]

@dagar One of the intermittent bugs showed it self during this CI run. With this PR, you can now see that the reached flag got set, but the mission (seq_current) isn't advancing to the next item, causing it to loiter endlessly. Hopefully this is a clue for you.

Take a look at the logs:

• rosunit-mission_test-vtol_old_3.txt
• http://ci.px4.io:8080/blue/organizations/jenkins/Firmware/detail/PR-8879/1/pipeline
• https://logs.px4.io/plot_app?log=435631a2-9512-4733-9e92-18508a8d3437

[dagar]

Thanks!