From 5ef26dd86239f2a2b9032303f5cd25b123ca85ed Mon Sep 17 00:00:00 2001
From: Martina <martina@rivizzigno.it>
Date: Tue, 6 Mar 2018 12:11:56 +0100
Subject: [PATCH] mavlink_receiver: decode mavlink message TRAJECTORY in uORB
 msgs trajectory_waypoint or trajectory_bezier depending on the mav trajectory
 representation type

---
 src/modules/mavlink/mavlink_receiver.cpp | 57 ++++++++++++++++++++++++
 src/modules/mavlink/mavlink_receiver.h   |  5 +++
 2 files changed, 62 insertions(+)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7bbdfc2fb4..280bf276e3 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -137,6 +137,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_rc_pub(nullptr),
 	_manual_pub(nullptr),
 	_obstacle_distance_pub(nullptr),
+	_trajectory_waypoint_pub(nullptr),
+	_trajectory_bezier_pub(nullptr),
 	_land_detector_pub(nullptr),
 	_follow_target_pub(nullptr),
 	_landing_target_pose_pub(nullptr),
@@ -328,6 +330,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_obstacle_distance(msg);
 		break;
 
+	case MAVLINK_MSG_ID_TRAJECTORY:
+		handle_message_trajectory(msg);
+		break;
+
 	case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
 		handle_message_named_value_float(msg);
 		break;
@@ -1630,6 +1636,57 @@ MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_trajectory(mavlink_message_t *msg)
+{
+	mavlink_trajectory_t trajectory;
+	mavlink_msg_trajectory_decode(msg, &trajectory);
+
+	if (trajectory.type == trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS) {
+
+		struct trajectory_waypoint_s trajectory_waypoint = {};
+
+		trajectory_waypoint.timestamp = hrt_absolute_time();
+		trajectory_waypoint.type = trajectory.type;
+
+		memcpy(trajectory_waypoint.point_0, trajectory.point_1, sizeof(trajectory_waypoint.point_0));
+		memcpy(trajectory_waypoint.point_1, trajectory.point_2, sizeof(trajectory_waypoint.point_1));
+		memcpy(trajectory_waypoint.point_2, trajectory.point_3, sizeof(trajectory_waypoint.point_2));
+		memcpy(trajectory_waypoint.point_3, trajectory.point_4, sizeof(trajectory_waypoint.point_3));
+		memcpy(trajectory_waypoint.point_4, trajectory.point_5, sizeof(trajectory_waypoint.point_4));
+
+		memcpy(trajectory_waypoint.point_valid, trajectory.point_valid, sizeof(trajectory_waypoint.point_valid));
+
+		if (_trajectory_waypoint_pub == nullptr) {
+			_trajectory_waypoint_pub = orb_advertise(ORB_ID(trajectory_waypoint), &trajectory_waypoint);
+
+		} else {
+			orb_publish(ORB_ID(trajectory_waypoint), _trajectory_waypoint_pub, &trajectory_waypoint);
+		}
+	} else if (trajectory.type == trajectory_bezier_s::MAV_TRAJECTORY_REPRESENTATION_BEZIER) {
+
+		struct trajectory_bezier_s trajectory_bezier = {};
+
+		trajectory_bezier.timestamp = hrt_absolute_time();
+		trajectory_bezier.type = trajectory.type;
+
+		memcpy(trajectory_bezier.point_0, trajectory.point_1, sizeof(trajectory_bezier.point_0));
+		memcpy(trajectory_bezier.point_1, trajectory.point_2, sizeof(trajectory_bezier.point_1));
+		memcpy(trajectory_bezier.point_2, trajectory.point_3, sizeof(trajectory_bezier.point_2));
+		memcpy(trajectory_bezier.point_3, trajectory.point_4, sizeof(trajectory_bezier.point_3));
+		memcpy(trajectory_bezier.point_4, trajectory.point_5, sizeof(trajectory_bezier.point_4));
+
+		memcpy(trajectory_bezier.point_valid, trajectory.point_valid, sizeof(trajectory_bezier.point_valid));
+
+		if (_trajectory_bezier_pub == nullptr) {
+			_trajectory_bezier_pub = orb_advertise(ORB_ID(trajectory_bezier), &trajectory_bezier);
+
+		} else {
+			orb_publish(ORB_ID(trajectory_bezier), _trajectory_bezier_pub, &trajectory_bezier);
+		}
+	}
+}
+
 switch_pos_t
 MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw)
 {
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 165a5a4f54..617cc59b29 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -67,6 +67,8 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/trajectory_bezier.h>
+#include <uORB/topics/trajectory_waypoint.h>
 #include <uORB/topics/transponder_report.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -157,6 +159,7 @@ private:
 	void handle_message_logging_ack(mavlink_message_t *msg);
 	void handle_message_play_tune(mavlink_message_t *msg);
 	void handle_message_obstacle_distance(mavlink_message_t *msg);
+	void handle_message_trajectory(mavlink_message_t *msg);
 	void handle_message_named_value_float(mavlink_message_t *msg);
 	void handle_message_debug(mavlink_message_t *msg);
 	void handle_message_debug_vect(mavlink_message_t *msg);
@@ -232,6 +235,8 @@ private:
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
 	orb_advert_t _obstacle_distance_pub;
+	orb_advert_t _trajectory_waypoint_pub;
+	orb_advert_t _trajectory_bezier_pub;
 	orb_advert_t _land_detector_pub;
 	orb_advert_t _follow_target_pub;
 	orb_advert_t _landing_target_pose_pub;