FixedwingPositionControl: refine / refactor waypoint navigation methods

[tstastny]

Solved Problem

#21358 (comment) (from #21358) reported turn around behavior after passing the land point during a fixed-wing autolanding. Sure enough - the vehicle was indeed hard turning just past the land point. this is dangerous behavior, and should not happen.

Turns out to be a regression made after merging #21635. The intent of #21635 was to safe guard waypoint navigation logic from vehicle runaways once past the target waypoint (caused in rare cases by race conditions with the Navigator module). The solution was to add logic to turn back to the target waypoint once past, with the nominal behavior being that this condition is never entered due to acceptance radius based switching logic triggering in the Navigator before the condition was ever reached.

What was missed, however, is that the navigateWaypoints() method was also used in auto takeoff and landing modes.

Further, both the logged value and input argument to npfg of the instantaneous position setpoint calculated in the navigateWaypoints() method also had an error here when desired_path was set to the vector from vehicle to waypoint (the direct fly-to case), leading to erroneous guidance commands:
https://github.com/PX4/PX4-Autopilot/blob/22e613a24ab142876d69fbd068768f596fbd933d/src/modules/fw_pos_control/FixedwingPositionControl.cpp#L2879C56-L2882

Vector2f unit_path_tangent{desired_path.normalized()};
_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
_closest_point_on_path = waypoint_A + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
_npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, waypoint_A, 0.0f);

The result in the "past the waypoint" case of the logged instantaneous position setpoint can be seen below:

It was additionally found during the investigation that the signed_track_error from NPFG was not getting logged due to a mistaken removal of this internal variable during a refactor.

Solution

Break navigateWaypoints() into navigateLine() and navigateWaypoint().

navigateLine() follows an infinite line defined by either two points on the path, or a point on the path and bearing of the path (two method overloads). This method is used for auto takeoff and landing logic.

navigateWaypoint() specifically handles the case where there is a single waypoint to fly-to. If no higher level state machine makes a switch before the vehicle reaches the waypoint, it will result in "flowering" behavior.

navigateWaypoints() is now only used where the entry and exit behaviors to a line segment described by two waypoints are desired, such as in nominal waypoint navigation. The figure below details each region and the corresponding approach vector that is generated according to the logic in the method.

The methods have also been refined and more explicitly interfaced / documented. Further, the controlAutoPosition() and controlAutoTakeoff() methods have benefited from some clean up, to make things slightly easier to follow.

Function briefs were also noticed out of date, and updated.

Changelog Entry

For release notes:

Bugfix FixedwingPositionControl: Fixes landing regression where the vehicle could turn back towards the landing point after passing it.
Documentation: Should update docs.px4.io/ with waypoint logic description and perhaps the figure included above.

Test coverage

See in a SITL simulation that the waypoint switching behaves in the nominal manner, the logged instantaneous position setpoint corresponds to the correct targets, and when the vehicle is past the land point, it continues to follow the infinite line.

Backport needed?

yes. to v1.14.

[sfuhrer]

Thanks for those fixes, separating it into a dedicated follow line method makes sense to me.
Would be good to get it flight tested before merging.

[KonradRudin]

Hey @tstastny , can you check the iris SITL tests? There was a segfault there, need to check what happened there before we can approve.

[tstastny]

Hey @tstastny , can you check the iris SITL tests? There was a segfault there, need to check what happened there before we can approve.

ok ci failure resolved with a rebase. rover still failing.. but also on main. @KonradRudin ok to go? then ill also make a backport to 1.14

[ryanjAA]

Was this ever flight tested?

Maybe Jakob (can't tag him [@jstrebel] for some reason)

[jstrebel]

Yes, successful tested. Jakob

[ryanjAA]

Great! Thanks @jstrebel