How to move autonomously with a custom flight mode?

Hello everyone, after following the custom flight mode tutorial (Adding a New Flight Mode to Copter — Dev documentation), I wanted to create a simple flight mode capable of moving forward using either the AC_PosControl or the AC_WPNav libraries, but have been unable to do so, as no matter what I do, the copter either accelerates in a random direction, or spirals around the start position. I’m using SITL with Gazebo.

Does anyone know how to fix this issue or provide some working flight mode code I could study?