Auto Mode Yaw Not Facing Next Waypoint

Hi all, I want to get help on a problem I have been facing since updating from copter 3.3. I have a TBS Discovery with a gimbal that I like to use for mapping and aerial footage. When mapping I fly the quadcopter in auto mode with a grid flight plan. With arducopter 3.3, the quad yaws to face the next way point pretty much before it even starts heading toward that waypoint. Now with arducopter 3.5 the quad takes the entire length between waypoints to yaw around to the right direction. So instead of flying straight down the line between waypoints, with yaw facing forward, it starts one waypoint facing backward, then only faces forward when it reaches the next waypoint. Changing distance between waypoints changes nothing; it still takes the whole distance to yaw around. Using splines between grid lines also makes no difference. A mission like this is unusable for mapping because the pictures are in different orientations. For now I have to yaw manually which is a burden.

I think this may be some bug in the firmware. Looking at the attached log, the actual yaw followings the desired yaw as intended, so it seems the desired yaw is not correct for the settings I have. Flashing back to 3.3 fixes everything, but the quad is less stable. I have tried WP_NAV_BEHAVIOR of 1,2, and 3 with no change. I have also made sure my yaw deadband is enough, my RC4 (yaw) is centered at 1500, and I am not commanding yaw during auto missions. Vibration levels look good and the quad flies fine is every other mode. Only other problem I see is that somethings the quad loses a few m altitude and has to fight to recover before continuing the mission. But I think that may be from thermal sink or wind gusts.

Auto Mode Log

So it looks like I was able to solve my problem. I’m not sure if it was a rogue parameter, corrupt firmware, or something else, but I erased the firmware and started fresh, and now the copter yaws correctly during missions. It does not seem as smooth as it was in 3.3; there is a brief delay before the copter yaws around. It may be because it waits to leave the WP radius before yawing. In 3.3 the copter yawed to face the next WP on arrival to a WP. Now it seems to do it upon leaving. Regardless, it is working well enough in my opinion to get my mapping working again.

WP_YAW_BEHAVIOR is the answer