diff options
Diffstat (limited to 'src/platforms/ros/nodes/mavlink/mavlink.cpp')
| -rw-r--r-- | src/platforms/ros/nodes/mavlink/mavlink.cpp | 24 |
1 files changed, 22 insertions, 2 deletions
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index b6413c3b4..131a4930f 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -47,10 +47,11 @@ using namespace px4; Mavlink::Mavlink() : - _n() + _n(), + _v_att_sub(_n.subscribe("vehicle_attitude", 10, &Mavlink::VehicleAttitudeCallback, this)) { - _link = mavconn::MAVConnInterface::open_url("udp://localhost:14551@localhost:14552"); + _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); } int main(int argc, char **argv) @@ -60,3 +61,22 @@ int main(int argc, char **argv) ros::spin(); return 0; } + +void Mavlink::VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg) +{ + mavlink_message_t msg_m; + mavlink_msg_attitude_quaternion_pack_chan( + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, //XXX hardcoded + get_time_micros() / 1000, + msg->q[0], + msg->q[1], + msg->q[2], + msg->q[3], + msg->rollspeed, + msg->pitchspeed, + msg->yawspeed); + _link->send_message(&msg_m); +} |