[DO NOT MERGE] LMC: Mavlink ULog rate control support

src/modules/logger/logger.cpp

@@ -1935,7 +1935,17 @@ void Logger::write_add_logged_msg(LogType type, LoggerSubscription &subscription
 	_writer.set_need_reliable_transfer(true);
 #ifdef LOGGER_PARALLEL_LOGGING
 	thread_data_t *th_data = (thread_data_t *) pthread_getspecific(pthread_data_key);
-	write_message(type, &msg, msg_size, true, th_data->wait_for_ack);
+
+	// Do not use reliable for data logger side, because add_logged_msg triggers
+	//  ulog parser to switch from definitions to data parsing causing latter
+	//  format messages to be ignored.
+	if (th_data->wait_for_ack) {
+		write_message(type, &msg, msg_size, true, th_data->wait_for_ack);
+
+	} else {
+		write_message(type, &msg, msg_size, false, th_data->wait_for_ack);
+	}
+
 #else
 	write_message(type, &msg, msg_size);
 #endif

src/modules/mavlink/mavlink_main.h

@@ -494,7 +494,7 @@ class Mavlink final : public ModuleParams
 	{
 		if (_mavlink_ulog) { return; }
 
-		_mavlink_ulog = MavlinkULog::try_start(_datarate, 0.7f, target_system, target_component);
+		_mavlink_ulog = MavlinkULog::try_start(_datarate, _param_mav_ulog_max_rate.get(), target_system, target_component);
 	}
 
 	const events::SendProtocol &get_events_protocol() const { return _events; };
@@ -675,6 +675,7 @@ class Mavlink final : public ModuleParams
 		(ParamBool<px4::params::MAV_HASH_CHK_EN>) _param_mav_hash_chk_en,
 		(ParamBool<px4::params::MAV_HB_FORW_EN>) _param_mav_hb_forw_en,
 		(ParamInt<px4::params::MAV_RADIO_TOUT>)      _param_mav_radio_timeout,
+		(ParamFloat<px4::params::MAV_ULOG_MAX_RT>) _param_mav_ulog_max_rate,
 		(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl,
 		(ParamBool<px4::params::SYS_FAILURE_EN>) _param_sys_failure_injection_enabled
 	)

src/modules/mavlink/mavlink_params.c

@@ -155,3 +155,12 @@ PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1);
  * @max 250
  */
 PARAM_DEFINE_INT32(MAV_RADIO_TOUT, 5);
+
+/**
+ * Maximum rate in msg/100ms for ULog streaming
+ *
+  * @group MAVLink
+  * @min 0.0
+  * @max 1.0
+ */
+PARAM_DEFINE_FLOAT(MAV_ULOG_MAX_RT, 0.2f);

src/modules/mavlink/mavlink_ulog.cpp

@@ -180,33 +180,31 @@ int MavlinkULog::handle_update(mavlink_channel_t channel)
 			++_current_num_msgs;
 		}
 
-		if (_ulog_stream_acked_sub.updated()) {
+		if (check_for_updates && _ulog_stream_acked_sub.updated()) {
 			_ulog_stream_acked_sub.update();
 
-			if (check_for_updates) {
-				const ulog_stream_s &ulog_data = _ulog_stream_acked_sub.get();
+			const ulog_stream_s &ulog_data = _ulog_stream_acked_sub.get();
 
-				if (ulog_data.timestamp > 0) {
-					_sent_tries = 1;
-					_last_sent_time = hrt_absolute_time();
-					lock();
-					_wait_for_ack_sequence = ulog_data.msg_sequence;
-					_ack_received = false;
-					unlock();
-
-					mavlink_logging_data_acked_t msg;
-					msg.sequence = ulog_data.msg_sequence;
-					msg.length = ulog_data.length;
-					msg.first_message_offset = ulog_data.first_message_offset;
-					msg.target_system = _target_system;
-					msg.target_component = _target_component;
-					memcpy(msg.data, ulog_data.data, sizeof(msg.data));
-					mavlink_msg_logging_data_acked_send_struct(channel, &msg);
-
-				}
+			if (ulog_data.timestamp > 0) {
+				_sent_tries = 1;
+				_last_sent_time = hrt_absolute_time();
+				lock();
+				_wait_for_ack_sequence = ulog_data.msg_sequence;
+				_ack_received = false;
+				unlock();
+
+				mavlink_logging_data_acked_t msg;
+				msg.sequence = ulog_data.msg_sequence;
+				msg.length = ulog_data.length;
+				msg.first_message_offset = ulog_data.first_message_offset;
+				msg.target_system = _target_system;
+				msg.target_component = _target_component;
+				memcpy(msg.data, ulog_data.data, sizeof(msg.data));
+				mavlink_msg_logging_data_acked_send_struct(channel, &msg);
 
-				++_current_num_msgs;
 			}
+
+			++_current_num_msgs;
 		}
 	}