Flight plan not taking straight path to waypoint

[RobbieNesmith]

I have noticed that when I add a NavigateToWaypointCommand that moves the drone horizontally and down, it will do the horizontal and vertical components separately. I would like it to linearly move to the next waypoint instead of doing this. Is that possible? Is there a setting I’m missing on the NavigateToWaypointCommand?

I have tried setting the acceptance radius lower and adding a ChangeSpeedCommand with 4 m/s (max vertical speed) but neither had any effect.

Here’s an unanswered post about the ChangeSpeedCommand not working, so I may be doing that wrong or it might just not work:

Set speed for waypoints in flight plan

How can i set speed for waypoints in flight plan? I tried to use ARMAVLINK_MissionItemUtils_CreateMavlinkChangeSpeedMissionItem before created a waypoint but it did not work. Thank you so much!

I was able to reproduce this behavior with these waypoints which should move down and west then back up and east:

var lastKnownLocaiton = gpsRef?.get()?.lastKnownLocation
val altitude = altimeterRef?.get()?.groundRelativeAltitude
val waypoint1 = NavigateToWaypointCommand(lastKnownLocation.latitude, lastKnownLocation.longitude - 0.0004, altitude - 10.0, 0.0, 1.0, 2.0)
val waypoint2 = NavigateToWaypointCommand(lastKnownLocation.latitude, lastKnownLocation.longitude, altitude, 0.0, 1.0, 2.0)

Here is the plot in the sphinx monitor showing that it only does this when moving down:

[RobbieNesmith]

I ended up using a different method of automating the drone path (GuidedPilotingItf) but before switching I did realize that the FlightPlanPilotingItf would only fly directly to a waypoint if I first added a waypoint at the drone’s current location. Is this expected behavior that I missed in the documentation or is this a bug?

Also due to the unpredictable behavior I have only tested this in the simulator, not with a real drone.