fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 12:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink_receiver: decode mavlink message TRAJECTORY in uORB msgs

Browse Source 
trajectory_waypoint or trajectory_bezier depending on the mav trajectory
representation type
This commit is contained in:
Martina
2018-03-06 12:11:56 +01:00
committed by Daniel Agar
parent e59a29197e
commit 5ef26dd862
2 changed files with 62 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+57
View File
@@ -137,6 +137,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_rc_pub(nullptr),
_manual_pub(nullptr),
_obstacle_distance_pub(nullptr),
_trajectory_waypoint_pub(nullptr),
_trajectory_bezier_pub(nullptr),
_land_detector_pub(nullptr),
_follow_target_pub(nullptr),
_landing_target_pose_pub(nullptr),
@@ -328,6 +330,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_obstacle_distance(msg);
break;
case MAVLINK_MSG_ID_TRAJECTORY:
handle_message_trajectory(msg);
break;
case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
handle_message_named_value_float(msg);
break;
@@ -1630,6 +1636,57 @@ MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_trajectory(mavlink_message_t *msg)
{
mavlink_trajectory_t trajectory;
mavlink_msg_trajectory_decode(msg, &trajectory);
if (trajectory.type == trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS) {
struct trajectory_waypoint_s trajectory_waypoint = {};
trajectory_waypoint.timestamp = hrt_absolute_time();
trajectory_waypoint.type = trajectory.type;
memcpy(trajectory_waypoint.point_0, trajectory.point_1, sizeof(trajectory_waypoint.point_0));
memcpy(trajectory_waypoint.point_1, trajectory.point_2, sizeof(trajectory_waypoint.point_1));
memcpy(trajectory_waypoint.point_2, trajectory.point_3, sizeof(trajectory_waypoint.point_2));
memcpy(trajectory_waypoint.point_3, trajectory.point_4, sizeof(trajectory_waypoint.point_3));
memcpy(trajectory_waypoint.point_4, trajectory.point_5, sizeof(trajectory_waypoint.point_4));
memcpy(trajectory_waypoint.point_valid, trajectory.point_valid, sizeof(trajectory_waypoint.point_valid));
if (_trajectory_waypoint_pub == nullptr) {
_trajectory_waypoint_pub = orb_advertise(ORB_ID(trajectory_waypoint), &trajectory_waypoint);
} else {
orb_publish(ORB_ID(trajectory_waypoint), _trajectory_waypoint_pub, &trajectory_waypoint);
}
} else if (trajectory.type == trajectory_bezier_s::MAV_TRAJECTORY_REPRESENTATION_BEZIER) {
struct trajectory_bezier_s trajectory_bezier = {};
trajectory_bezier.timestamp = hrt_absolute_time();
trajectory_bezier.type = trajectory.type;
memcpy(trajectory_bezier.point_0, trajectory.point_1, sizeof(trajectory_bezier.point_0));
memcpy(trajectory_bezier.point_1, trajectory.point_2, sizeof(trajectory_bezier.point_1));
memcpy(trajectory_bezier.point_2, trajectory.point_3, sizeof(trajectory_bezier.point_2));
memcpy(trajectory_bezier.point_3, trajectory.point_4, sizeof(trajectory_bezier.point_3));
memcpy(trajectory_bezier.point_4, trajectory.point_5, sizeof(trajectory_bezier.point_4));
memcpy(trajectory_bezier.point_valid, trajectory.point_valid, sizeof(trajectory_bezier.point_valid));
if (_trajectory_bezier_pub == nullptr) {
_trajectory_bezier_pub = orb_advertise(ORB_ID(trajectory_bezier), &trajectory_bezier);
} else {
orb_publish(ORB_ID(trajectory_bezier), _trajectory_bezier_pub, &trajectory_bezier);
}
}
}
switch_pos_t
MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw)
{
src/modules/mavlink/mavlink_receiver.h
+5
View File
@@ -67,6 +67,8 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/trajectory_bezier.h>
#include <uORB/topics/trajectory_waypoint.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -157,6 +159,7 @@ private:
void handle_message_logging_ack(mavlink_message_t *msg);
void handle_message_play_tune(mavlink_message_t *msg);
void handle_message_obstacle_distance(mavlink_message_t *msg);
void handle_message_trajectory(mavlink_message_t *msg);
void handle_message_named_value_float(mavlink_message_t *msg);
void handle_message_debug(mavlink_message_t *msg);
void handle_message_debug_vect(mavlink_message_t *msg);
@@ -232,6 +235,8 @@ private:
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
orb_advert_t _obstacle_distance_pub;
orb_advert_t _trajectory_waypoint_pub;
orb_advert_t _trajectory_bezier_pub;
orb_advert_t _land_detector_pub;
orb_advert_t _follow_target_pub;
orb_advert_t _landing_target_pose_pub;