[Bug] GotoControl: yaw getting stuck at -180 while crossing from -180 to 180

[evePurna]

Describe the bug

Description: While using goto waypoints, while crossing from -180 to 180, the goto trajectory get stuck at -180 and the drone never converges to the target yaw.

Occurrence Frequency: Every time the drone tries to cross -180 to 180 using GotoControl

Detailed Description:

For my project, I’ve been using goto waypoints to navigate the drone where I noticed this problem of yaw stuck. I’ve attached a plot and a log file where after takeoff, I provide a goto yaw target of 135 degrees. To achieve this, drones tries to rotate anticlockwise but gets stuck at -180 degrees.

On further research I’ve noticed the issue is in the max limits defined in HeadingSmoothing.cpp file in line 38. The limits are defined from -pi to pi. While updating the setpoint using “HeadingSmoothing::update” function, we initially convert the setpoint to face in the direction of minimum yaw motion. For eg. our drone is at -170 yaw, and the target is +170, the function first converts the target to -190 to make sure the drone rotates anticlockwise to take the minimum path. Now while we are updating the setpoint using “VelocitySmoothing::updateDurations” function, the first thing it does is constrains the target to -180 to 180 and thus our -190 setpoint converts to -180. This makes the drone stuck at -180 and never converge to the target.

To Reproduce

1. Takeoff the drone
2. Manually point the drone at -135 yaw (just an example)
3. Put drone in offboard mode or any auto mode.
4. Provide goto waypoint commnad to yaw 135 degrees

(Above provided is just an example, the problem arises in similar conditions where the drone has to cross from -pi to pi to achieve goto yaw target)

Expected behavior

The drone will never reach target yaw (i.e. 135) and will get stuck at -180 degrees.

Screenshot / Media

No response

Flight Log

Following is the log where above suggested test to reproduce the bug was done.

https://review.px4.io/plot_app?log=57ba7935-ac38-4da5-9d1e-97ea7826c93a

Software Version

1.15.0 dev

Flight controller

ark_fmu-v6x

Vehicle type

Multicopter

How are the different components wired up (including port information)

No response

Additional context

Suggested solution:
Define max limits -2pi to 2pi in HeadingSmoothing.cpp file in line 38.

Results:
After building the suggested update and testing the same scenario, the problem seems to be solved. I’ve also attached a plot showing the yaw target now converges to the goto target provided.

Log after suggested fix:
https://review.px4.io/plot_app?log=d0794e7a-825f-46a5-a97c-59b71f1f55df

[bresch]

@evePurna Thanks for reporting, I was able to reproduce the issue in an unit test and fixed it in #23665