From aa1b46f85a2188ca056ad19290e831c779c8b675 Mon Sep 17 00:00:00 2001
From: Martina Rivizzigno <martina@rivizzigno.it>
Date: Mon, 4 Mar 2019 08:31:35 +0100
Subject: [PATCH] ObstacleAvoidance library: save current waypoint in global
 variable to check progress

---
 .../FlightTasks/tasks/Utility/ObstacleAvoidance.cpp | 13 ++++++-------
 .../FlightTasks/tasks/Utility/ObstacleAvoidance.hpp |  2 ++
 2 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
index e49898ffc8..6675b9e30b 100644
--- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
@@ -119,6 +119,7 @@ void ObstacleAvoidance::updateAvoidanceWaypoints(const Vector3f &curr_wp, const
 {
 	_desired_waypoint.timestamp = hrt_absolute_time();
 	_desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
+	_curr_wp = curr_wp;
 
 	curr_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position);
 	Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity);
@@ -149,21 +150,19 @@ void ObstacleAvoidance::updateAvoidanceSetpoints(const Vector3f &pos_sp, const V
 }
 
 void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp,
-		float target_acceptance_radius,
-		const Vector2f &closest_pt)
+		float target_acceptance_radius, const Vector2f &closest_pt)
 {
 	position_controller_status_s pos_control_status = {};
 	pos_control_status.timestamp = hrt_absolute_time();
-	Vector3f curr_wp = _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position;
 
 	// vector from previous triplet to current target
-	Vector2f prev_to_target = Vector2f(curr_wp - prev_wp);
+	Vector2f prev_to_target = Vector2f(_curr_wp - prev_wp);
 	// vector from previous triplet to the vehicle projected position on the line previous-target triplet
-	Vector2f prev_to_closest_pt = closest_pt - Vector2f(prev_wp);
+	Vector2f prev_to_closest_pt = closest_pt - Vector2f(prev_wp); prev_to_closest_pt.print();
 	// fraction of the previous-tagerget line that has been flown
 	const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length();
 
-	Vector2f pos_to_target = Vector2f(curr_wp - pos);
+	Vector2f pos_to_target = Vector2f(_curr_wp - pos);
 
 	if (prev_curr_travelled > 1.0f) {
 		// if the vehicle projected position on the line previous-target is past the target waypoint,
@@ -171,7 +170,7 @@ void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector
 		pos_control_status.acceptance_radius = pos_to_target.length() + 0.5f;
 	}
 
-	const float pos_to_target_z = fabsf(curr_wp(2) - pos(2));
+	const float pos_to_target_z = fabsf(_curr_wp(2) - pos(2));
 
 	if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > NAV_MC_ALT_RAD.get()) {
 		// vehicle above or below the target waypoint
diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
index e4bccbf012..a279e5be72 100644
--- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
@@ -82,6 +82,8 @@ private:
 	orb_advert_t _pub_pos_control_status{nullptr};
 	orb_advert_t _pub_vehicle_command{nullptr};
 
+	matrix::Vector3f _curr_wp = {};
+
 	void _publish_avoidance_desired_waypoint();
 	void _publish_vehicle_cmd_do_loiter();