diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 6a3e7d66c9..68a903c20b 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -3082,12 +3082,12 @@ public:
 
 	static const char *get_name_static()
 	{
-		return "TRAJECTORY";
+		return "TRAJECTORY_REPRESENTATION_WAYPOINTS";
 	}
 
 	static uint16_t get_id_static()
 	{
-		return MAVLINK_MSG_ID_TRAJECTORY;
+		return MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS;
 	}
 
 	uint16_t get_id()
@@ -3102,7 +3102,8 @@ public:
 
 	unsigned get_size()
 	{
-		return _traj_wp_avoidance_sub->is_published() ? (MAVLINK_MSG_ID_TRAJECTORY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)
+		return _traj_wp_avoidance_sub->is_published() ? (MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN +
+				MAVLINK_NUM_NON_PAYLOAD_BYTES)
 		       : 0;
 	}
 
@@ -3125,62 +3126,36 @@ protected:
 		struct vehicle_trajectory_waypoint_s traj_wp_avoidance_desired;
 
 		if (_traj_wp_avoidance_sub->update(&_traj_wp_avoidance_time, &traj_wp_avoidance_desired)) {
-			mavlink_trajectory_t msg = {};
+			mavlink_trajectory_representation_waypoints_t msg = {};
 
 			msg.time_usec = traj_wp_avoidance_desired.timestamp;
-			msg.type = traj_wp_avoidance_desired.type;
+			int number_valid_points = 0;
+
+			for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) {
+				msg.pos_x[i] = traj_wp_avoidance_desired.waypoints[i].position[0];
+				msg.pos_y[i] = traj_wp_avoidance_desired.waypoints[i].position[1];
+				msg.pos_z[i] = traj_wp_avoidance_desired.waypoints[i].position[2];
+
+				msg.vel_x[i] = traj_wp_avoidance_desired.waypoints[i].velocity[0];
+				msg.vel_y[i] = traj_wp_avoidance_desired.waypoints[i].velocity[1];
+				msg.vel_z[i] = traj_wp_avoidance_desired.waypoints[i].velocity[2];
+
+				msg.acc_x[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[0];
+				msg.acc_y[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[1];
+				msg.acc_z[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[2];
+
+				msg.pos_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw;
+				msg.vel_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw_speed;
+
+				if (traj_wp_avoidance_desired.waypoints[i].point_valid) {
+					number_valid_points++;
+				}
 
-			for (int i = 0; i < 3; ++i) {
-				msg.point_1[i] = traj_wp_avoidance_desired.waypoints[0].position[i];
-				msg.point_1[3 + i] = traj_wp_avoidance_desired.waypoints[0].velocity[i];
-				msg.point_1[6 + i] = traj_wp_avoidance_desired.waypoints[0].acceleration[i];
 			}
 
-			msg.point_1[9] = traj_wp_avoidance_desired.waypoints[0].yaw;
-			msg.point_1[10] = traj_wp_avoidance_desired.waypoints[0].yaw_speed;
-			msg.point_valid[0] = traj_wp_avoidance_desired.waypoints[0].point_valid;
+			msg.valid_points = number_valid_points;
 
-			for (int i = 0; i < 3; ++i) {
-				msg.point_2[i] = traj_wp_avoidance_desired.waypoints[1].position[i];
-				msg.point_2[3 + i] = traj_wp_avoidance_desired.waypoints[1].velocity[i];
-				msg.point_2[6 + i] = traj_wp_avoidance_desired.waypoints[1].acceleration[i];
-			}
-
-			msg.point_2[9] = traj_wp_avoidance_desired.waypoints[1].yaw;
-			msg.point_2[10] = traj_wp_avoidance_desired.waypoints[1].yaw_speed;
-			msg.point_valid[1] = traj_wp_avoidance_desired.waypoints[1].point_valid;
-
-			for (int i = 0; i < 3; ++i) {
-				msg.point_3[i] = traj_wp_avoidance_desired.waypoints[2].position[i];
-				msg.point_3[3 + i] = traj_wp_avoidance_desired.waypoints[2].velocity[i];
-				msg.point_3[6 + i] = traj_wp_avoidance_desired.waypoints[2].acceleration[i];
-			}
-
-			msg.point_3[9] = traj_wp_avoidance_desired.waypoints[2].yaw;
-			msg.point_3[10] = traj_wp_avoidance_desired.waypoints[2].yaw_speed;
-			msg.point_valid[2] = traj_wp_avoidance_desired.waypoints[2].point_valid;
-
-			for (int i = 0; i < 3; ++i) {
-				msg.point_4[i] = traj_wp_avoidance_desired.waypoints[3].position[i];
-				msg.point_4[3 + i] = traj_wp_avoidance_desired.waypoints[3].velocity[i];
-				msg.point_4[6 + i] = traj_wp_avoidance_desired.waypoints[3].acceleration[i];
-			}
-
-			msg.point_4[9] = traj_wp_avoidance_desired.waypoints[3].yaw;
-			msg.point_4[10] = traj_wp_avoidance_desired.waypoints[3].yaw_speed;
-			msg.point_valid[3] = traj_wp_avoidance_desired.waypoints[3].point_valid;
-
-			for (int i = 0; i < 3; ++i) {
-				msg.point_5[i] = traj_wp_avoidance_desired.waypoints[4].position[i];
-				msg.point_5[3 + i] = traj_wp_avoidance_desired.waypoints[4].velocity[i];
-				msg.point_5[6 + i] = traj_wp_avoidance_desired.waypoints[4].acceleration[i];
-			}
-
-			msg.point_5[9] = traj_wp_avoidance_desired.waypoints[4].yaw;
-			msg.point_5[10] = traj_wp_avoidance_desired.waypoints[4].yaw_speed;
-			msg.point_valid[4] = traj_wp_avoidance_desired.waypoints[4].point_valid;
-
-			mavlink_msg_trajectory_send_struct(_mavlink->get_channel(), &msg);
+			mavlink_msg_trajectory_representation_waypoints_send_struct(_mavlink->get_channel(), &msg);
 
 			return true;
 		}
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 4aa1d5b1cb..d2976caa9b 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -329,8 +329,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_obstacle_distance(msg);
 		break;
 
-	case MAVLINK_MSG_ID_TRAJECTORY:
-		handle_message_trajectory(msg);
+	case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS:
+		handle_message_trajectory_representation_waypoints(msg);
 		break;
 
 	case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
@@ -1636,76 +1636,50 @@ MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg)
 }
 
 void
-MavlinkReceiver::handle_message_trajectory(mavlink_message_t *msg)
+MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_message_t *msg)
 {
-	mavlink_trajectory_t trajectory;
-	mavlink_msg_trajectory_decode(msg, &trajectory);
-
-	if (trajectory.type == vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS) {
-
-		struct vehicle_trajectory_waypoint_s trajectory_waypoint = {};
-
-		trajectory_waypoint.timestamp = hrt_absolute_time();
-		trajectory_waypoint.type = trajectory.type;
+	mavlink_trajectory_representation_waypoints_t trajectory;
+	mavlink_msg_trajectory_representation_waypoints_decode(msg, &trajectory);
 
 
-		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];
+	struct vehicle_trajectory_waypoint_s trajectory_waypoint = {};
 
-		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];
+	trajectory_waypoint.timestamp = hrt_absolute_time();
+	const int number_valid_points = trajectory.valid_points;
 
-		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];
+	for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) {
+		trajectory_waypoint.waypoints[i].position[0] = trajectory.pos_x[i];
+		trajectory_waypoint.waypoints[i].position[1] = trajectory.pos_y[i];
+		trajectory_waypoint.waypoints[i].position[2] = trajectory.pos_z[i];
 
-		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];
+		trajectory_waypoint.waypoints[i].velocity[0] = trajectory.vel_x[i];
+		trajectory_waypoint.waypoints[i].velocity[1] = trajectory.vel_y[i];
+		trajectory_waypoint.waypoints[i].velocity[2] = trajectory.vel_z[i];
 
-		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];
+		trajectory_waypoint.waypoints[i].acceleration[0] = trajectory.acc_x[i];
+		trajectory_waypoint.waypoints[i].acceleration[1] = trajectory.acc_y[i];
+		trajectory_waypoint.waypoints[i].acceleration[2] = trajectory.acc_z[i];
 
-		if (_trajectory_waypoint_pub == nullptr) {
-			_trajectory_waypoint_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint), &trajectory_waypoint);
+		trajectory_waypoint.waypoints[i].yaw = trajectory.pos_yaw[i];
+		trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i];
 
-		} else {
-			orb_publish(ORB_ID(vehicle_trajectory_waypoint), _trajectory_waypoint_pub, &trajectory_waypoint);
-		}
 	}
+
+	for (int i = 0; i < number_valid_points; ++i) {
+		trajectory_waypoint.waypoints[i].point_valid = true;
+	}
+
+	for (int i = number_valid_points; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) {
+		trajectory_waypoint.waypoints[i].point_valid = false;
+	}
+
+	if (_trajectory_waypoint_pub == nullptr) {
+		_trajectory_waypoint_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint), &trajectory_waypoint);
+
+	} else {
+		orb_publish(ORB_ID(vehicle_trajectory_waypoint), _trajectory_waypoint_pub, &trajectory_waypoint);
+	}
+
 }
 
 switch_pos_t
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index fa5e1e6304..085dc75ddc 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -158,7 +158,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_trajectory_representation_waypoints(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);