Merge pull request #41 from byu-magicc/mag

Added support for streaming magnetometer messages

include/fcu_io.h

@@ -16,6 +16,7 @@
 #include <std_msgs/Int32.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/FluidPressure.h>
+#include <sensor_msgs/MagneticField.h>
 #include <sensor_msgs/Temperature.h>
 #include <sensor_msgs/Range.h>
 #include <std_srvs/Trigger.h>
@@ -60,6 +61,7 @@ class fcuIO :
   void handle_rc_channels_raw_msg(const mavlink_message_t &msg);
   void handle_diff_pressure_msg(const mavlink_message_t &msg);
   void handle_small_baro_msg(const mavlink_message_t &msg);
+  void handle_small_mag_msg(const mavlink_message_t &msg);
   void handle_named_value_int_msg(const mavlink_message_t &msg);
   void handle_named_value_float_msg(const mavlink_message_t &msg);
   void handle_named_command_struct_msg(const mavlink_message_t &msg);
@@ -96,6 +98,7 @@ class fcuIO :
   ros::Publisher temperature_pub_;
   ros::Publisher baro_pub_;
   ros::Publisher sonar_pub_;
+  ros::Publisher mag_pub_;
   ros::Publisher attitude_pub_;
   std::map<std::string, ros::Publisher> named_value_int_pubs_;
   std::map<std::string, ros::Publisher> named_value_float_pubs_;

src/fcu_io.cpp

@@ -74,6 +74,9 @@ void fcuIO::handle_mavlink_message(const mavlink_message_t &msg)
   case MAVLINK_MSG_ID_SMALL_IMU:
     handle_small_imu_msg(msg);
     break;
+  case MAVLINK_MSG_ID_SMALL_MAG:
+    handle_small_mag_msg(msg);
+    break;
   case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
     handle_servo_output_raw_msg(msg);
     break;
@@ -488,6 +491,25 @@ void fcuIO::handle_small_baro_msg(const mavlink_message_t &msg)
   }
 }
 
+void fcuIO::handle_small_mag_msg(const mavlink_message_t &msg)
+{
+  mavlink_small_mag_t mag;
+  mavlink_msg_small_mag_decode(&msg, &mag);
+
+  //! \todo calibration, correct units, floating point message type
+  sensor_msgs::MagneticField mag_msg;
+  mag_msg.header.stamp = ros::Time::now();
+  mag_msg.magnetic_field.x = (double) mag.xmag;
+  mag_msg.magnetic_field.y = (double) mag.ymag;
+  mag_msg.magnetic_field.z = (double) mag.zmag;
+
+  if (mag_pub_.getTopic().empty())
+  {
+    mag_pub_ = nh_.advertise<sensor_msgs::MagneticField>("magnetometer", 1);
+  }
+  mag_pub_.publish(mag_msg);
+}
+
 void fcuIO::handle_distance_sensor(const mavlink_message_t &msg)
 {
   mavlink_distance_sensor_t distance;