From 17a08a9de7edbc547a3749e783949d81e55f91f3 Mon Sep 17 00:00:00 2001
From: Martina <martina@rivizzigno.it>
Date: Mon, 9 Jul 2018 11:52:27 +0200
Subject: [PATCH] FlightTaskAuto: add update method to get triplets for the
 trajectory interface

---
 .../FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 28 +++++++++++++++++++
 .../FlightTasks/tasks/Auto/FlightTaskAuto.hpp |  1 +
 2 files changed, 29 insertions(+)

diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
index f5c9344685..3361b23f94 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
@@ -214,6 +214,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 
 	if (triplet_update || (_current_state != previous_state)) {
 		_updateInternalWaypoints();
+		_updateAvoidanceWaypoints();
 	}
 
 	return true;
@@ -264,6 +265,33 @@ void FlightTaskAuto::_set_heading_from_mode()
 	}
 }
 
+void FlightTaskAuto::_updateAvoidanceWaypoints()
+{
+	_desired_waypoint.timestamp = hrt_absolute_time();
+
+	_target.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);
+	Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].acceleration);
+
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = _sub_triplet_setpoint->get().current.yaw;
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed =
+		_sub_triplet_setpoint->get().current.yawspeed_valid ?
+		_sub_triplet_setpoint->get().current.yawspeed : NAN;
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true;
+
+
+	_next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position);
+	Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity);
+	Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].acceleration);
+
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw = _sub_triplet_setpoint->get().next.yaw;
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed =
+		_sub_triplet_setpoint->get().next.yawspeed_valid ?
+		_sub_triplet_setpoint->get().next.yawspeed : NAN;
+	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true;
+
+}
+
 bool FlightTaskAuto::_isFinite(const position_setpoint_s sp)
 {
 	return (PX4_ISFINITE(sp.lat) && PX4_ISFINITE(sp.lon) && PX4_ISFINITE(sp.alt));
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
index 98368265ee..3941aca1ab 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
@@ -84,6 +84,7 @@ protected:
 	matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/
 	void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
 	bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */
+	void _updateAvoidanceWaypoints(); /**< fill desired_waypoints with the triplets. */
 
 	matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
 	matrix::Vector3f _prev_wp{}; /**< Previous waypoint  (local frame). If no previous triplet is available, the prev_wp is set to current position. */