mavlink_receiver: refatcor to use vehicle_trajectory_waypoint

src/modules/mavlink/mavlink_receiver.cpp

@@ -1642,26 +1642,69 @@ 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) {
+	if (trajectory.type == vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS) {
 
-		struct trajectory_waypoint_s trajectory_waypoint = {};
+		struct vehicle_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));
+		matrix::Vector3f(trajectory.point_1[0], trajectory.point_1[1],
+				 trajectory.point_1[2]).copyTo(trajectory_waypoint.waypoints[0].position);
+		matrix::Vector3f(trajectory.point_1[3], trajectory.point_1[4],
+				 trajectory.point_1[5]).copyTo(trajectory_waypoint.waypoints[0].velocity);
+		matrix::Vector3f(trajectory.point_1[6], trajectory.point_1[7],
+				 trajectory.point_1[8]).copyTo(trajectory_waypoint.waypoints[0].acceleration);
+		trajectory_waypoint.waypoints[0].yaw = trajectory.point_1[9];
+		trajectory_waypoint.waypoints[0].yaw_speed = trajectory.point_1[10];
+		trajectory_waypoint.waypoints[0].point_valid = trajectory.point_valid[0];
+
+		matrix::Vector3f(trajectory.point_2[0], trajectory.point_2[1],
+				 trajectory.point_2[2]).copyTo(trajectory_waypoint.waypoints[1].position);
+		matrix::Vector3f(trajectory.point_2[3], trajectory.point_2[4],
+				 trajectory.point_2[5]).copyTo(trajectory_waypoint.waypoints[1].velocity);
+		matrix::Vector3f(trajectory.point_2[6], trajectory.point_2[7],
+				 trajectory.point_2[8]).copyTo(trajectory_waypoint.waypoints[1].acceleration);
+		trajectory_waypoint.waypoints[1].yaw = trajectory.point_2[9];
+		trajectory_waypoint.waypoints[1].yaw_speed = trajectory.point_2[10];
+		trajectory_waypoint.waypoints[1].point_valid = trajectory.point_valid[1];
+
+		matrix::Vector3f(trajectory.point_3[0], trajectory.point_3[1],
+				 trajectory.point_3[2]).copyTo(trajectory_waypoint.waypoints[2].position);
+		matrix::Vector3f(trajectory.point_3[3], trajectory.point_3[4],
+				 trajectory.point_3[5]).copyTo(trajectory_waypoint.waypoints[2].velocity);
+		matrix::Vector3f(trajectory.point_3[6], trajectory.point_3[7],
+				 trajectory.point_3[8]).copyTo(trajectory_waypoint.waypoints[2].acceleration);
+		trajectory_waypoint.waypoints[2].yaw = trajectory.point_3[9];
+		trajectory_waypoint.waypoints[2].yaw_speed = trajectory.point_3[10];
+		trajectory_waypoint.waypoints[2].point_valid = trajectory.point_valid[2];
+
+		matrix::Vector3f(trajectory.point_4[0], trajectory.point_4[1],
+				 trajectory.point_4[2]).copyTo(trajectory_waypoint.waypoints[3].position);
+		matrix::Vector3f(trajectory.point_4[3], trajectory.point_4[4],
+				 trajectory.point_4[5]).copyTo(trajectory_waypoint.waypoints[3].velocity);
+		matrix::Vector3f(trajectory.point_4[6], trajectory.point_4[7],
+				 trajectory.point_4[8]).copyTo(trajectory_waypoint.waypoints[3].acceleration);
+		trajectory_waypoint.waypoints[3].yaw = trajectory.point_4[9];
+		trajectory_waypoint.waypoints[3].yaw_speed = trajectory.point_4[10];
+		trajectory_waypoint.waypoints[3].point_valid = trajectory.point_valid[3];
+
+		matrix::Vector3f(trajectory.point_5[0], trajectory.point_5[1],
+				 trajectory.point_5[2]).copyTo(trajectory_waypoint.waypoints[4].position);
+		matrix::Vector3f(trajectory.point_5[3], trajectory.point_5[4],
+				 trajectory.point_5[5]).copyTo(trajectory_waypoint.waypoints[4].velocity);
+		matrix::Vector3f(trajectory.point_5[6], trajectory.point_5[7],
+				 trajectory.point_5[8]).copyTo(trajectory_waypoint.waypoints[4].acceleration);
+		trajectory_waypoint.waypoints[4].yaw = trajectory.point_5[9];
+		trajectory_waypoint.waypoints[4].yaw_speed = trajectory.point_5[10];
+		trajectory_waypoint.waypoints[4].point_valid = trajectory.point_valid[4];
 
 		if (_trajectory_waypoint_pub == nullptr) {
-			_trajectory_waypoint_pub = orb_advertise(ORB_ID(trajectory_waypoint), &trajectory_waypoint);
+			_trajectory_waypoint_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint), &trajectory_waypoint);
 
 		} else {
-			orb_publish(ORB_ID(trajectory_waypoint), _trajectory_waypoint_pub, &trajectory_waypoint);
+			orb_publish(ORB_ID(vehicle_trajectory_waypoint), _trajectory_waypoint_pub, &trajectory_waypoint);
 		}
 
 	} else if (trajectory.type == trajectory_bezier_s::MAV_TRAJECTORY_REPRESENTATION_BEZIER) {

src/modules/mavlink/mavlink_receiver.h

@@ -68,7 +68,7 @@
 #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/vehicle_trajectory_waypoint.h>
 #include <uORB/topics/transponder_report.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vehicle_control_mode.h>