diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 125ed23a5d..3edf67676b 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/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;
 	}
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 8839c91b2a..9cd58d66d8 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -275,11 +275,16 @@ public:
 		return "HEARTBEAT";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_HEARTBEAT;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamHeartbeat(mavlink);
@@ -339,11 +344,16 @@ public:
 		return "STATUSTEXT";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_STATUSTEXT;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamStatustext(mavlink);
@@ -471,11 +481,16 @@ public:
 		return "COMMAND_LONG";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_COMMAND_LONG;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamCommandLong(mavlink);
@@ -539,11 +554,16 @@ public:
 		return "SYS_STATUS";
 	}
 
-	uint8_t get_id()
+    static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_SYS_STATUS;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamSysStatus(mavlink);
@@ -645,11 +665,16 @@ public:
 		return "HIGHRES_IMU";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_HIGHRES_IMU;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamHighresIMU(mavlink);
@@ -751,11 +776,16 @@ public:
 		return "ATTITUDE";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_ATTITUDE;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamAttitude(mavlink);
@@ -815,11 +845,16 @@ public:
 		return "ATTITUDE_QUATERNION";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamAttitudeQuaternion(mavlink);
@@ -880,11 +915,16 @@ public:
 		return "VFR_HUD";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_VFR_HUD;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamVFRHUD(mavlink);
@@ -985,11 +1025,16 @@ public:
 		return "GPS_RAW_INT";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_GPS_RAW_INT;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamGPSRawInt(mavlink);
@@ -1048,10 +1093,15 @@ public:
 		return "SYSTEM_TIME";
 	}
 
-	uint8_t get_id() {
+	static uint8_t get_id_static() {
 		return MAVLINK_MSG_ID_SYSTEM_TIME;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink) {
 		return new MavlinkStreamSystemTime(mavlink);
 	}
@@ -1093,10 +1143,15 @@ public:
 		return "TIMESYNC";
 	}
 
-	uint8_t get_id() {
+	static uint8_t get_id_static() {
 		return MAVLINK_MSG_ID_TIMESYNC;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink) {
 		return new MavlinkStreamTimesync(mavlink);
 	}
@@ -1137,11 +1192,16 @@ public:
 		return "ADSB_VEHICLE";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_ADSB_VEHICLE;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamADSBVehicle(mavlink);
@@ -1205,11 +1265,16 @@ public:
 		return "CAMERA_TRIGGER";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_CAMERA_TRIGGER;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamCameraTrigger(mavlink);
@@ -1265,11 +1330,16 @@ public:
 		return "GLOBAL_POSITION_INT";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamGlobalPositionInt(mavlink);
@@ -1338,11 +1408,16 @@ public:
         return "VISION_POSITION_NED";
     }
 
-    uint8_t get_id()
+    static uint8_t get_id_static()
     {
         return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE;
     }
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
     static MavlinkStream *new_instance(Mavlink *mavlink)
     {
         return new MavlinkStreamVisionPositionNED(mavlink);
@@ -1402,11 +1477,16 @@ public:
 		return "LOCAL_POSITION_NED";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamLocalPositionNED(mavlink);
@@ -1465,11 +1545,16 @@ public:
 		return "LOCAL_POSITION_NED_COV";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamLocalPositionNEDCOV(mavlink);
@@ -1537,11 +1622,16 @@ public:
 		return "ESTIMATOR_STATUS";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_VIBRATION;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamEstimatorStatus(mavlink);
@@ -1600,11 +1690,16 @@ public:
 		return "ATT_POS_MOCAP";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_ATT_POS_MOCAP;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamAttPosMocap(mavlink);
@@ -1664,11 +1759,16 @@ public:
 		return "HOME_POSITION";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_HOME_POSITION;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamHomePosition(mavlink);
@@ -1734,11 +1834,16 @@ public:
 		return MavlinkStreamServoOutputRaw<N>::get_name_static();
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static const char *get_name_static()
 	{
 		switch (N) {
@@ -1831,11 +1936,16 @@ public:
 		}
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamActuatorControlTarget<N>(mavlink);
@@ -1912,11 +2022,16 @@ public:
 		return "HIL_CONTROLS";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_HIL_CONTROLS;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamHILControls(mavlink);
@@ -2071,11 +2186,16 @@ public:
 		return "POSITION_TARGET_GLOBAL_INT";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamPositionTargetGlobalInt(mavlink);
@@ -2130,11 +2250,16 @@ public:
 		return "POSITION_TARGET_LOCAL_NED";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamLocalPositionSetpoint(mavlink);
@@ -2198,11 +2323,16 @@ public:
 		return "ATTITUDE_TARGET";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_ATTITUDE_TARGET;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamAttitudeTarget(mavlink);
@@ -2270,11 +2400,16 @@ public:
 		return "RC_CHANNELS";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_RC_CHANNELS;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamRCChannels(mavlink);
@@ -2366,11 +2501,16 @@ public:
 		return "MANUAL_CONTROL";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_MANUAL_CONTROL;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamManualControl(mavlink);
@@ -2434,11 +2574,16 @@ public:
 		return "OPTICAL_FLOW_RAD";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamOpticalFlowRad(mavlink);
@@ -2502,11 +2647,16 @@ public:
 		return "NAMED_VALUE_FLOAT";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamNamedValueFloat(mavlink);
@@ -2562,11 +2712,16 @@ public:
 		return "NAV_CONTROLLER_OUTPUT";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamNavControllerOutput(mavlink);
@@ -2629,11 +2784,16 @@ public:
 		return "CAMERA_CAPTURE";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return 0;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamCameraCapture(mavlink);
@@ -2693,11 +2853,16 @@ public:
 		return "DISTANCE_SENSOR";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_DISTANCE_SENSOR;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamDistanceSensor(mavlink);
@@ -2777,11 +2942,16 @@ public:
 		return "EXTENDED_SYS_STATE";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_EXTENDED_SYS_STATE;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamExtendedSysState(mavlink);
@@ -2871,11 +3041,16 @@ public:
 		return "ALTITUDE";
 	}
 
-	uint8_t get_id()
+	static uint8_t get_id_static()
 	{
 		return MAVLINK_MSG_ID_ALTITUDE;
 	}
 
+    uint8_t get_id()
+    {
+        return get_id_static();
+    }
+
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
 		return new MavlinkStreamAltitude(mavlink);
@@ -2953,46 +3128,46 @@ protected:
 };
 
 const StreamListItem *streams_list[] = {
-	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
-	new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static),
-	new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static),
-	new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static),
-	new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static),
-	new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static),
-	new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static),
-	new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static),
-	new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static),
-	new StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static),
-	new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static),
-	new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
-	new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
-	new StreamListItem(&MavlinkStreamVisionPositionNED::new_instance, &MavlinkStreamVisionPositionNED::get_name_static),
-	new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static),
-	new StreamListItem(&MavlinkStreamEstimatorStatus::new_instance, &MavlinkStreamEstimatorStatus::get_name_static),
-	new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static),
-	new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static),
-	new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
-	new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
-	new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static),
-	new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static),
-	new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static),
-	new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static),
-	new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
-	new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
-	new StreamListItem(&MavlinkStreamRCChannels::new_instance, &MavlinkStreamRCChannels::get_name_static),
-	new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
-	new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static),
-	new StreamListItem(&MavlinkStreamActuatorControlTarget<0>::new_instance, &MavlinkStreamActuatorControlTarget<0>::get_name_static),
-	new StreamListItem(&MavlinkStreamActuatorControlTarget<1>::new_instance, &MavlinkStreamActuatorControlTarget<1>::get_name_static),
-	new StreamListItem(&MavlinkStreamActuatorControlTarget<2>::new_instance, &MavlinkStreamActuatorControlTarget<2>::get_name_static),
-	new StreamListItem(&MavlinkStreamActuatorControlTarget<3>::new_instance, &MavlinkStreamActuatorControlTarget<3>::get_name_static),
-	new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
-	new StreamListItem(&MavlinkStreamNavControllerOutput::new_instance, &MavlinkStreamNavControllerOutput::get_name_static),
-	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
-	new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static),
-	new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),
-	new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static),
-	new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static),
-	new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static),
+	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static),
+	new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static),
+	new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static, &MavlinkStreamCommandLong::get_id_static),
+	new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static, &MavlinkStreamSysStatus::get_id_static),
+	new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static, &MavlinkStreamHighresIMU::get_id_static),
+	new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static, &MavlinkStreamAttitude::get_id_static),
+	new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static, &MavlinkStreamAttitudeQuaternion::get_id_static),
+	new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static, &MavlinkStreamVFRHUD::get_id_static),
+	new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static, &MavlinkStreamGPSRawInt::get_id_static),
+	new StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static, &MavlinkStreamSystemTime::get_id_static),
+	new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static, &MavlinkStreamTimesync::get_id_static),
+	new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static, &MavlinkStreamGlobalPositionInt::get_id_static),
+	new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static, &MavlinkStreamLocalPositionNED::get_id_static),
+	new StreamListItem(&MavlinkStreamVisionPositionNED::new_instance, &MavlinkStreamVisionPositionNED::get_name_static, &MavlinkStreamVisionPositionNED::get_id_static),
+	new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static, &MavlinkStreamLocalPositionNEDCOV::get_id_static),
+	new StreamListItem(&MavlinkStreamEstimatorStatus::new_instance, &MavlinkStreamEstimatorStatus::get_name_static, &MavlinkStreamEstimatorStatus::get_id_static),
+	new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static, &MavlinkStreamAttPosMocap::get_id_static),
+	new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static, &MavlinkStreamHomePosition::get_id_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static, &MavlinkStreamServoOutputRaw<0>::get_id_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static, &MavlinkStreamServoOutputRaw<1>::get_id_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static, &MavlinkStreamServoOutputRaw<2>::get_id_static),
+	new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static, &MavlinkStreamServoOutputRaw<3>::get_id_static),
+	new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static, &MavlinkStreamHILControls::get_id_static),
+	new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static, &MavlinkStreamPositionTargetGlobalInt::get_id_static),
+	new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static, &MavlinkStreamLocalPositionSetpoint::get_id_static),
+	new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static, &MavlinkStreamAttitudeTarget::get_id_static),
+	new StreamListItem(&MavlinkStreamRCChannels::new_instance, &MavlinkStreamRCChannels::get_name_static, &MavlinkStreamRCChannels::get_id_static),
+	new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static, &MavlinkStreamManualControl::get_id_static),
+	new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static, &MavlinkStreamOpticalFlowRad::get_id_static),
+	new StreamListItem(&MavlinkStreamActuatorControlTarget<0>::new_instance, &MavlinkStreamActuatorControlTarget<0>::get_name_static, &MavlinkStreamActuatorControlTarget<0>::get_id_static),
+	new StreamListItem(&MavlinkStreamActuatorControlTarget<1>::new_instance, &MavlinkStreamActuatorControlTarget<1>::get_name_static, &MavlinkStreamActuatorControlTarget<1>::get_id_static),
+	new StreamListItem(&MavlinkStreamActuatorControlTarget<2>::new_instance, &MavlinkStreamActuatorControlTarget<2>::get_name_static, &MavlinkStreamActuatorControlTarget<2>::get_id_static),
+	new StreamListItem(&MavlinkStreamActuatorControlTarget<3>::new_instance, &MavlinkStreamActuatorControlTarget<3>::get_name_static, &MavlinkStreamActuatorControlTarget<3>::get_id_static),
+	new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static, &MavlinkStreamNamedValueFloat::get_id_static),
+	new StreamListItem(&MavlinkStreamNavControllerOutput::new_instance, &MavlinkStreamNavControllerOutput::get_name_static, &MavlinkStreamNavControllerOutput::get_id_static),
+	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static, &MavlinkStreamCameraCapture::get_id_static),
+	new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static, &MavlinkStreamCameraTrigger::get_id_static),
+	new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static, &MavlinkStreamDistanceSensor::get_id_static),
+	new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static, &MavlinkStreamExtendedSysState::get_id_static),
+	new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static, &MavlinkStreamAltitude::get_id_static),
+	new StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static, &MavlinkStreamADSBVehicle::get_id_static),
 	nullptr
 };
diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h
index 5b6b9d6335..685eaff475 100644
--- a/src/modules/mavlink/mavlink_messages.h
+++ b/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() {};
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 9d33711b9e..f04d98f887 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/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
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 28ee0fa04e..7894c3b817 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/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