ros: mavlink onboard node: send attitude via mavlink

4 files changed, 27 insertions, 2 deletions

diff --git a/NuttX b/NuttX
-Subproject 11afcdfee6a3961952dd92f02c1abaa4756b115
+Subproject 787aca971a86219d4e791100646b54ed8245a73
diff --git a/launch/multicopter.launch b/launch/multicopter.launch
index 95400bd82..bc0e37771 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 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);
 };
 
 }