How to make quadcopter yaw in a new flight mode

I have created a new flight mode called homeonbeacon. This mode will enable a quadcopter to use bluetooth direction finding to home on a bluetooth beacon.

To this point, I have successfully added the new flight mode into commander.cpp, I have successfully built a new process to interface with the bluetooth direction finder and send azimuth information to the new flight task that I have successfully built and integrated to receive that azimuth data.

I can command this flight mode using a switch on the RC transmitter, and because I have added the flight mode to Qgroundcontrol, I can also select it through the flight mode menu there.

What I have not yet done successfully is get the drone to yaw upon command, then travel to the beacon, and I am looking for help to get this last step working.

I am using FlightTaskOrbit as an example of how it should be done, but it is not helping me with getting the yaw working correctly. Presently I am only using the mavlink simulator (make px4_sitl jmavsim) and won’t try to fly it until I have this sorted.

At this time, when I switch from homeonbeacon mode to hold mode or to land mode or return mode, the drone promptly yaws roughly pi/6 radians, then yaws back. This suggests to me that my yaw commands are being received but some sort of update is being missed, and only seen when I change modes out of homeonbeacon.

Can anyone here guide me a bit?