Sub: 4.1.2 "go here" in guided mode has a strange wobble

[clydemcqueen]

Issue details

There is strange yaw behavior using guided mode in sub 4.1.2 and QGC 4.3.0: the sub rotates to face the direction of travel, then travels to the target, then rotates back / forth, ending with a rotation that is +30 degrees. I've attached a video.

QGC is sending 0 for param4 in the MAV_CMD_NAV_WAYPOINT command inside the MISSION_ITEM message; per the spec this should rotate the sub to yaw of 0 (north), not toward the direction of travel. (And there should not be a wobble.)

https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_WAYPOINT

In sub 4.5 beta1 the sub does not rotate in the direction of travel, and there is no wobble. However, 4.5 beta 1 does not quite honor the MAVLink spec, in that the sub does not rotate to yaw=0 (north).

Related to #14783 (DO_REPOSITION is not supported in sub).

You must use QGC 4.3.0 to see this; in prior versions of QGC "go here" did not work with sub.

Version

• sub 4.1.2 (rotates toward direction of travel, wobbles at the end)
• sub 4.5 beta 1 (does not rotate to param4 value)

Platform
[ ] All
[ ] AntennaTracker
[ ] Copter
[ ] Plane
[ ] Rover
[ x ] Submarine

Airframe type

All

Hardware type

All

Logs

Trivial to repro in SITL
Screencast from 04-29-2024 01:39:45 PM.webm