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: [PATCH] ros: mavlink onboard node: send attitude via mavlink

---
 NuttX                                       |  2 +-
 launch/multicopter.launch                   |  1 +
 src/platforms/ros/nodes/mavlink/mavlink.cpp | 24 +++++++++++++++++++--
 src/platforms/ros/nodes/mavlink/mavlink.h   |  4 ++++
 4 files changed, 28 insertions(+), 3 deletions(-)

diff --git a/NuttX b/NuttX
index 11afcdfee6..787aca971a 160000
--- a/NuttX
+++ b/NuttX
@@ -1 +1 @@
-Subproject commit 11afcdfee6a3961952dd92f02c1abaa4756b115f
+Subproject commit 787aca971a86219d4e791100646b54ed8245a733
diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 95400bd82d..bc0e377715 100644
--- a/launch/multicopter.launch
+++ b/launch/multicopter.launch
@@ -9,6 +9,7 @@
 	<node pkg="px4" name="position_estimator" type="position_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
 	<node pkg="px4" name="mc_pos_control" type="mc_pos_control"/>
+	<node pkg="px4" name="mavlink" type="mavlink"/>
 	<!-- <node pkg="rosbag" type="record" name="record" output="screen" args="-a -O px4_multicopter"/> -->
 </group>
 
diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp
index b6413c3b4e..131a4930f2 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 5b39468870..e683597a94 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);
 };
 
 }