diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 4640be99e3..58504df6dd 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -138,9 +138,7 @@ private:
 	bool				_fake_gps;					///< fake gps output
 	int 				_gps_num;					///< number of GPS connected
 
-	static const int _orb_inject_data_fd_count = 4;
-	int _orb_inject_data_fd[_orb_inject_data_fd_count];
-	int _orb_inject_data_next = 0;
+	int _orb_inject_data_fd;
 
 	orb_advert_t _dump_communication_pub;			///< if non-null, dump communication
 	gps_dump_s *_dump_to_device;
@@ -259,6 +257,7 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num
 	_last_rate_rtcm_injection_count(0),
 	_fake_gps(fake_gps),
 	_gps_num(gps_num),
+	_orb_inject_data_fd(-1),
 	_dump_communication_pub(nullptr),
 	_dump_to_device(nullptr),
 	_dump_from_device(nullptr)
@@ -276,10 +275,6 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num
 		_p_report_sat_info = &_sat_info->_data;
 		memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
 	}
-
-	for (int i = 0; i < _orb_inject_data_fd_count; ++i) {
-		_orb_inject_data_fd[i] = -1;
-	}
 }
 
 GPS::~GPS()
@@ -426,19 +421,18 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 
 void GPS::handleInjectDataTopic()
 {
-	if (_orb_inject_data_fd[0] == -1) {
+	if (_orb_inject_data_fd == -1) {
 		return;
 	}
 
 	bool updated = false;
 
 	do {
-		int orb_inject_data_cur_fd = _orb_inject_data_fd[_orb_inject_data_next];
-		orb_check(orb_inject_data_cur_fd, &updated);
+		orb_check(_orb_inject_data_fd, &updated);
 
 		if (updated) {
 			struct gps_inject_data_s msg;
-			orb_copy(ORB_ID(gps_inject_data), orb_inject_data_cur_fd, &msg);
+			orb_copy(ORB_ID(gps_inject_data), _orb_inject_data_fd, &msg);
 
 			/* Write the message to the gps device. Note that the message could be fragmented.
 			 * But as we don't write anywhere else to the device during operation, we don't
@@ -446,8 +440,6 @@ void GPS::handleInjectDataTopic()
 			 */
 			injectData(msg.data, msg.len);
 
-			_orb_inject_data_next = (_orb_inject_data_next + 1) % _orb_inject_data_fd_count;
-
 			++_last_rate_rtcm_injection_count;
 		}
 	} while (updated);
@@ -658,9 +650,7 @@ GPS::task_main()
 	fcntl(_serial_fd, F_SETFL, flags | O_NONBLOCK);
 #endif
 
-	for (int i = 0; i < _orb_inject_data_fd_count; ++i) {
-		_orb_inject_data_fd[i] = orb_subscribe_multi(ORB_ID(gps_inject_data), i);
-	}
+	_orb_inject_data_fd = orb_subscribe(ORB_ID(gps_inject_data));
 
 	initializeCommunicationDump();
 
@@ -834,10 +824,7 @@ GPS::task_main()
 
 	PX4_INFO("exiting");
 
-	for (size_t i = 0; i < _orb_inject_data_fd_count; ++i) {
-		orb_unsubscribe(_orb_inject_data_fd[i]);
-		_orb_inject_data_fd[i] = -1;
-	}
+	orb_unsubscribe(_orb_inject_data_fd);
 
 	if (_dump_communication_pub) {
 		orb_unadvertise(_dump_communication_pub);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index d0a97ab1bd..600bdd3673 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -125,6 +125,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_time_offset_pub(nullptr),
 	_follow_target_pub(nullptr),
 	_transponder_report_pub(nullptr),
+	_gps_inject_data_pub(nullptr),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
@@ -143,9 +144,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_mom_switch_pos{},
 	_mom_switch_state(0)
 {
-	for (int i = 0; i < _gps_inject_data_pub_size; ++i) {
-		_gps_inject_data_pub[i] = nullptr;
-	}
 }
 
 MavlinkReceiver::~MavlinkReceiver()
@@ -1873,19 +1871,16 @@ void MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg)
 	memcpy(gps_inject_data_topic.data, gps_rtcm_data_msg.data,
 	       math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_rtcm_data_msg.len));
 
-	orb_advert_t &pub = _gps_inject_data_pub[_gps_inject_data_next_idx];
+	orb_advert_t &pub = _gps_inject_data_pub;
 
 	if (pub == nullptr) {
-		int idx = _gps_inject_data_next_idx;
-		pub = orb_advertise_multi(ORB_ID(gps_inject_data), &gps_inject_data_topic,
-					  &idx, ORB_PRIO_DEFAULT);
+		pub = orb_advertise_queue(ORB_ID(gps_inject_data), &gps_inject_data_topic,
+					  _gps_inject_data_queue_size);
 
 	} else {
 		orb_publish(ORB_ID(gps_inject_data), pub, &gps_inject_data_topic);
 	}
 
-	_gps_inject_data_next_idx = (_gps_inject_data_next_idx + 1) % _gps_inject_data_pub_size;
-
 }
 
 void
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index b2521a6813..c271810ea6 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -218,9 +218,8 @@ private:
 	orb_advert_t _time_offset_pub;
 	orb_advert_t _follow_target_pub;
 	orb_advert_t _transponder_report_pub;
-	static const int _gps_inject_data_pub_size = 4;
-	orb_advert_t _gps_inject_data_pub[_gps_inject_data_pub_size];
-	int _gps_inject_data_next_idx = 0;
+	static const int _gps_inject_data_queue_size = 6;
+	orb_advert_t _gps_inject_data_pub;
 	int _control_mode_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;