Flyaway after loiter to altitude

[kibidev]

Describe the bug

While flying a VTOL mission in simulator with WPs with large altitude difference close together (to provoke loiter to altitude), I experienced that the drone after completing loiter to altitude (WP 4) selected the WP after the next (WP 6, but currently unsure if WP 5 was briefly selected in between) but continued straight east, but adjusting the altitude to that given in WP 5.

To Reproduce

I have not been able to reproduce yet, but the steps I did when got the error is given below.

Steps to reproduce the behavior:

1. Started simulator make px4_sitl gazebo_standard_vtol
2. Uploaded mission bumpy.zip
3. Started mission
4. Error after WP 4, as described/shown in screenshot

Expected behavior

The drone should have gone to WP 5 and loitered to corrected altitude after WP 4.

Log Files and Screenshots

https://logs.px4.io/plot_app?log=d5fb6ec1-4393-406c-904d-77450f2c6b4f

Drone (please complete the following information):

• Simulator, gazebo_standard_vtol

Additional context

Used latest main branch version:
0ae296bfe21add6276f5c28a97884059fb317999
Build/run in px4io/px4-dev-simulation-focal docker image.

After the screenshot was taken, I selected WP 6 and continued mission from there. No unexpected behavior after this.

[Jaeyoung-Lim]

@kibidev Behaviorwise this looks like a similar problem that #18996 fixed

[kibidev]

Got an other data point:

It actually changes WPs here, but is flying at wrong position in a line, when it should be loitering.

Edit:
Log: https://logs.px4.io/plot_app?log=1fc36355-0265-4f81-a060-bdb2341135fd
Plan: bumpy2.zip

Edit 2:
This new plan seem to be easier to reproduce, I suspect the key is to get the WPs very close together. It still does not happen every time, though.

[kibidev]

@kibidev Behaviorwise this looks like a similar problem that #18996 fixed

Thanks @Jaeyoung-Lim, this does indeed seem relevant.
I will investigate a bit more, and try to pinpoint which versions are impacted.

[kibidev]

I have not been able to reproduce this in 1.13.2, so I believe some change after this is the cause, but I can not be 100 % sure.

[kibidev]

Using bisect, I have found the commit/PR that first causes this behavior:
#21168

I have not studied it in details, but I suspect that there is some other older code that is the actual error, and this just stopped hiding the problem.

[kibidev]

There might be something with this function:

PX4-Autopilot/src/modules/fw_path_navigation/FixedwingPositionControl.cpp

Lines 2821 to 2856 in 0afda91

	void FixedwingPositionControl::navigateWaypoints(const Vector2f &waypoint_A, const Vector2f &waypoint_B,
	const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel)
	{
	// similar to logic found in ECL_L1_Pos_Controller method of same name
	// BUT no arbitrary max approach angle, approach entirely determined by generated
	// bearing vectors
	Vector2f vector_A_to_B = waypoint_B - waypoint_A;
	Vector2f vector_A_to_vehicle = vehicle_pos - waypoint_A;
	if (vector_A_to_B.norm() < FLT_EPSILON) {
	// the waypoints are on top of each other and should be considered as a
	// single waypoint, fly directly to it
	if (vector_A_to_vehicle.norm() > FLT_EPSILON) {
	vector_A_to_B = -vector_A_to_vehicle;
	} else {
	// Fly to a point and on it. Stay to the current control. Do not update the npfg library to get last output.
	return;
	}
	} else if ((vector_A_to_B.dot(vector_A_to_vehicle) < -FLT_EPSILON)) {
	// we are in front of waypoint A, fly directly to it until we are within switch distance.
	if (vector_A_to_vehicle.norm() > _npfg.switchDistance(500.0f)) {
	vector_A_to_B = -vector_A_to_vehicle;
	}
	}
	// track the line segment
	Vector2f unit_path_tangent{vector_A_to_B.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);
	} // navigateWaypoints

Line 2843 seems strange to me, as it should intuitive be a positive dot product that indicates that vehicle is in front of A. Am I missing something? However, just flipping just makes the drone circle around a point on the line between A and B.

Using loiter radius instead of FLT_EPSILON on 2831 does seem to work, but I guess that is mostly for this special case, and not really the right way to do it.

[DronecodeBot]

This issue has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-maintainers-call-may-23-2023/32245/1

[sfuhrer]

Thanks for the report @kibidev , sadly I only saw it now. I was able to reproduce and now look for a solution with the hints you already posted.
(PS feel free to ping me or another maintainer on issues/PRs if you don't get a reply fast, the real issues like this one are otherwise easy to miss with all the other noise we get)