FlightTaskAuto: add update method to get triplets for the trajectory

interface

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

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. */