navigator: set acceptance radius even for IDLE waypoint

[RomanBapst]

This fixes the heading drift people have been seeing right after a takeoff.
The problem was that the auto flighttask was creating a yaw setpoint in the direction of the velocity setpoint. There is code implemented which locks the yaw setpoint if the vehicle is within the acceptance radius of the target, however, the navigator published an IDLE setpoint with an acceptance radius of 0 and this is what was used by the flight task.

Still not sure why the acceptance radius was not updated once the takeoff setpoint was sent.

[RomanBapst]

@MaEtUgR FYI

[RomanBapst]

Before this PR:

After this PR:

[dagar]

This seems kind of weird and out of place. What if instead we used Navigator::reset_triplets() in that section of logic to set appropriate defaults in all the position setpoints, and then set it to position_setpoint_s::SETPOINT_TYPE_IDLE?

[MaEtUgR]

@RomanBapst Please mention the original issue when you make a pr for a fix. It was this one: #13675

Your findings explain why the yaw setpoint is not exactly the yaw estimate to begin with. But it doesn't explain why the yaw setpoint is quickly unfiltered jumping to zero and otherwise gets heavily filtered even though it's a reset. At least the jump to zero is a concern to me.

Is there an easy way to reproduce the issue in SITL such that it happens every takeoff? That would help debugging the other issues.

[RomanBapst]

@MaEtUgR Have a look at the plot above with the fix. I don't see any yaw clamping anymore.
I investigated why the yaw setpoint was moving around in arbitrary directions in the first place. That was obviously causes by the flighttask doing it's velocity vector heading calculations which was in turn caused by the acceptance radius being publishes as 0 by the navigator.

If you still see any clamping issues let me know, I do not see them.
Any by the way, the problem was easily reproducible in SITL. The only reason why you did not see it 100% of the time was because sometimes by coincidence the yaw setpoint was actually close to the current yaw during takeoff and so the effect was not really visible.

[RomanBapst]

@dagar I followed your suggestion, please have a look.

[bresch]

Good catch!

[MaEtUgR]

Sorry that I didn't have time to look at all the details. You should probably get the fix in before we figure out all the questions I added in my comment.

[MaEtUgR]

Suggested change