From e464502d2d6fcca841561be5b6cc13c3c1cc5cc2 Mon Sep 17 00:00:00 2001
From: Martina Rivizzigno <martina@rivizzigno.it>
Date: Sun, 3 Mar 2019 15:04:40 +0100
Subject: [PATCH] remove empty_trajectory_waypoint and getter method for
 avoidance waypoints in FlightTasks. In obstacle avoidace library reset
 desired avoidance waypoints after publication

---
 src/lib/FlightTasks/FlightTasks.cpp               | 15 ---------------
 .../FlightTasks/tasks/FlightTask/FlightTask.cpp   |  9 ---------
 .../tasks/Utility/ObstacleAvoidance.cpp           | 14 ++++++++++++--
 3 files changed, 12 insertions(+), 26 deletions(-)

diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp
index dec5fa00ef..b7f4d15f4a 100644
--- a/src/lib/FlightTasks/FlightTasks.cpp
+++ b/src/lib/FlightTasks/FlightTasks.cpp
@@ -57,21 +57,6 @@ const landing_gear_s FlightTasks::getGear()
 	}
 }
 
-const vehicle_trajectory_waypoint_s FlightTasks::getAvoidanceWaypoint()
-{
-	if (isAnyTaskActive()) {
-		return _current_task.task->getAvoidanceWaypoint();
-
-	} else {
-		return FlightTask::empty_trajectory_waypoint;
-	}
-}
-
-const vehicle_trajectory_waypoint_s &FlightTasks::getEmptyAvoidanceWaypoint()
-{
-	return FlightTask::empty_trajectory_waypoint;
-}
-
 int FlightTasks::switchTask(FlightTaskIndex new_task_index)
 {
 	// switch to the running task, nothing to do
diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
index f94ed879ca..4517644d10 100644
--- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
@@ -8,14 +8,6 @@ const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NA
 
 const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {}};
 const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
-const vehicle_trajectory_waypoint_s FlightTask::empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0},
-	{	{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}},
-		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}},
-		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}},
-		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}},
-		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}}
-	}
-};
 
 bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
 {
@@ -92,7 +84,6 @@ void FlightTask::_resetSetpoints()
 	_jerk_setpoint.setAll(NAN);
 	_thrust_setpoint.setAll(NAN);
 	_yaw_setpoint = _yawspeed_setpoint = NAN;
-	_desired_waypoint = FlightTask::empty_trajectory_waypoint;
 }
 
 void FlightTask::_evaluateVehicleLocalPosition()
diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
index f979e44b1f..229f588482 100644
--- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
@@ -46,10 +46,19 @@ using namespace time_literals;
 /** Timeout in us for trajectory data to get considered invalid */
 static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms;
 
+const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0},
+	{	{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}},
+		{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}}
+	}
+};
+
 ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
 	ModuleParams(parent)
 {
-
+	_desired_waypoint = empty_trajectory_waypoint;
 }
 
 ObstacleAvoidance::~ObstacleAvoidance()
@@ -123,10 +132,11 @@ void ObstacleAvoidance::updateAvoidanceSetpoints(const Vector3f &pos_sp, const V
 {
 	pos_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
 	vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
 
 	_publish_avoidance_desired_waypoint();
 
-	// TODO: reset all fields to NaN
+	_desired_waypoint = empty_trajectory_waypoint;
 }
 
 void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp,