From bce9e024de248f4b6fa6f67f83a245be27c1cb85 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 22 Sep 2025 13:37:24 +0200 Subject: [PATCH] gazebo_mavlink_interface: remove support for MAVLink v1 --- include/gazebo_mavlink_interface.h | 2 -- src/gazebo_mavlink_interface.cpp | 22 ++++------------------ 2 files changed, 4 insertions(+), 20 deletions(-) diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index d37b53046a..d3ba01eea1 100644 --- a/include/gazebo_mavlink_interface.h +++ b/include/gazebo_mavlink_interface.h @@ -148,8 +148,6 @@ class GazeboMavlinkInterface : public ModelPlugin { bool received_first_actuator_{false}; Eigen::VectorXd input_reference_; - float protocol_version_{2.0}; - std::unique_ptr mavlink_interface_; std::string namespace_{kDefaultNamespace}; diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index f4750f9b43..6674c82816 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -196,10 +196,6 @@ 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(); - } - node_handle_ = transport::NodePtr(new transport::Node()); node_handle_->Init(namespace_); @@ -539,18 +535,8 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf mavlink_status_t* chan_state = mavlink_get_channel_status(MAVLINK_COMM_0); - // 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"; - } + chan_state->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); + gzmsg << "Using MAVLink protocol v2.0\n"; mavlink_interface_->Load(); } @@ -963,7 +949,7 @@ void GazeboMavlinkInterface::IRLockCallback(IRLockPtr& irlock_message) { } void GazeboMavlinkInterface::targetReleativeCallback(TargetRelativePtr& targetRelative_message) { - + mavlink_target_relative_t sensor_msg; sensor_msg.timestamp = targetRelative_message->time_usec(); sensor_msg.x = targetRelative_message->pos_x(); @@ -1056,7 +1042,7 @@ void GazeboMavlinkInterface::VisionCallback(OdomPtr& odom_message) { odom_message->angular_velocity().z())); // Only sends ODOMETRY msgs if send_odometry is set and the protocol version is 2.0 - if (send_odometry_ && protocol_version_ == 2.0) { + if (send_odometry_) { // send ODOMETRY Mavlink msg mavlink_odometry_t odom;