As I mentioned in the description of the pull request, it is a workaround. Because if the drone moves, the heading towards the waypoint will be a different one, the funtion has to be called continuously. I found that the easiest way of doing this without adding such functionality was to have an exception. This way, the exception will call the set_nav_heading_by_waypoint function without interfering with the rest of the block it is in.