diff --git a/libs/mavlink/include/mavlink/v2.0 b/libs/mavlink/include/mavlink/v2.0
index 5be9f5bf6002d58199cafafb5d416929fb8d8bcd..a31f7d989dffc2e554c26ad2c22c2a432a48fa74 160000
--- a/libs/mavlink/include/mavlink/v2.0
+++ b/libs/mavlink/include/mavlink/v2.0
@@ -1 +1 @@
-Subproject commit 5be9f5bf6002d58199cafafb5d416929fb8d8bcd
+Subproject commit a31f7d989dffc2e554c26ad2c22c2a432a48fa74
diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
index 1a0b3ff2881d7ad45c1dfb01941b9d251d31bf26..3a5c7972eabcedf1c66a487c20bb5aa6f3c36519 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
+++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
@@ -290,7 +290,7 @@ QList<MAV_CMD> PX4FirmwarePlugin::supportedMissionCommands(void)
          << MAV_CMD_DO_SET_SERVO
          << MAV_CMD_DO_CHANGE_SPEED
          << MAV_CMD_DO_LAND_START
-         << MAV_CMD_DO_SET_ROI
+         << MAV_CMD_DO_SET_ROI_LOCATION << MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET << MAV_CMD_DO_SET_ROI_NONE
          << MAV_CMD_DO_MOUNT_CONFIGURE
          << MAV_CMD_DO_MOUNT_CONTROL
          << MAV_CMD_SET_CAMERA_MODE
diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json
index 1995c7651206463ab86c6d75ae47323f1fb7873a..9a7dd6cbcd962409dda9e1d84456787cdde3d03a 100644
--- a/src/MissionManager/MavCmdInfoCommon.json
+++ b/src/MissionManager/MavCmdInfoCommon.json
@@ -624,6 +624,52 @@
                 "default":          0
             }
         },
+        {
+            "id":                   195,
+            "rawName":              "MAV_CMD_DO_SET_ROI_LOCATION",
+            "friendlyName":         "Region of interest (ROI)" ,
+            "description":          "Sets the region of interest for cameras.",
+            "specifiesCoordinate":  true,
+            "standaloneCoordinate": true,
+            "friendlyEdit":         true,
+            "category":             "Camera"
+        },
+        {
+            "id":                   196,
+            "rawName":              "MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET",
+            "friendlyName":         "ROI to next waypoint" ,
+            "description":          "Sets the region of interest to point towards the next waypoint with optional offsets.",
+            "specifiesCoordinate":  false,
+            "friendlyEdit":         true,
+            "category":             "Camera",
+            "param5": {
+                "label":            "Pitch offset",
+                "default":          0,
+                "units":            "deg",
+                "decimalPlaces":    0
+            },
+            "param6": {
+                "label":            "Roll offset",
+                "default":          0,
+                "units":            "deg",
+                "decimalPlaces":    0
+            },
+            "param7": {
+                "label":            "Yaw offset",
+                "default":          0,
+                "units":            "deg",
+                "decimalPlaces":    0
+            }
+        },
+        {
+            "id":                   197,
+            "rawName":              "MAV_CMD_DO_SET_ROI_NONE",
+            "friendlyName":         "Cancel ROI" ,
+            "description":          "Cancels the region of interest.",
+            "specifiesCoordinate":  false,
+            "friendlyEdit":         true,
+            "category":             "Camera"
+        },
         { "id": 200, "rawName": "MAV_CMD_DO_CONTROL_VIDEO", "friendlyName": "Control video" },
         {
             "id":                   201,
diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc
index e4e28dcc56eb8146a8f49ff2ee480a9c95e93065..013e81e9538baa2baffd8f346005e5b2da0d541e 100644
--- a/src/comm/MockLink.cc
+++ b/src/comm/MockLink.cc
@@ -914,12 +914,8 @@ void MockLink::_respondWithAutopilotVersion(void)
                                             (uint8_t *)&customVersion,       // os_custom_version,
                                             0,                               // vendor_id,
                                             0,                               // product_id,
-                                            0                                // uid
-#if defined(NO_ARDUPILOT_DIALECT)
-                                            //-- Once the MAVLink module is updated, this should show up. In the mean time, it's disabled.
-                                            ,0                               // uid2
-#endif
-                                            );
+                                            0,                               // uid
+                                            0);                              // uid2
     respondWithMavlinkMessage(msg);
 }