fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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].point_valid = true;
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.timestamp = hrt_absolute_time();
_trajectory_waypoint_pub.publish(trajectory_waypoint);
}