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));

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

@@ -43,6 +43,7 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/position_setpoint.h>
 #include <uORB/topics/home_position.h>
+#include <uORB/topics/position_controller_status.h>
 #include <lib/ecl/geo/geo.h>
 
 /**
@@ -130,10 +131,13 @@ private:
 	WeatherVane *_ext_yaw_handler =
 		nullptr;	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 
+	orb_advert_t _pub_pos_control_status = nullptr; /**< Publisher for the position controller status */
+
 	bool _evaluateTriplets(); /**< Checks and sets triplets. */
 	bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */
 	bool _evaluateGlobalReference(); /**< Check is global reference is available. */
 	float _getVelocityFromAngle(const float angle); /**< Computes the speed at target depending on angle. */
 	State _getCurrentState(); /**< Computes the current vehicle state based on the vehicle position and navigator triplets. */
 	void _set_heading_from_mode(); /**< @see  MPC_YAW_MODE */
+	void _checkAvoidanceProgress();
 };