fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 14:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add mavlink parsing for bezier message

Browse Source
This commit is contained in:
Julian Kent
2020-03-09 09:51:49 +01:00
committed by Julian Kent
parent 6e1185d4f2
commit d2507e831e
2 changed files with 30 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+27
View File
@@ -241,6 +241,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_obstacle_distance(msg);
break;
case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER:
handle_message_trajectory_representation_bezier(msg);
break;
case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS:
handle_message_trajectory_representation_waypoints(msg);
break;
@@ -1776,6 +1780,29 @@ MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg)
_obstacle_distance_pub.publish(obstacle_distance);
}
void
MavlinkReceiver::handle_message_trajectory_representation_bezier(mavlink_message_t *msg)
{
mavlink_trajectory_representation_bezier_t trajectory;
mavlink_msg_trajectory_representation_bezier_decode(msg, &trajectory);
vehicle_trajectory_bezier_s trajectory_bezier{};
trajectory_bezier.timestamp = _mavlink_timesync.sync_stamp(trajectory.time_usec);
for (int i = 0; i < vehicle_trajectory_bezier_s::NUMBER_POINTS; ++i) {
trajectory_bezier.control_points[i].position[0] = trajectory.pos_x[i];
trajectory_bezier.control_points[i].position[1] = trajectory.pos_y[i];
trajectory_bezier.control_points[i].position[2] = trajectory.pos_z[i];
trajectory_bezier.control_points[i].delta = trajectory.delta[i];
trajectory_bezier.control_points[i].yaw = trajectory.pos_yaw[i];
}
trajectory_bezier.bezier_order = math::min(trajectory.valid_points, vehicle_trajectory_bezier_s::NUMBER_POINTS);
_trajectory_bezier_pub.publish(trajectory_bezier);
}
void
MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_message_t *msg)
{