FlightTaskAuto: add check for xy mission progress

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp

@@ -221,6 +221,8 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_updateAvoidanceWaypoints();
 	}
 
+	_checkAvoidanceProgress();
+
 	return true;
 }
 
@@ -304,6 +306,37 @@ void FlightTaskAuto::_updateAvoidanceWaypoints()
 	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true;
 }
 
+void FlightTaskAuto::_checkAvoidanceProgress()
+{
+	position_controller_status_s pos_control_status = {};
+	pos_control_status.timestamp = hrt_absolute_time();
+
+	// vector from previous triplet to current target
+	Vector2f prev_to_target = Vector2f(&(_triplet_target - _triplet_prev_wp)(0));
+	// vector from previous triplet to the vehicle projected position on the line previous-target triplet
+	Vector2f prev_to_closest_pt = _closest_pt - Vector2f(&(_triplet_prev_wp)(0));
+	// fraction of the previous-tagerget line that has been flown
+	const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length();
+
+	if (prev_curr_travelled > 1.0f) {
+		// if the vehicle projected position on the line previous-target is past the target waypoint,
+		// increase the target acceptance radius such that navigator will update the triplets
+		pos_control_status.acceptance_radius = Vector2f(&(_triplet_target - _position)(0)).length() + 0.5f;
+	}
+
+	// do not check for waypoints yaw acceptance in navigator
+	pos_control_status.yaw_acceptance = NAN;
+
+	if (_pub_pos_control_status == nullptr) {
+		_pub_pos_control_status = orb_advertise(ORB_ID(position_controller_status), &pos_control_status);
+
+	} else {
+		orb_publish(ORB_ID(position_controller_status), _pub_pos_control_status, &pos_control_status);
+
+	}
+
+}
+
 bool FlightTaskAuto::_isFinite(const position_setpoint_s &sp)
 {
 	return (PX4_ISFINITE(sp.lat) && PX4_ISFINITE(sp.lon) && PX4_ISFINITE(sp.alt));