diff --git a/src/modules/mavlink/mavlink_events.cpp b/src/modules/mavlink/mavlink_events.cpp
index 016e8e6bb8..2e27646a3e 100644
--- a/src/modules/mavlink/mavlink_events.cpp
+++ b/src/modules/mavlink/mavlink_events.cpp
@@ -138,7 +138,9 @@ void SendProtocol::update(const hrt_abstime &now)
 
 	while (_latest_sequence != buffer_sequence) {
 		// only send if enough tx buffer space available
-		if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_EVENT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
+		if ((_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_EVENT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)
+		    || _mavlink.over_data_rate()) {
+
 			break;
 		}
 
@@ -218,7 +220,9 @@ void SendProtocol::on_gcs_connected()
 void SendProtocol::send_current_sequence(const hrt_abstime &now)
 {
 	// only send if enough tx buffer space available
-	if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
+	if ((_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)
+	    || _mavlink.over_data_rate()) {
+
 		return;
 	}
 
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 5e62c0ee9e..a36729d317 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -1064,7 +1064,8 @@ void MavlinkFTP::send()
 	unsigned max_bytes_to_send = _mavlink->get_free_tx_buf();
 	PX4_DEBUG("MavlinkFTP::send max_bytes_to_send(%u) get_free_tx_buf(%u)", max_bytes_to_send, _mavlink->get_free_tx_buf());
 
-	if (max_bytes_to_send < get_size()) {
+	// Skip send if not enough room
+	if ((max_bytes_to_send < get_size()) || _mavlink->over_data_rate()) {
 		return;
 	}
 
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 7fb34a583b..e2a9da82e1 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -731,25 +731,11 @@ void Mavlink::send_start(int length)
 {
 	pthread_mutex_lock(&_send_mutex);
 	_last_write_try_time = hrt_absolute_time();
-
-	// check if there is space in the buffer
-	if (length > (int)get_free_tx_buf()) {
-		// not enough space in buffer to send
-		count_txerrbytes(length);
-
-		_tstatus.tx_buffer_overruns++;
-
-		// prevent writes
-		_tx_buffer_low = true;
-
-	} else {
-		_tx_buffer_low = false;
-	}
 }
 
 void Mavlink::send_finish()
 {
-	if (_tx_buffer_low || (_buf_fill == 0)) {
+	if (_buf_fill == 0) {
 		pthread_mutex_unlock(&_send_mutex);
 		return;
 	}
@@ -806,6 +792,8 @@ void Mavlink::send_finish()
 		count_txbytes(_buf_fill);
 		_last_write_success_time = _last_write_try_time;
 
+		_over_data_rate.store(_bytes_tx > _datarate * hrt_elapsed_time(&_bytes_timestamp) * 1e-6f);
+
 	} else {
 		count_txerrbytes(_buf_fill);
 	}
@@ -817,14 +805,12 @@ void Mavlink::send_finish()
 
 void Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 {
-	if (!_tx_buffer_low) {
-		if (_buf_fill + packet_len < sizeof(_buf)) {
-			memcpy(&_buf[_buf_fill], buf, packet_len);
-			_buf_fill += packet_len;
+	if (_buf_fill + packet_len < sizeof(_buf)) {
+		memcpy(&_buf[_buf_fill], buf, packet_len);
+		_buf_fill += packet_len;
 
-		} else {
-			perf_count(_send_byte_error_perf);
-		}
+	} else {
+		perf_count(_send_byte_error_perf);
 	}
 }
 
@@ -1155,10 +1141,12 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 			if (interval != 0) {
 				/* set new interval */
 				stream->set_interval(interval);
+				_force_rate_mult_update = true;
 
 			} else {
 				/* delete stream */
 				_streams.deleteNode(stream);
+				_force_rate_mult_update = true;
 				return OK; // must finish with loop after node is deleted
 			}
 
@@ -1173,6 +1161,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 	if (stream != nullptr) {
 		stream->set_interval(interval);
 		_streams.add(stream);
+		_force_rate_mult_update = true;
 
 		return OK;
 	}
@@ -1364,23 +1353,39 @@ Mavlink::close_shell()
 void
 Mavlink::update_rate_mult()
 {
+	int min_interval = INT32_MAX;
+
 	float const_rate = 0.0f;
 	float rate = 0.0f;
 
 	/* scale down rates if their theoretical bandwidth is exceeding the link bandwidth */
 	for (const auto &stream : _streams) {
-		if (stream->const_rate()) {
-			const_rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
+		const int interval = stream->get_interval();
 
-		} else {
-			rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
+		if (interval > 0) {
+			if (stream->const_rate()) {
+				const_rate += stream->get_size_avg() * 1e6f / interval;
+
+			} else {
+				rate += stream->get_size_avg() * 1e6f / interval;
+			}
+
+			if (interval < min_interval) {
+				min_interval = interval;
+			}
 		}
 	}
 
+	// update main loop delay
+	_main_loop_delay = math::constrain(min_interval / 3, MAVLINK_MIN_INTERVAL, MAVLINK_MAX_INTERVAL);
+
 	float mavlink_ulog_streaming_rate_inv = 1.0f;
 
 	if (_mavlink_ulog) {
 		mavlink_ulog_streaming_rate_inv = 1.0f - _mavlink_ulog->current_data_rate();
+
+		// ensure update is at least twice the default logger rate
+		_main_loop_delay = math::min(_main_loop_delay, 3500 / 2); // TODO: poll ulog_stream
 	}
 
 	/* scale up and down as the link permits */
@@ -1416,17 +1421,21 @@ Mavlink::update_rate_mult()
 		hardware_mult *= _radio_status_mult;
 	}
 
+	// reset
+	_radio_status_changed = false;
+
 	pthread_mutex_unlock(&_radio_status_mutex);
 
 	if (log_radio_timeout) {
 		PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id);
 	}
 
-	/* pick the minimum from bandwidth mult and hardware mult as limit */
-	_rate_mult = fminf(bandwidth_mult, hardware_mult);
+	// pick the minimum from bandwidth mult and hardware mult as limit
+	// ensure the rate multiplier never drops below 5% so that something is always sent
+	float rate_mult = math::constrain(fminf(bandwidth_mult, hardware_mult), 0.05f, 1.0f);
 
-	/* ensure the rate multiplier never drops below 5% so that something is always sent */
-	_rate_mult = math::constrain(_rate_mult, 0.05f, 1.0f);
+	_rate_mult = rate_mult;
+	_rate_div = 1.f / rate_mult;
 }
 
 void
@@ -1438,6 +1447,9 @@ Mavlink::update_radio_status(const radio_status_s &radio_status)
 
 	if (_use_software_mav_throttling) {
 
+		const bool radio_status_critical = _radio_status_critical;
+		const float radio_status_mult = _radio_status_mult;
+
 		/* check hardware limits */
 		_radio_status_critical = (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE);
 
@@ -1456,6 +1468,10 @@ Mavlink::update_radio_status(const radio_status_s &radio_status)
 
 		/* Constrain radio status multiplier between 1% and 100% to allow recovery */
 		_radio_status_mult = math::constrain(_radio_status_mult, 0.01f, 1.0f);
+
+		if ((radio_status_critical != _radio_status_critical) || (fabsf(radio_status_mult - _radio_status_mult) > 0.01f)) {
+			_radio_status_changed = true;
+		}
 	}
 
 	pthread_mutex_unlock(&_radio_status_mutex);
@@ -1821,6 +1837,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		break;
 	}
 
+	_force_rate_mult_update = true;
+
 	if (configure_single_stream && !stream_configured && strcmp(configure_single_stream, "HEARTBEAT") != 0) {
 		// stream was not found, assume it is disabled by default
 		return configure_stream(configure_single_stream, 0.0f);
@@ -2190,19 +2208,6 @@ Mavlink::task_main(int argc, char *argv[])
 		PX4_ERR("configure_streams_to_default() failed");
 	}
 
-	/* set main loop delay depending on data rate to minimize CPU overhead */
-	_main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate;
-
-	/* hard limit to 1000 Hz at max */
-	if (_main_loop_delay < MAVLINK_MIN_INTERVAL) {
-		_main_loop_delay = MAVLINK_MIN_INTERVAL;
-	}
-
-	/* hard limit to 100 Hz at least */
-	if (_main_loop_delay > MAVLINK_MAX_INTERVAL) {
-		_main_loop_delay = MAVLINK_MAX_INTERVAL;
-	}
-
 	/* open the UART device after setting the instance, as it might block */
 	if (get_protocol() == Protocol::SERIAL) {
 		_uart_fd = mavlink_open_uart(_baudrate, _device_name, _flow_control);
@@ -2234,6 +2239,7 @@ Mavlink::task_main(int argc, char *argv[])
 	uint16_t event_sequence_offset = 0; // offset to account for skipped events, not sent via MAVLink
 
 	_mavlink_start_time = hrt_absolute_time();
+	_bytes_timestamp = _mavlink_start_time;
 
 	while (!should_exit()) {
 		/* main loop */
@@ -2249,7 +2255,12 @@ Mavlink::task_main(int argc, char *argv[])
 
 		const hrt_abstime t = hrt_absolute_time();
 
-		update_rate_mult();
+		// update rate multiplier periodically, or immediately if radio status or intervals have changed
+		if (_radio_status_changed || _force_rate_mult_update || (t >= _rate_multi_update_last + 1_s)) {
+			update_rate_mult();
+			_rate_multi_update_last = t;
+			_force_rate_mult_update = false;
+		}
 
 		// check for parameter updates
 		if (_parameter_update_sub.updated()) {
@@ -2436,19 +2447,6 @@ Mavlink::task_main(int argc, char *argv[])
 		/* update streams */
 		for (const auto &stream : _streams) {
 			stream->update(t);
-
-			if (!_first_heartbeat_sent) {
-				if (_mode == MAVLINK_MODE_IRIDIUM) {
-					if (stream->get_id() == MAVLINK_MSG_ID_HIGH_LATENCY2) {
-						_first_heartbeat_sent = stream->first_message_sent();
-					}
-
-				} else {
-					if (stream->get_id() == MAVLINK_MSG_ID_HEARTBEAT) {
-						_first_heartbeat_sent = stream->first_message_sent();
-					}
-				}
-			}
 		}
 
 		/* check for ulog streaming messages */
@@ -2551,21 +2549,25 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 		}
 
-		/* update TX/RX rates*/
-		if (t > _bytes_timestamp + 1_s) {
-			if (_bytes_timestamp != 0) {
-				const float dt = (t - _bytes_timestamp) * 1e-6f;
+		// update TX/RX rates
+		if (t >= _bytes_timestamp + 1_s) {
+			pthread_mutex_lock(&_send_mutex);
 
-				_tstatus.tx_rate_avg = _bytes_tx / dt;
-				_tstatus.tx_error_rate_avg = _bytes_txerr / dt;
-				_tstatus.rx_rate_avg = _bytes_rx / dt;
+			const float dt_inv = 1e6f / (t - _bytes_timestamp);
 
-				_bytes_tx = 0;
-				_bytes_txerr = 0;
-				_bytes_rx = 0;
-			}
+			_tstatus.tx_rate_avg = _bytes_tx * dt_inv;
+			_tstatus.tx_error_rate_avg = _bytes_txerr * dt_inv;
+			_tstatus.rx_rate_avg = _bytes_rx * dt_inv;
 
-			_bytes_timestamp = t;
+			_bytes_tx = 0;
+			_bytes_txerr = 0;
+			_bytes_rx = 0;
+
+			_bytes_timestamp = _last_write_try_time;
+
+			_force_rate_mult_update = true;
+
+			pthread_mutex_unlock(&_send_mutex);
 		}
 
 		// publish status at 1 Hz, or sooner if HEARTBEAT has updated
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 12b078e97b..98fe6cd80f 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -405,6 +405,7 @@ public:
 	List<MavlinkStream *> &get_streams() { return _streams; }
 
 	float			get_rate_mult() const { return _rate_mult; }
+	float			get_rate_div() const { return _rate_div; }
 
 	float			get_baudrate() { return _baudrate; }
 
@@ -415,6 +416,19 @@ public:
 	bool			get_wait_to_transmit() { return _wait_to_transmit; }
 	bool			should_transmit() { return (_transmitting_enabled && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); }
 
+	bool canTransmit(size_t size_bytes)
+	{
+		if (size_bytes <= get_free_tx_buf()) {
+			if ((_bytes_tx + size_bytes) <= _datarate * hrt_elapsed_time(&_bytes_timestamp) * 1e-6f) {
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	bool 			over_data_rate() { return _over_data_rate.load(); }
+
 	bool			message_buffer_write(const void *ptr, int size);
 
 	void			lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
@@ -519,7 +533,9 @@ public:
 
 	static hrt_abstime &get_first_start_time() { return _first_start_time; }
 
-	bool radio_status_critical() const { return _radio_status_critical; }
+	bool radio_status_critical() const { return _radio_status_available && _radio_status_critical; }
+
+	void set_first_heartbeat_sent() { _first_heartbeat_sent = true; }
 
 private:
 	MavlinkReceiver 	_receiver;
@@ -548,9 +564,8 @@ private:
 
 	static bool		_boot_complete;
 
-	static constexpr int	MAVLINK_MIN_INTERVAL{1500};
-	static constexpr int	MAVLINK_MAX_INTERVAL{10000};
-	static constexpr float	MAVLINK_MIN_MULTIPLIER{0.0005f};
+	static constexpr int	MAVLINK_MIN_INTERVAL{1000};  // 1000 Hz
+	static constexpr int	MAVLINK_MAX_INTERVAL{20000}; //   50 Hz
 
 	mavlink_message_t	_mavlink_buffer {};
 	mavlink_status_t	_mavlink_status {};
@@ -563,7 +578,7 @@ private:
 
 	px4::atomic_bool	_should_check_events{false};    /**< Events subscription: only one MAVLink instance should check */
 
-	unsigned		_main_loop_delay{1000};	/**< mainloop delay, depends on data rate */
+	int		_main_loop_delay{MAVLINK_MAX_INTERVAL};	/**< mainloop delay, depends on data rate */
 
 	List<MavlinkStream *>		_streams;
 
@@ -585,9 +600,11 @@ private:
 	int			_baudrate{57600};
 	int			_datarate{1000};		///< data rate for normal streams (attitude, position, etc.)
 	float			_rate_mult{1.0f};
+	float			_rate_div{1.0f};
 
 	bool			_radio_status_available{false};
 	bool			_radio_status_critical{false};
+	bool			_radio_status_changed{false};
 	float			_radio_status_mult{1.0f};
 
 	/**
@@ -616,6 +633,11 @@ private:
 	unsigned		_bytes_rx{0};
 	hrt_abstime		_bytes_timestamp{0};
 
+	px4::atomic_bool        _over_data_rate{false};
+
+	hrt_abstime             _rate_multi_update_last{0};
+	bool			_force_rate_mult_update{true};
+
 #if defined(MAVLINK_UDP)
 	BROADCAST_MODE		_mav_broadcast {BROADCAST_MODE_OFF};
 
@@ -635,8 +657,6 @@ private:
 	uint8_t			_buf[MAVLINK_MAX_PACKET_LEN] {};
 	unsigned		_buf_fill{0};
 
-	bool			_tx_buffer_low{false};
-
 	const char 		*_interface_name{nullptr};
 
 	int			_socket_fd{-1};
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index a63081f47a..4fc2f794c8 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -466,12 +466,7 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
 void
 MavlinkMissionManager::send()
 {
-	// do not send anything over high latency communication
-	if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
-		return;
-	}
-
-	mission_result_s mission_result{};
+	mission_result_s mission_result;
 
 	if (_mission_result_sub.update(&mission_result)) {
 
@@ -1744,11 +1739,6 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 
 void MavlinkMissionManager::check_active_mission()
 {
-	// do not send anything over high latency communication
-	if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
-		return;
-	}
-
 	if (!(_my_dataman_id == _dataman_id)) {
 		PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating");
 
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index 1ffafb796e..501fb54733 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -66,6 +66,9 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 			mavlink_param_request_list_t req_list;
 			mavlink_msg_param_request_list_decode(msg, &req_list);
 
+			PX4_DEBUG("PARAM_REQUEST_LIST target_system: %d, target_component: %d", req_list.target_system,
+				  req_list.target_component);
+
 			if (req_list.target_system == mavlink_system.sysid &&
 			    (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
 				if (_send_all_index < 0) {
@@ -96,6 +99,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 			mavlink_param_set_t set;
 			mavlink_msg_param_set_decode(msg, &set);
 
+			PX4_DEBUG("PARAM_SET param_id:%s", set.param_id);
+
 			if (set.target_system == mavlink_system.sysid &&
 			    (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
 
@@ -162,6 +167,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 		}
 
 	case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
+			PX4_DEBUG("PARAM_REQUEST_READ");
 			/* request one parameter */
 			mavlink_param_request_read_t req_read;
 			mavlink_msg_param_request_read_decode(msg, &req_read);
@@ -183,6 +189,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 						strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
 						param_value.param_type = MAV_PARAM_TYPE_UINT32;
 						memcpy(&param_value.param_value, &hash, sizeof(hash));
+
+						PX4_DEBUG("sending PARAM_REQUEST_READ param_id:'%s', param_index:%d", param_value.param_id, param_value.param_index);
 						mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &param_value);
 
 					} else {
@@ -228,6 +236,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 		}
 
 	case MAVLINK_MSG_ID_PARAM_MAP_RC: {
+			PX4_DEBUG("PARAM_MAP_RC");
 			/* map a rc channel to a parameter */
 			mavlink_param_map_rc_t map_rc;
 			mavlink_msg_param_map_rc_decode(msg, &map_rc);
@@ -278,6 +287,10 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 void
 MavlinkParametersManager::send()
 {
+	if (_mavlink->over_data_rate() || _mavlink->radio_status_critical()) {
+		return;
+	}
+
 	if (!_first_send) {
 		// parameters QGC can't tolerate not finding (2020-11-11)
 		param_find("BAT_CRIT_THR");
@@ -308,21 +321,32 @@ MavlinkParametersManager::send()
 		_first_send = true;
 	}
 
-	int max_num_to_send;
+	int max_num_to_send = 3;
 
-	if (_mavlink->get_protocol() == Protocol::SERIAL && !_mavlink->is_usb_uart()) {
-		max_num_to_send = 3;
-
-	} else {
-		// speed up parameter loading via UDP or USB: try to send 20 at once
-		max_num_to_send = 20;
+	if ((_mavlink->get_protocol() != Protocol::SERIAL)
+	    || _mavlink->is_usb_uart()
+	    || ((_mavlink->get_protocol() == Protocol::SERIAL) && _mavlink->get_flow_control_enabled())
+	   ) {
+		// speed up parameter loading via UDP, USB, or serial with working flow control: try to send up to 30 at once (respecting max data rate)
+		max_num_to_send = 30;
 	}
 
-	int i = 0;
+	for (int i = 0; i < max_num_to_send; i++) {
+		// Send while burst is not exceeded, we still have buffer space and still something to send
+		bool transmit = _mavlink->get_free_tx_buf() >= get_size() && !_mavlink->radio_status_critical()
+				&& !_mavlink->over_data_rate();
 
-	// Send while burst is not exceeded, we still have buffer space and still something to send
-	while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical()
-	       && send_params()) {}
+		if (transmit) {
+			if (!send_params()) {
+				return;
+			}
+
+		} else {
+			PX4_DEBUG("MavlinkParametersManager::send() can't transmit, free TX buf: %d, over data rate: %d",
+				  _mavlink->get_free_tx_buf(), _mavlink->over_data_rate());
+			return;
+		}
+	}
 }
 
 bool
@@ -382,6 +406,7 @@ MavlinkParametersManager::send_untransmitted()
 				}
 			}
 		} while ((_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical()
+			 && !_mavlink->over_data_rate()
 			 && (_param_update_index < (int) param_count()));
 
 		// Flag work as done once all params have been sent
@@ -397,7 +422,7 @@ bool
 MavlinkParametersManager::send_uavcan()
 {
 	/* Send parameter values received from the UAVCAN topic */
-	uavcan_parameter_value_s value{};
+	uavcan_parameter_value_s value;
 
 	if (_uavcan_parameter_value_sub.update(&value)) {
 
@@ -470,6 +495,8 @@ MavlinkParametersManager::send_one()
 			strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
 			msg.param_type = MAV_PARAM_TYPE_UINT32;
 			memcpy(&msg.param_value, &hash, sizeof(hash));
+
+			PX4_DEBUG("sending PARAM_VALUE param_id:'%s', param_index:%d", msg.param_id, msg.param_index);
 			mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
 
 			/* after this we should start sending all params */
@@ -511,12 +538,7 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
 		return 1;
 	}
 
-	/* no free TX buf to send this param */
-	if (_mavlink->get_free_tx_buf() < MAVLINK_MSG_ID_PARAM_VALUE_LEN) {
-		return 1;
-	}
-
-	mavlink_param_value_t msg;
+	mavlink_param_value_t msg{};
 
 	/*
 	 * get param value, since MAVLink encodes float and int params in the same
@@ -579,6 +601,8 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
 
 	/* default component ID */
 	if (component_id < 0) {
+		PX4_DEBUG("sending PARAM_VALUE param_id:'%s', param_index:%d, param_count:%d", msg.param_id, msg.param_index,
+			  msg.param_count);
 		mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
 
 	} else {
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c66300c057..7bc83b2b2c 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -3085,9 +3085,6 @@ MavlinkReceiver::run()
 		px4_prctl(PR_SET_NAME, thread_name, px4_getpid());
 	}
 
-	// poll timeout in ms. Also defines the max update frequency of the mission & param manager, etc.
-	const int timeout = 10;
-
 #if defined(__PX4_POSIX)
 	/* 1500 is the Wifi MTU, so we make sure to fit a full packet */
 	uint8_t buf[1600 * 5];
@@ -3133,7 +3130,10 @@ MavlinkReceiver::run()
 			updateParams();
 		}
 
-		int ret = poll(&fds[0], 1, timeout);
+		// poll timeout in ms. Also defines the max update frequency of the mission & param manager, etc.
+		const int timeout_ms = 10;
+
+		int ret = poll(&fds[0], 1, timeout_ms);
 
 		if (ret > 0) {
 			if (_mavlink->get_protocol() == Protocol::SERIAL) {
@@ -3268,17 +3268,24 @@ MavlinkReceiver::run()
 
 		CheckHeartbeats(t);
 
-		if (t - last_send_update > timeout * 1000) {
-			_mission_manager.check_active_mission();
-			_mission_manager.send();
+		if (t - last_send_update > timeout_ms * 1000) {
 
-			_parameters_manager.send();
+			// do not send anything over high latency communication
+			if (_mavlink->get_mode() != Mavlink::MAVLINK_MODE_IRIDIUM) {
+
+				_mission_manager.check_active_mission();
+				_mission_manager.send();
+
+				_parameters_manager.send();
+
+				if (_mavlink->ftp_enabled()) {
+					_mavlink_ftp.send();
+				}
+
+				_mavlink_log_handler.send();
 
-			if (_mavlink->ftp_enabled()) {
-				_mavlink_ftp.send();
 			}
 
-			_mavlink_log_handler.send();
 			last_send_update = t;
 		}
 
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index 6b00fd4e49..edfae3e0f0 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -47,7 +47,6 @@
 MavlinkStream::MavlinkStream(Mavlink *mavlink) :
 	_mavlink(mavlink)
 {
-	_last_sent = hrt_absolute_time();
 }
 
 /**
@@ -58,71 +57,38 @@ MavlinkStream::update(const hrt_abstime &t)
 {
 	update_data();
 
-	// If the message has never been sent before we want
-	// to send it immediately and can return right away
-	if (_last_sent == 0) {
-		// this will give different messages on the same run a different
-		// initial timestamp which will help spacing them out
-		// on the link scheduling
-		if (send()) {
-			_last_sent = hrt_absolute_time();
-
-			if (!_first_message_sent) {
-				_first_message_sent = true;
-			}
-		}
+	int sz = get_size();
 
+	if ((sz <= 0) || (_interval == 0)) {
+		// We don't need to send anything if the size or interval is 0.
 		return 0;
-	}
 
-	// One of the previous iterations sent the update
-	// already before the deadline
-	if (_last_sent > t) {
-		return -1;
-	}
-
-	int64_t dt = t - _last_sent;
-	int interval = _interval;
-
-	if (!const_rate()) {
-		interval /= _mavlink->get_rate_mult();
-	}
-
-	// We don't need to send anything if the inverval is 0. send() will be called manually.
-	if (interval == 0) {
-		return 0;
-	}
-
-	const bool unlimited_rate = interval < 0;
-
-	// Send the message if it is due or
-	// if it will overrun the next scheduled send interval
-	// by 30% of the interval time. This helps to avoid
-	// sending a scheduled message on average slower than
-	// scheduled. Doing this at 50% would risk sending
-	// the message too often as the loop runtime of the app
-	// needs to be accounted for as well.
-	// This method is not theoretically optimal but a suitable
-	// stopgap as it hits its deadlines well (0.5 Hz, 50 Hz and 250 Hz)
-
-	if (unlimited_rate || (dt > (interval - (_mavlink->get_main_loop_delay() / 10) * 3))) {
-		// interval expired, send message
-
-		// If the interval is non-zero and dt is smaller than 1.5 times the interval
-		// do not use the actual time but increment at a fixed rate, so that processing delays do not
-		// distort the average rate. The check of the maximum interval is done to ensure that after a
-		// long time not sending anything, sending multiple messages in a short time is avoided.
-		if (send()) {
-			_last_sent = ((interval > 0) && ((int64_t)(1.5f * interval) > dt)) ? _last_sent + interval : t;
-
-			if (!_first_message_sent) {
-				_first_message_sent = true;
+	} else if ((_interval < 0) || (_last_sent == 0)) {
+		// if unlimited rate or message has never been sent before
+		// we want to send it immediately and can return right away
+		if (_mavlink->canTransmit(sz)) {
+			if (send()) {
+				_last_sent = t;
+				return 0;
 			}
 
-			return 0;
-
 		} else {
-			return -1;
+			_mavlink->count_txerrbytes(sz);
+		}
+
+	} else {
+		int interval = const_rate() ? _interval : (_interval * _mavlink->get_rate_div());
+
+		if (t >= _last_sent + interval) {
+			if (_mavlink->canTransmit(sz)) {
+				if (send()) {
+					_last_sent = math::max(_last_sent + interval, t - interval);
+					return 0;
+				}
+
+			} else {
+				_mavlink->count_txerrbytes(sz);
+			}
 		}
 	}
 
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 4bb4c111c3..9e99f42df6 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -114,13 +114,7 @@ public:
 	/**
 	 * @return true if the first message of this stream has been sent
 	 */
-	bool first_message_sent() const { return _first_message_sent; }
-
-	/**
-	 * Reset the time of last sent to 0. Can be used if a message over this
-	 * stream needs to be sent immediately.
-	 */
-	void reset_last_sent() { _last_sent = 0; }
+	bool first_message_sent() const { return _last_sent != 0; }
 
 protected:
 	Mavlink      *const _mavlink;
@@ -138,7 +132,6 @@ protected:
 
 private:
 	hrt_abstime _last_sent{0};
-	bool _first_message_sent{false};
 };
 
 
diff --git a/src/modules/mavlink/streams/ADSB_VEHICLE.hpp b/src/modules/mavlink/streams/ADSB_VEHICLE.hpp
index 56f2a94e42..389c9fe187 100644
--- a/src/modules/mavlink/streams/ADSB_VEHICLE.hpp
+++ b/src/modules/mavlink/streams/ADSB_VEHICLE.hpp
@@ -61,14 +61,12 @@ private:
 
 	bool send() override
 	{
-		bool sent = false;
-
 		transponder_report_s pos;
 
-		while ((_mavlink->get_free_tx_buf() >= get_size()) && _transponder_report_sub.update(&pos)) {
+		if (_transponder_report_sub.update(&pos)) {
 
 			if (!(pos.flags & transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE)) {
-				continue;
+				return false;
 			}
 
 			mavlink_adsb_vehicle_t msg{};
@@ -100,10 +98,10 @@ private:
 			if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_SQUAWK) { msg.flags |= ADSB_FLAGS_VALID_SQUAWK; }
 
 			mavlink_msg_adsb_vehicle_send_struct(_mavlink->get_channel(), &msg);
-			sent = true;
+			return true;
 		}
 
-		return sent;
+		return false;
 	}
 };
 
diff --git a/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp b/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp
index fede0f3c27..9a8a81d8d2 100644
--- a/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp
+++ b/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp
@@ -63,7 +63,7 @@ private:
 	{
 		camera_capture_s capture;
 
-		if ((_mavlink->get_free_tx_buf() >= get_size()) && _capture_sub.update(&capture)) {
+		if (_capture_sub.update(&capture)) {
 			mavlink_camera_image_captured_t msg{};
 
 			msg.time_boot_ms = capture.timestamp / 1000;
diff --git a/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp b/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp
index e64a772d2d..f4c091fb39 100644
--- a/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp
+++ b/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp
@@ -76,7 +76,7 @@ private:
 	{
 		camera_trigger_s camera_trigger;
 
-		if ((_mavlink->get_free_tx_buf() >= get_size()) && _camera_trigger_sub.update(&camera_trigger)) {
+		if (_camera_trigger_sub.update(&camera_trigger)) {
 			/* ensure that only active trigger events are sent and ignore camera capture feedback messages*/
 			if (camera_trigger.timestamp > 0 && !camera_trigger.feedback) {
 				mavlink_camera_trigger_t msg{};
diff --git a/src/modules/mavlink/streams/COLLISION.hpp b/src/modules/mavlink/streams/COLLISION.hpp
index a3469c62d9..fb8586d2ec 100644
--- a/src/modules/mavlink/streams/COLLISION.hpp
+++ b/src/modules/mavlink/streams/COLLISION.hpp
@@ -62,7 +62,7 @@ private:
 		collision_report_s report;
 		bool sent = false;
 
-		while ((_mavlink->get_free_tx_buf() >= get_size()) && _collision_sub.update(&report)) {
+		if (_collision_sub.update(&report)) {
 			mavlink_collision_t msg = {};
 
 			msg.src = report.src;
diff --git a/src/modules/mavlink/streams/COMMAND_LONG.hpp b/src/modules/mavlink/streams/COMMAND_LONG.hpp
index f216c34810..8f1a4a2b8c 100644
--- a/src/modules/mavlink/streams/COMMAND_LONG.hpp
+++ b/src/modules/mavlink/streams/COMMAND_LONG.hpp
@@ -60,38 +60,32 @@ private:
 	bool send() override
 	{
 		bool sent = false;
+		const unsigned last_generation = _vehicle_command_sub.get_last_generation();
+		vehicle_command_s cmd;
 
-		static constexpr size_t COMMAND_LONG_SIZE = MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		if (_vehicle_command_sub.update(&cmd)) {
+			if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
+				PX4_ERR("COMMAND_LONG vehicle_command lost, generation %d -> %d", last_generation,
+					_vehicle_command_sub.get_last_generation());
+			}
 
-		while ((_mavlink->get_free_tx_buf() >= COMMAND_LONG_SIZE) && _vehicle_command_sub.updated()) {
+			// mavlink mavlink commands are <= UINT16_MAX
+			const bool px4_internal_cmd = (cmd.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START);
 
-			const unsigned last_generation = _vehicle_command_sub.get_last_generation();
-			vehicle_command_s cmd;
+			// internal commands
+			const bool target_system_internal = (cmd.target_system == _mavlink->get_system_id())
+							    && (cmd.target_component == _mavlink->get_component_id())
+							    && (cmd.source_system == cmd.target_system)
+							    && (cmd.source_component == cmd.target_component);
 
-			if (_vehicle_command_sub.update(&cmd)) {
-				if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
-					PX4_ERR("COMMAND_LONG vehicle_command lost, generation %d -> %d", last_generation,
-						_vehicle_command_sub.get_last_generation());
-				}
+			if (!cmd.from_external && !px4_internal_cmd && !target_system_internal) {
+				PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
 
-				// mavlink mavlink commands are <= UINT16_MAX
-				const bool px4_internal_cmd = (cmd.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START);
+				MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
+				sent = true;
 
-				// internal commands
-				const bool target_system_internal = (cmd.target_system == _mavlink->get_system_id())
-								    && (cmd.target_component == _mavlink->get_component_id())
-								    && (cmd.source_system == cmd.target_system)
-								    && (cmd.source_component == cmd.target_component);
-
-				if (!cmd.from_external && !px4_internal_cmd && !target_system_internal) {
-					PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
-
-					MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
-					sent = true;
-
-				} else {
-					PX4_DEBUG("not forwarding command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
-				}
+			} else {
+				PX4_DEBUG("not forwarding command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
 			}
 		}
 
diff --git a/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp b/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp
index 8730d2e881..355e3a8480 100644
--- a/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp
+++ b/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp
@@ -60,9 +60,8 @@ private:
 	bool send() override
 	{
 		gps_inject_data_s gps_inject_data;
-		bool sent = false;
 
-		while ((_mavlink->get_free_tx_buf() >= get_size()) && _gps_inject_data_sub.update(&gps_inject_data)) {
+		if (_gps_inject_data_sub.update(&gps_inject_data)) {
 			mavlink_gps_rtcm_data_t msg{};
 
 			msg.len = gps_inject_data.len;
@@ -71,10 +70,10 @@ private:
 
 			mavlink_msg_gps_rtcm_data_send_struct(_mavlink->get_channel(), &msg);
 
-			sent = true;
+			return true;
 		}
 
-		return sent;
+		return false;
 	}
 };
 
diff --git a/src/modules/mavlink/streams/HEARTBEAT.hpp b/src/modules/mavlink/streams/HEARTBEAT.hpp
index 2e0aefb7ff..eb379ae2d2 100644
--- a/src/modules/mavlink/streams/HEARTBEAT.hpp
+++ b/src/modules/mavlink/streams/HEARTBEAT.hpp
@@ -67,85 +67,83 @@ private:
 
 	bool send() override
 	{
-		if (_mavlink->get_free_tx_buf() >= get_size()) {
-			// always send the heartbeat, independent of the update status of the topics
-			vehicle_status_s vehicle_status{};
-			_vehicle_status_sub.copy(&vehicle_status);
+		// always send the heartbeat, independent of the update status of the topics
+		vehicle_status_s vehicle_status{};
+		_vehicle_status_sub.copy(&vehicle_status);
 
-			vehicle_status_flags_s vehicle_status_flags{};
-			_vehicle_status_flags_sub.copy(&vehicle_status_flags);
+		vehicle_status_flags_s vehicle_status_flags{};
+		_vehicle_status_flags_sub.copy(&vehicle_status_flags);
 
-			vehicle_control_mode_s vehicle_control_mode{};
-			_vehicle_control_mode_sub.copy(&vehicle_control_mode);
+		vehicle_control_mode_s vehicle_control_mode{};
+		_vehicle_control_mode_sub.copy(&vehicle_control_mode);
 
-			actuator_armed_s actuator_armed{};
-			_acturator_armed_sub.copy(&actuator_armed);
+		actuator_armed_s actuator_armed{};
+		_acturator_armed_sub.copy(&actuator_armed);
 
-			// uint8_t base_mode (MAV_MODE_FLAG) - System mode bitmap.
-			uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+		// uint8_t base_mode (MAV_MODE_FLAG) - System mode bitmap.
+		uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
 
-			if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-				base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-			}
-
-			if (vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON) {
-				base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
-			}
-
-			if (vehicle_control_mode.flag_control_manual_enabled) {
-				base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-			}
-
-			if (vehicle_control_mode.flag_control_attitude_enabled) {
-				base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-			}
-
-			if (vehicle_control_mode.flag_control_auto_enabled) {
-				base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-			}
-
-
-			// uint32_t custom_mode - A bitfield for use for autopilot-specific flags
-			union px4_custom_mode custom_mode {get_px4_custom_mode(vehicle_status.nav_state)};
-
-
-			// uint8_t system_status (MAV_STATE) - System status flag.
-			uint8_t system_status = MAV_STATE_UNINIT;
-
-			switch (vehicle_status.arming_state) {
-			case vehicle_status_s::ARMING_STATE_ARMED:
-				system_status = vehicle_status.failsafe ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE;
-				break;
-
-			case vehicle_status_s::ARMING_STATE_STANDBY:
-				system_status = MAV_STATE_STANDBY;
-				break;
-
-			case vehicle_status_s::ARMING_STATE_SHUTDOWN:
-				system_status = MAV_STATE_POWEROFF;
-				break;
-			}
-
-			// system_status overrides
-			if (actuator_armed.force_failsafe || actuator_armed.lockdown || actuator_armed.manual_lockdown
-			    || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) {
-				system_status = MAV_STATE_FLIGHT_TERMINATION;
-
-			} else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) {
-				system_status = MAV_STATE_EMERGENCY;
-
-			} else if (vehicle_status_flags.condition_calibration_enabled) {
-				system_status = MAV_STATE_CALIBRATING;
-			}
-
-
-			mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4,
-						   base_mode, custom_mode.data, system_status);
-
-			return true;
+		if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+			base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 		}
 
-		return false;
+		if (vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON) {
+			base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+		}
+
+		if (vehicle_control_mode.flag_control_manual_enabled) {
+			base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+		}
+
+		if (vehicle_control_mode.flag_control_attitude_enabled) {
+			base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
+		}
+
+		if (vehicle_control_mode.flag_control_auto_enabled) {
+			base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		}
+
+
+		// uint32_t custom_mode - A bitfield for use for autopilot-specific flags
+		union px4_custom_mode custom_mode {get_px4_custom_mode(vehicle_status.nav_state)};
+
+
+		// uint8_t system_status (MAV_STATE) - System status flag.
+		uint8_t system_status = MAV_STATE_UNINIT;
+
+		switch (vehicle_status.arming_state) {
+		case vehicle_status_s::ARMING_STATE_ARMED:
+			system_status = vehicle_status.failsafe ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE;
+			break;
+
+		case vehicle_status_s::ARMING_STATE_STANDBY:
+			system_status = MAV_STATE_STANDBY;
+			break;
+
+		case vehicle_status_s::ARMING_STATE_SHUTDOWN:
+			system_status = MAV_STATE_POWEROFF;
+			break;
+		}
+
+		// system_status overrides
+		if (actuator_armed.force_failsafe || actuator_armed.lockdown || actuator_armed.manual_lockdown
+		    || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) {
+			system_status = MAV_STATE_FLIGHT_TERMINATION;
+
+		} else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) {
+			system_status = MAV_STATE_EMERGENCY;
+
+		} else if (vehicle_status_flags.condition_calibration_enabled) {
+			system_status = MAV_STATE_CALIBRATING;
+		}
+
+
+		mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4,
+					   base_mode, custom_mode.data, system_status);
+
+		_mavlink->set_first_heartbeat_sent();
+
+		return true;
 	}
 };
 
diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp
index c52127f1f7..d587c0e872 100644
--- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp
+++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp
@@ -87,7 +87,6 @@ private:
 		_throttle(SimpleAnalyzer::AVERAGE),
 		_windspeed(SimpleAnalyzer::AVERAGE)
 	{
-		reset_last_sent();
 	}
 
 	struct PerBatteryData {
@@ -203,6 +202,10 @@ private:
 				mavlink_msg_high_latency2_send_struct(_mavlink->get_channel(), &msg);
 			}
 
+			if (updated) {
+				_mavlink->set_first_heartbeat_sent();
+			}
+
 			return updated;
 
 		} else {
diff --git a/src/modules/mavlink/streams/LINK_NODE_STATUS.hpp b/src/modules/mavlink/streams/LINK_NODE_STATUS.hpp
index 774a4f9b91..694f4b3571 100644
--- a/src/modules/mavlink/streams/LINK_NODE_STATUS.hpp
+++ b/src/modules/mavlink/streams/LINK_NODE_STATUS.hpp
@@ -55,29 +55,25 @@ private:
 
 	bool send() override
 	{
-		if (_mavlink->get_free_tx_buf() >= get_size()) {
-			mavlink_link_node_status_t link_node_status{};
+		mavlink_link_node_status_t link_node_status{};
 
-			const telemetry_status_s &tstatus = _mavlink->telemetry_status();
-			link_node_status.tx_buf = 0; // % TODO
-			link_node_status.rx_buf = 0; // % TODO
-			link_node_status.tx_rate = tstatus.tx_rate_avg;
-			link_node_status.rx_rate = tstatus.rx_rate_avg;
-			link_node_status.rx_parse_err = tstatus.rx_parse_errors;
-			link_node_status.tx_overflows = tstatus.tx_buffer_overruns;
-			link_node_status.rx_overflows = tstatus.rx_buffer_overruns;
-			link_node_status.messages_sent = tstatus.tx_message_count;
-			link_node_status.messages_received = tstatus.rx_message_count;
-			link_node_status.messages_lost = tstatus.rx_message_lost_count;
+		const telemetry_status_s &tstatus = _mavlink->telemetry_status();
+		link_node_status.tx_buf = 0; // % TODO
+		link_node_status.rx_buf = 0; // % TODO
+		link_node_status.tx_rate = tstatus.tx_rate_avg;
+		link_node_status.rx_rate = tstatus.rx_rate_avg;
+		link_node_status.rx_parse_err = tstatus.rx_parse_errors;
+		link_node_status.tx_overflows = tstatus.tx_buffer_overruns;
+		link_node_status.rx_overflows = tstatus.rx_buffer_overruns;
+		link_node_status.messages_sent = tstatus.tx_message_count;
+		link_node_status.messages_received = tstatus.rx_message_count;
+		link_node_status.messages_lost = tstatus.rx_message_lost_count;
 
-			link_node_status.timestamp = hrt_absolute_time();
+		link_node_status.timestamp = hrt_absolute_time();
 
-			mavlink_msg_link_node_status_send_struct(_mavlink->get_channel(), &link_node_status);
+		mavlink_msg_link_node_status_send_struct(_mavlink->get_channel(), &link_node_status);
 
-			return true;
-		}
-
-		return false;
+		return true;
 	}
 };
 
diff --git a/src/modules/mavlink/streams/STATUSTEXT.hpp b/src/modules/mavlink/streams/STATUSTEXT.hpp
index 0ee6e45543..fdb902c6e4 100644
--- a/src/modules/mavlink/streams/STATUSTEXT.hpp
+++ b/src/modules/mavlink/streams/STATUSTEXT.hpp
@@ -69,56 +69,53 @@ private:
 	bool send() override
 	{
 		if (_mavlink->is_connected()) {
-			while (_mavlink_log_sub.updated() && (_mavlink->get_free_tx_buf() >= get_size())) {
+			const unsigned last_generation = _mavlink_log_sub.get_last_generation();
 
-				const unsigned last_generation = _mavlink_log_sub.get_last_generation();
+			mavlink_log_s mavlink_log;
 
-				mavlink_log_s mavlink_log;
+			if (_mavlink_log_sub.update(&mavlink_log)) {
+				// don't send stale messages
+				if (hrt_elapsed_time(&mavlink_log.timestamp) < 2_s) {
 
-				if (_mavlink_log_sub.update(&mavlink_log)) {
-					// don't send stale messages
-					if (hrt_elapsed_time(&mavlink_log.timestamp) < 2_s) {
-
-						if (_mavlink_log_sub.get_last_generation() != (last_generation + 1)) {
-							perf_count(_missed_msg_count_perf);
-							PX4_DEBUG("channel %d has missed %d mavlink log messages", _mavlink->get_channel(),
-								  perf_event_count(_missed_msg_count_perf));
-						}
-
-						mavlink_statustext_t msg{};
-						const char *text = mavlink_log.text;
-						constexpr unsigned max_chunk_size = sizeof(msg.text);
-						msg.severity = mavlink_log.severity;
-						msg.chunk_seq = 0;
-						msg.id = _id++;
-						unsigned text_size;
-
-						while ((text_size = strlen(text)) > 0) {
-							unsigned chunk_size = math::min(text_size, max_chunk_size);
-
-							if (chunk_size < max_chunk_size) {
-								memcpy(&msg.text[0], &text[0], chunk_size);
-								// pad with zeros
-								memset(&msg.text[0] + chunk_size, 0, max_chunk_size - chunk_size);
-
-							} else {
-								memcpy(&msg.text[0], &text[0], chunk_size);
-							}
-
-							mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
-
-							if (text_size <= max_chunk_size) {
-								break;
-
-							} else {
-								text += max_chunk_size;
-							}
-
-							msg.chunk_seq += 1;
-						}
-
-						return true;
+					if (_mavlink_log_sub.get_last_generation() != (last_generation + 1)) {
+						perf_count(_missed_msg_count_perf);
+						PX4_DEBUG("channel %d has missed %d mavlink log messages", _mavlink->get_channel(),
+							  perf_event_count(_missed_msg_count_perf));
 					}
+
+					mavlink_statustext_t msg{};
+					const char *text = mavlink_log.text;
+					constexpr unsigned max_chunk_size = sizeof(msg.text);
+					msg.severity = mavlink_log.severity;
+					msg.chunk_seq = 0;
+					msg.id = _id++;
+					unsigned text_size;
+
+					while ((text_size = strlen(text)) > 0) {
+						unsigned chunk_size = math::min(text_size, max_chunk_size);
+
+						if (chunk_size < max_chunk_size) {
+							memcpy(&msg.text[0], &text[0], chunk_size);
+							// pad with zeros
+							memset(&msg.text[0] + chunk_size, 0, max_chunk_size - chunk_size);
+
+						} else {
+							memcpy(&msg.text[0], &text[0], chunk_size);
+						}
+
+						mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
+
+						if (text_size <= max_chunk_size) {
+							break;
+
+						} else {
+							text += max_chunk_size;
+						}
+
+						msg.chunk_seq += 1;
+					}
+
+					return true;
 				}
 			}
 		}