From 0bdcd5418468526d28cd09ac4c181d37422cf077 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 6 Oct 2021 09:51:38 -0400 Subject: [PATCH] mavlink: reciever trajectory_representation_waypoints resepect valid_points - fixes https://github.com/PX4/PX4-Autopilot/issues/18369 --- src/modules/mavlink/mavlink_receiver.cpp | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bfac5533794d..87199da55181 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1917,10 +1917,9 @@ MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_mess vehicle_trajectory_waypoint_s trajectory_waypoint{}; - trajectory_waypoint.timestamp = hrt_absolute_time(); - const int number_valid_points = trajectory.valid_points; + const int number_valid_points = math::min(trajectory.valid_points, vehicle_trajectory_waypoint_s::NUMBER_POINTS); - for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) { + for (int i = 0; i < number_valid_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]; @@ -1936,17 +1935,12 @@ MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_mess trajectory_waypoint.waypoints[i].yaw = trajectory.pos_yaw[i]; trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i]; - trajectory_waypoint.waypoints[i].type = UINT8_MAX; - } - - 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; + trajectory_waypoint.waypoints[i].type = UINT8_MAX; } + trajectory_waypoint.timestamp = hrt_absolute_time(); _trajectory_waypoint_pub.publish(trajectory_waypoint); }