From 3e5cbfcf77939d5f650885c7a82aaf527d40a094 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 11 Feb 2015 23:02:58 +0100
Subject: ros: mavlink onboard node: send attitude via mavlink

---
 src/platforms/ros/nodes/mavlink/mavlink.cpp | 24 ++++++++++++++++++++++--
 src/platforms/ros/nodes/mavlink/mavlink.h   |  4 ++++
 2 files changed, 26 insertions(+), 2 deletions(-)

(limited to 'src/platforms/ros/nodes')

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);
+}
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h
index 5b3946887..e683597a9 100644
--- a/src/platforms/ros/nodes/mavlink/mavlink.h
+++ b/src/platforms/ros/nodes/mavlink/mavlink.h
@@ -42,6 +42,7 @@
 
 #include "ros/ros.h"
 #include <mavconn/interface.h>
+#include <px4/vehicle_attitude.h>
 
 namespace px4
 {
@@ -57,6 +58,9 @@ protected:
 
 	ros::NodeHandle _n;
 	mavconn::MAVConnInterface::Ptr _link;
+	ros::Subscriber _v_att_sub;
+
+	void VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg);
 };
 
 }
-- 
cgit v1.2.3