Sub: use renamed AC_WPNav::get_yaw_cd and check is active

ArduSub/mode_guided.cpp

@@ -826,8 +826,7 @@ float ModeGuided::get_auto_heading()
     default:
         // point towards next waypoint.
         // we don't use wp_bearing because we don't want the vehicle to turn too much during flight
-        return sub.wp_nav.get_yaw();
-        break;
+        return sub.wp_nav.is_active() ? sub.wp_nav.get_yaw_cd() : sub.pos_control.get_yaw_cd();
     }
 }
 // guided_limit_set - set guided timeout and movement limits