Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 03:36:07 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

mavlink: reciever trajectory_representation_waypoints resepect valid_points

Browse Source 
- fixes https://github.com/PX4/PX4-Autopilot/issues/18369
This commit is contained in:
Daniel Agar
2021-10-06 09:51:38 -04:00
parent 31a599f303
commit 0bdcd54184
1 changed files with 5 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+5 -11
View File
@@ -1917,10 +1917,9 @@ MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_mess
vehicle_trajectory_waypoint_s trajectory_waypoint{}; vehicle_trajectory_waypoint_s trajectory_waypoint{};
trajectory_waypoint.timestamp = hrt_absolute_time(); const int number_valid_points = math::min(trajectory.valid_points, vehicle_trajectory_waypoint_s::NUMBER_POINTS);
const int number_valid_points = trajectory.valid_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[0] = trajectory.pos_x[i];
trajectory_waypoint.waypoints[i].position[1] = trajectory.pos_y[i]; trajectory_waypoint.waypoints[i].position[1] = trajectory.pos_y[i];
trajectory_waypoint.waypoints[i].position[2] = trajectory.pos_z[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 = trajectory.pos_yaw[i];
trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i]; trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i];
trajectory_waypoint.waypoints[i].point_valid = true;
trajectory_waypoint.waypoints[i].type = UINT8_MAX; trajectory_waypoint.waypoints[i].type = UINT8_MAX;
} }
for (int i = 0; i < number_valid_points; ++i) { trajectory_waypoint.timestamp = hrt_absolute_time();
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_pub.publish(trajectory_waypoint); _trajectory_waypoint_pub.publish(trajectory_waypoint);
} }