Handle additional case with NPFG waypoint navigation handling

[Jaeyoung-Lim]

Describe problem solved by this pull request
The NPFG waypoint handling logic did not handle the case when the vehicle was further along the line segment and passed the current waypoint.

This resulted in the vehicle continuing along the line segment defined by the previous(Waypoint A) and current waypoint(Waypoint B) even though the vehicle has passed the current waypoint (Waypoint B).

This can happen when the current waypoint altitude difference is big, so that the vehicle switches to a loiter setpoint to climb, and then switches back to a considering it as a position setpoint only after when it has passed the current waypoint.

My guess is that this was hidden since we never had position waypoints that were consecutively on different altitudes.

Describe your solution
This PR adds additional case handling of the navigate waypoints in NPFG

Test data / coverage
Tested in SITL Gazebo

make px4_sitl gazebo_believer

• Before PR
  Flight Log: https://logs.px4.io/plot_app?log=b38bb0ad-52ea-448d-b18f-425c6e8b24c6
  

• After PR
  Flight Log: https://logs.px4.io/plot_app?log=7eba14da-5c0e-4d35-9770-da4c31254e9b
  

Additional context

• Fixes Vehicle deviates from waypoint and does not recover #52

[acfloria]

Is this a proper fix or more like a hack that it does not happen again?

[Jaeyoung-Lim]

Is this a proper fix or more like a hack that it does not happen again?

@acfloria My intention was a proper fix (The case was simply not handled in the waypoint logic), but if there are other possible ways to improve the logic please let me know!

[acfloria]

How does L1 actually handle this?

I think such a fix might be fine but I kind of don't like the hard switch of behavior. Maybe @tstastny has an idea how to fix it since this will also need to be fixed in the upstream PR.

[Jaeyoung-Lim]

I think such a fix might be fine but I kind of don't like the hard switch of behavior.

I fully agree @acfloria I believe the case in L1 is this one:

ethzasl_fw_px4/src/lib/l1/ECL_L1_Pos_Controller.cpp

Lines 150 to 158 in 0c2bf74

	} else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) {
	/*
	* Extension, fly back to waypoint.
	*
	* This corner case is possible if the system was following
	* the AB line from waypoint A to waypoint B, then is
	* switched to manual mode (or otherwise misses the waypoint)
	* and behind the waypoint continues to follow the AB line.
	*/

and seems like the same corner case with the issue this PR is addressing

[acfloria]

I see. I would probably also do something like this. Using a cone instead of checking if it is in front of the waypoint, because then it would react a bit earlier. Also it is closer to L1, which would make the upstream merge a bit easier.

[tstastny]

@Jaeyoung-Lim - good catch. However, maybe two comments.. 1) if you want to do a point tracking conversion to handle this case similar to L1 logic, I wouldn't recommend defining tracking to the opposite direction line as you have it now. Rather use the guideToPoint() method. 2) I made a fix for this moving the handling of this case to the mission block in PX4#18810 (comment) - this would be my preferred fix. Let me know what you think.

@acfloria I'm a bit skeptical on the 100 deg cone as L1 implements it.. as this even limits the range of where the aircraft will start turning back towards the waypoint. Having the switch logic occur at either in the acceptance radius or passed the terminal waypoint simplifies all this.

[Jaeyoung-Lim]

@tstastny Thanks for the review and comments!

1. if you want to do a point tracking conversion to handle this case similar to L1 logic, I wouldn't recommend defining tracking to the opposite direction line as you have it now. Rather use the guideToPoint() method.

This feels like a much more sensible thing to do 😄 I will update this PR once upstream is merged so that there are no additional changes that require a backport

[tstastny]

this is addressed in PX4#18996 and PX4#18810

[stale]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.