Implement code review feedback. Add get_id_static to MavlinkStream items. Add implementation of MAV_CMD_SET_MESSAGE_INTERVAL, MAV_CMD_GET_MESSAGE_INTERVAL. Add deprecation message to REQUEST_DATA_STREAM.

src/modules/mavlink/mavlink_main.cpp

@@ -1346,7 +1346,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 		}
 	}
 
-	if (interval == 0) {
+	if (interval <= 0) {
 		/* stream was not active and is requested to be disabled, do nothing */
 		return OK;
 	}

src/modules/mavlink/mavlink_messages.h

@@ -48,10 +48,12 @@ class StreamListItem {
 public:
 	MavlinkStream* (*new_instance)(Mavlink *mavlink);
 	const char* (*get_name)();
+	uint8_t (*get_id)();
 
-	StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) :
+	StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)(), uint8_t (*id)()) :
 		new_instance(inst),
-		get_name(name) {};
+		get_name(name),
+        get_id(id) {};
 
 	~StreamListItem() {};
 };

src/modules/mavlink/mavlink_receiver.cpp

@@ -355,6 +355,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		} else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
 			_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
 
+        } else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
+            set_message_interval((int)cmd_mavlink.param1, cmd_mavlink.param2);
+
+        } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
+            get_message_interval((int)cmd_mavlink.param1);
+
 		} else {
 
 			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
@@ -1368,37 +1374,75 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
 	mavlink_request_data_stream_t req;
 	mavlink_msg_request_data_stream_decode(msg, &req);
 
-	if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid
-	    && req.req_message_rate != 0) {
-		float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
+    PX4_WARN("REQUEST_DATA_STREAM is deprecated, please use SET_MESSAGE_INTERVAL instead");
 
-		MavlinkStream *stream;
-		LL_FOREACH(_mavlink->get_streams(), stream) {
-			if (req.req_stream_id == stream->get_id()) {
-				_mavlink->configure_stream_threadsafe(stream->get_name(), rate);
-                // if the command here was to unsubscribe then the stream will be deleted
-                // in which case we cannot continue this LL_FOREACH loop.  We have to return.
-                // Besides there shouldn't be more than one stream with the same id, right?
-				return;
-			}
-		}
+	if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid &&
+	        req.req_stream_id != 0)
+	{
+	    // compute interval in microseconds.
+        float interval = 0;
+	    int rate = req.req_message_rate;
+	    if (req.start_stop == 0 || rate == 0) {
+	        rate = -1;
+	    }
+	    else if (rate > 0) {
+	        interval = 1000000.0f / rate;
+	    }
 
-        // If we get here then the stream may have been deleted, in which case we need to recreate it.
-        // todo: we could simplify this code by adding a get_id_static (get_name_static) so that
-		// we could do both subscribe and unsubscribe using the streams_list.
+	    // todo: note it is kind of bogus to interpret the req_stream_id as a message id
+	    // but this is how it was.  MAVLINK defines separate MAV_DATA_STREAM ids which bundle
+	    // messages into groups, but that is all deprecated now anyway, and this code was
+	    // previously interpreting the req_stream_id as a message id, so we'll leave it that way.
+	    set_message_interval(req.req_stream_id, interval);
+	}
+}
+
+void
+MavlinkReceiver::set_message_interval(int msgId, float interval)
+{
+    // configure_stream wants a rate (msgs/second), so convert here.
+    float rate = 0;
+    if (interval < 0) {
+        // stop the stream.
+        rate = 0;
+    }
+    else if (interval > 0) {
+        rate = 1000000.0f / interval;
+    }
+    else {
+        // note: mavlink spec says rate == 0 is requesting a default rate but our streams
+        // don't publish a default rate so for now let's pick a default rate of zero.
+    }
+
+    // The interval between two messages is in microseconds.
+    // Set to -1 to disable and 0 to request default rate
+    if (msgId != 0)
+    {
         for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
-            MavlinkStream *new_instance = streams_list[i]->new_instance(_mavlink);
-            bool found = false;
-            if (req.req_stream_id == new_instance->get_id()) {
-                _mavlink->configure_stream_threadsafe(new_instance->get_name(), rate);
-                found = true;
-            }
-            delete new_instance;
-            if (found) {
+            const StreamListItem* item = streams_list[i];
+            if (msgId == item->get_id()) {
+                _mavlink->configure_stream_threadsafe(item->get_name(), rate);
                 break;
             }
         }
-	}
+    }
+}
+
+void
+MavlinkReceiver::get_message_interval(int msgId)
+{
+    unsigned interval = 0;
+
+    MavlinkStream *stream;
+    LL_FOREACH(_mavlink->get_streams(), stream) {
+        if (stream->get_id() == msgId) {
+            interval = stream->get_interval();
+            break;
+        }
+    }
+
+    // send back this value...
+    mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval);
 }
 
 void

src/modules/mavlink/mavlink_receiver.h

@@ -145,6 +145,13 @@ private:
 
 	void *receive_thread(void *arg);
 
+    /**
+     * Set the interval at which the given message stream is published.
+     * The rate is the number of messages per second.
+     */
+	void set_message_interval(int msgId, float rate);
+	void get_message_interval(int msgId);
+
 	/**
 	 * Convert remote timestamp to local hrt time (usec)
 	 * Use timesync if available, monotonic boot time otherwise