What is your mission like? is the first waypoint some distance away and the craft is trying to move laterally before it’s even got off the ground?
I try to set the first waypoint at some height and roughly straight above where you expect to launch. That way the craft will fly straight up with minimal sideways travel.
If you’re using spline waypoints set the first waypoint (at least) as a standard waypoint - then you know exactly where in space that will be. Probably even set your last waypoint as a standard waypoint too. Spline waypoints give a “3D” path so the craft may fly in a curve higher or lower (or further off track) than what you expect in order to reach the next spline waypoint.
Check and set WP_NAVALT_MIN,3 (3 meters or as much as 5) so the craft will rise to that altitude before trying to move in any other direction.