dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add mavlink parsing for bezier message

This commit is contained in:
Julian Kent 2020-02-19 10:47:25 +01:00 committed by Julian Kent
parent 6e1185d4f2
commit d2507e831e
2 changed files with 30 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/modules/mavlink/mavlink_receiver.cpp
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)
{

3
src/modules/mavlink/mavlink_receiver.h
View File

@ -98,6 +98,7 @@
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_trajectory_bezier.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
class Mavlink;
@ -166,6 +167,7 @@ private:
void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
void handle_message_set_position_target_global_int(mavlink_message_t *msg);
void handle_message_statustext(mavlink_message_t *msg);
void handle_message_trajectory_representation_bezier(mavlink_message_t *msg);
void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg);
void handle_message_utm_global_position(mavlink_message_t *msg);
void handle_message_vision_position_estimate(mavlink_message_t *msg);
@ -248,6 +250,7 @@ private:
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::Publication<vehicle_rates_setpoint_s> _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication<vehicle_trajectory_bezier_s> _trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)};
uORB::Publication<vehicle_trajectory_waypoint_s> _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
// ORB publications (multi)