From cd91ef411aff947f234eb829479e6242d9fabcf7 Mon Sep 17 00:00:00 2001
From: TSC21 <n.marques21@hotmail.com>
Date: Fri, 11 May 2018 11:50:17 +0100
Subject: [PATCH] mavlink_interface: set protocol ver to 2.0; only send
 ODOMETRY in that case

---
 include/gazebo_mavlink_interface.h |  5 ++++-
 src/gazebo_mavlink_interface.cpp   | 21 +++++++++++++++++++--
 2 files changed, 23 insertions(+), 3 deletions(-)

diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h
index 48c6ca0f1d..d94313df54 100644
--- a/include/gazebo_mavlink_interface.h
+++ b/include/gazebo_mavlink_interface.h
@@ -120,13 +120,14 @@ class GazeboMavlinkInterface : public ModelPlugin {
   GazeboMavlinkInterface() : ModelPlugin(),
     received_first_referenc_(false),
     namespace_(kDefaultNamespace),
+    protocol_version_(2.0),
     motor_velocity_reference_pub_topic_(kDefaultMotorVelocityReferencePubTopic),
     use_propeller_pid_(false),
     use_elevator_pid_(false),
     use_left_elevon_pid_(false),
     use_right_elevon_pid_(false),
     vehicle_is_tailsitter_(false),
-    send_odometry_(false),
+    send_odometry_(true),
     imu_sub_topic_(kDefaultImuTopic),
     opticalFlow_sub_topic_(kDefaultOpticalFlowTopic),
     lidar_sub_topic_(kDefaultLidarTopic),
@@ -178,6 +179,8 @@ class GazeboMavlinkInterface : public ModelPlugin {
   bool received_first_referenc_;
   Eigen::VectorXd input_reference_;
 
+  float protocol_version_;
+
   std::string namespace_;
   std::string motor_velocity_reference_pub_topic_;
   std::string mavlink_control_sub_topic_;
diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp
index c462a77869..034ae6a9a5 100644
--- a/src/gazebo_mavlink_interface.cpp
+++ b/src/gazebo_mavlink_interface.cpp
@@ -47,6 +47,10 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf
     gzerr << "[gazebo_mavlink_interface] Please specify a robotNamespace.\n";
   }
 
+  if (_sdf->HasElement("protocol_version")) {
+    protocol_version_ = _sdf->GetElement("protocol_version")->Get<float>();
+  }
+
   node_handle_ = transport::NodePtr(new transport::Node());
   node_handle_->Init(namespace_);
 
@@ -335,7 +339,19 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf
   fds[0].events = POLLIN;
 
   mavlink_status_t* chan_state = mavlink_get_channel_status(MAVLINK_COMM_0);
-  chan_state->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+
+  // set the Mavlink protocol version to use on the link
+  if (protocol_version_ == 2.0) {
+    chan_state->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
+    gzmsg << "Using MAVLink protocol v2.0\n";
+  }
+  else if (protocol_version_ == 1.0) {
+    chan_state->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+    gzmsg << "Using MAVLink protocol v1.0\n";
+  }
+  else {
+    gzerr << "Unkown protocol version! Using v" << protocol_version_ << "by default \n";
+  }
 }
 
 // This gets called by the world update start event.
@@ -779,7 +795,8 @@ void GazeboMavlinkInterface::VisionCallback(OdomPtr& odom_message) {
     odom_message->angular_velocity().y(),
     odom_message->angular_velocity().z()));
 
-  if (send_odometry_){
+  // Only sends ODOMETRY msgs if send_odometry is set and the protocol version is 2.0
+  if (send_odometry_ && protocol_version_ == 2.0) {
     // send ODOMETRY Mavlink msg
     mavlink_odometry_t odom;