navigator: add yaw_acceptance getter to incorporate feedback from position

controller. The yaw acceptance is defined by the mission item. If the pos
control sets it to NAN, then the yaw at a waypoint is ignored.

src/modules/navigator/mission_block.cpp

@@ -322,7 +322,7 @@ MissionBlock::is_mission_item_reached()
 
 		if ((_navigator->get_vstatus()->is_rotary_wing
 		     || (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading))
-		    && PX4_ISFINITE(_mission_item.yaw)) {
+		    && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))) {
 
 			/* check course if defined only for rotary wing except takeoff */
 			float cog = _navigator->get_vstatus()->is_rotary_wing ? _navigator->get_global_position()->yaw : atan2f(

src/modules/navigator/navigator.h

@@ -248,6 +248,16 @@ public:
 	 */
 	float		get_acceptance_radius(float mission_item_radius);
 
+	/**
+	 * Get the yaw acceptance given the current mission item
+	 *
+	 * @param mission_item_yaw the yaw to use in case the controller-derived radius is finite
+	 *
+	 * @return the yaw at which the next waypoint should be used or if the yaw at a waypoint
+	 * should be ignored
+	 */
+	float 		get_yaw_acceptance(float mission_item_yaw);
+
 	orb_advert_t	*get_mavlink_log_pub() { return &_mavlink_log_pub; }
 
 	void		increment_mission_instance_count() { _mission_result.instance_count++; }

src/modules/navigator/navigator_main.cpp

@@ -946,6 +946,20 @@ Navigator::get_acceptance_radius(float mission_item_radius)
 	return radius;
 }
 
+float
+Navigator::get_yaw_acceptance(float mission_item_yaw)
+{
+	float yaw = mission_item_yaw;
+
+	const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
+
+	if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && !PX4_ISFINITE(pos_ctrl_status.yaw_acceptance)) {
+		yaw = pos_ctrl_status.yaw_acceptance;
+	}
+
+	return yaw;
+}
+
 void
 Navigator::load_fence_from_file(const char *filename)
 {