diff options
Diffstat (limited to 'src/platforms/ros/nodes')
-rw-r--r-- | src/platforms/ros/nodes/mavlink/mavlink.cpp | 21 | ||||
-rw-r--r-- | src/platforms/ros/nodes/mavlink/mavlink.h | 12 |
2 files changed, 31 insertions, 2 deletions
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 8d658caa5..3485b1f4e 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -49,6 +49,7 @@ using namespace px4; Mavlink::Mavlink() : _n(), _v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)), + _v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)), _offboard_control_mode_pub(_n.advertise<offboard_control_mode>("offboard_control_mode", 1)) { _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); @@ -71,7 +72,7 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) _link->get_system_id(), _link->get_component_id(), _link->get_channel(), - &msg_m, //XXX hardcoded + &msg_m, get_time_micros() / 1000, msg->q[0], msg->q[1], @@ -83,6 +84,24 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) _link->send_message(&msg_m); } +void Mavlink::VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg) +{ + mavlink_message_t msg_m; + mavlink_msg_local_position_ned_pack_chan( + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, + get_time_micros() / 1000, + msg->x, + msg->y, + msg->z, + msg->vx, + msg->vy, + msg->vz); + _link->send_message(&msg_m); +} + void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) { (void)sysid; (void)compid; diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index 2ee383b4f..a246af4a4 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -43,6 +43,7 @@ #include "ros/ros.h" #include <mavconn/interface.h> #include <px4/vehicle_attitude.h> +#include <px4/vehicle_local_position.h> #include <px4/vehicle_attitude_setpoint.h> #include <px4/offboard_control_mode.h> @@ -61,17 +62,26 @@ protected: ros::NodeHandle _n; mavconn::MAVConnInterface::Ptr _link; ros::Subscriber _v_att_sub; + ros::Subscriber _v_local_pos_sub; ros::Publisher _v_att_sp_pub; ros::Publisher _offboard_control_mode_pub; /** * * Simulates output of attitude data from the FCU - * Equivalent to the mavlink stream ATTITUDE + * Equivalent to the mavlink stream ATTITUDE_QUATERNION * * */ void VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg); + /** + * + * Simulates output of local position data from the FCU + * Equivalent to the mavlink stream LOCAL_POSITION_NED + * + * */ + void VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg); + /** * |