Add support for MAV_CMD_REQUEST_OPERATOR_CONTROL

Tools/autotest/arducopter.py

@@ -14267,6 +14267,244 @@ def tests1c(self):
         ])
         return ret
 
+    def MAV_CMD_REQUEST_OPERATOR_CONTROL(self):
+        '''test MAV_CMD_REQUEST_OPERATOR_CONTROL GCS operator control protocol'''
+        self.context_push()
+        # SERIAL1 and SERIAL2 default to MAVLink2, ports 5762 and 5763
+        mav2 = self.context_create_mavlink_connection(
+            "tcp:localhost:%u" % self.adjust_ardupilot_port(5762),
+            source_system=7,
+            source_component=7,
+        )
+        mav3 = self.context_create_mavlink_connection(
+            "tcp:localhost:%u" % self.adjust_ardupilot_port(5763),
+            source_system=9,
+            source_component=9,
+        )
+
+        # without GCS_SYSID_ENFORCE, operator control is not pre-populated from
+        # params, so initial gcs_main should be 0
+        m = self.poll_message("CONTROL_STATUS")
+        if m.gcs_main != 0:
+            raise NotAchievedException("Expected gcs_main=0 initially, got %u" % m.gcs_main)
+        if not (m.flags & mavutil.mavlink.GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER):
+            raise NotAchievedException("Expected SYSTEM_MANAGER flag set")
+
+        # mav2 (sysid=7) requests single-GCS control
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL,
+            p1=1,  # request
+            p2=0,  # no auto-takeover
+            p4=7,  # req_sysid
+            x=0,   # req_sysid_high=0 (single GCS)
+            mav=mav2,
+        )
+        m = self.assert_receive_message("CONTROL_STATUS", timeout=3)
+        if m.gcs_main != 7:
+            raise NotAchievedException("Expected gcs_main=7, got %u" % m.gcs_main)
+
+        # re-request by same GCS is idempotent
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL,
+            p1=1, p2=0, p4=7, x=0,
+            mav=mav2,
+        )
+
+        # mav3 tries to release when not owner -> DENIED
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL,
+            p1=0,  # release
+            p4=9,
+            mav=mav3,
+            want_result=mavutil.mavlink.MAV_RESULT_DENIED,
+        )
+
+        # mav3 tries to take over with allow_takeover=0 -> FAILED, and a
+        # notification is forwarded to the current owner (mav2) on their channel
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL,
+            p1=1, p2=0, p4=9, x=0,
+            mav=mav3,
+            want_result=mavutil.mavlink.MAV_RESULT_FAILED,
+        )
+        # notification is broadcast on all channels; self.mav is always connected.
+        # recv_match loops until it finds a COMMAND_LONG(REQUEST_OPERATOR_CONTROL).
+        tstart = time.time()
+        notification = None
+        while time.time() - tstart < 5:
+            m = self.mav.recv_match(blocking=True, timeout=0.5)
+            if m is None:
+                continue
+            if m.get_type() == "COMMAND_LONG" and m.command == mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL:
+                notification = m
+                break
+        if notification is None:
+            raise NotAchievedException(
+                "Did not receive REQUEST_OPERATOR_CONTROL notification")
+        if int(notification.param4) != 9:
+            raise NotAchievedException(
+                "Expected notification param4=9 (requester sysid), got %u" %
+                int(notification.param4))
+
+        # mav2 sets allow_takeover=1; CONTROL_STATUS should reflect the new flag
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL,
+            p1=1, p2=1, p4=7, x=0,
+            mav=mav2,
+        )
+        m = self.assert_receive_message("CONTROL_STATUS", timeout=3)
+        if not (m.flags & mavutil.mavlink.GCS_CONTROL_STATUS_FLAGS_TAKEOVER_ALLOWED):
+            raise NotAchievedException("Expected TAKEOVER_ALLOWED flag after allow_takeover=1")
+
+        # mav3 takes over (allow_takeover is now set)
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL,
+            p1=1, p2=0, p4=9, x=0,
+            mav=mav3,
+        )
+        m = self.assert_receive_message("CONTROL_STATUS", timeout=3)
+        if m.gcs_main != 9:
+            raise NotAchievedException("Expected gcs_main=9 after takeover, got %u" % m.gcs_main)
+
+        # mav2 tries to release after losing control -> DENIED
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL,
+            p1=0, p4=7,
+            mav=mav2,
+            want_result=mavutil.mavlink.MAV_RESULT_DENIED,
+        )
+
+        # mav3 releases control
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL,
+            p1=0, p4=9,
+            mav=mav3,
+        )
+        m = self.assert_receive_message("CONTROL_STATUS", timeout=3)
+        if m.gcs_main != 0:
+            raise NotAchievedException("Expected gcs_main=0 after release, got %u" % m.gcs_main)
+
+        # sender sysid out of range: mav3 (sysid=9) requests for sysid=5 only -> DENIED
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL,
+            p1=1, p4=5, x=0,
+            mav=mav3,
+            want_result=mavutil.mavlink.MAV_RESULT_DENIED,
+        )
+
+        # range control: mav2 requests [7, 9] so both mav2 and mav3 are in control
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL,
+            p1=1, p2=0,
+            p4=7,  # lower bound
+            x=9,   # upper bound: sysids 7–9 are all operators
+            mav=mav2,
+        )
+        m = self.assert_receive_message("CONTROL_STATUS", timeout=3)
+        if m.gcs_main != 7:
+            raise NotAchievedException(
+                "Expected gcs_main=7 (lower bound of range), got %u" % m.gcs_main)
+
+        # heartbeats from mav3 (sysid=9, within range) should populate gcs_secondary
+        mav3.mav.heartbeat_send(
+            mavutil.mavlink.MAV_TYPE_GCS,
+            mavutil.mavlink.MAV_AUTOPILOT_INVALID,
+            0, 0, 0,
+        )
+        m = self.poll_message("CONTROL_STATUS")
+        if 9 not in m.gcs_secondary:
+            raise NotAchievedException(
+                "Expected sysid 9 in gcs_secondary, got %s" % str(m.gcs_secondary))
+
+        # mav3 (sysid=9) is within the range and can release
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL,
+            p1=0, p4=9,
+            mav=mav3,
+        )
+        m = self.assert_receive_message("CONTROL_STATUS", timeout=3)
+        if m.gcs_main != 0:
+            raise NotAchievedException(
+                "Expected gcs_main=0 after range release, got %u" % m.gcs_main)
+
+        # heartbeat disconnect: when all operators stop heartbeating, control releases
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL,
+            p1=1, p2=0, p4=7, x=0,
+            mav=mav2,
+        )
+        m = self.assert_receive_message("CONTROL_STATUS", timeout=3)
+        if m.gcs_main != 7:
+            raise NotAchievedException("Expected gcs_main=7 before disconnect test")
+        mav2.close()
+        self.delay_sim_time(7)  # > GCS_OPERATOR_HEARTBEAT_TIMEOUT_MS (5 s)
+        m = self.poll_message("CONTROL_STATUS")
+        if m.gcs_main != 0:
+            raise NotAchievedException(
+                "Expected gcs_main=0 after operator disconnect timeout, got %u" % m.gcs_main)
+        # reopen mav2 for the enforcement test
+        mav2 = self.context_create_mavlink_connection(
+            "tcp:localhost:%u" % self.adjust_ardupilot_port(5762),
+            source_system=7,
+            source_component=7,
+        )
+
+        # -- Enforcement tests: enable GCS_SYSID_ENFORCE and reboot --
+        # close existing extra connections before rebooting
+        mav2.close()
+        mav3.close()
+        self.set_parameter("MAV_GCS_SYSID", self.mav.source_system)
+        self.set_parameter("MAV_OPTIONS", 1)  # GCS_SYSID_ENFORCE bit
+        self.reboot_sitl()
+
+        # reconnect after reboot using context management for cleanup
+        mav2 = self.context_create_mavlink_connection(
+            "tcp:localhost:%u" % self.adjust_ardupilot_port(5762),
+            source_system=7,
+            source_component=7,
+        )
+        mav3 = self.context_create_mavlink_connection(
+            "tcp:localhost:%u" % self.adjust_ardupilot_port(5763),
+            source_system=9,
+            source_component=9,
+        )
+
+        # with GCS_SYSID_ENFORCE, operator is pre-populated from MAV_GCS_SYSID
+        m = self.poll_message("CONTROL_STATUS")
+        if m.gcs_main != self.mav.source_system:
+            raise NotAchievedException(
+                "Expected gcs_main=%u (pre-populated), got %u" %
+                (self.mav.source_system, m.gcs_main))
+
+        # mode change from non-operator is dropped by accept_packet (no COMMAND_ACK)
+        self.send_cmd(
+            mavutil.mavlink.MAV_CMD_DO_SET_MODE,
+            p1=mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+            p2=4,  # GUIDED
+            mav=mav3,
+        )
+        self.assert_not_receive_message("COMMAND_ACK", mav=mav3, timeout=2)
+
+        # arm/disarm from non-operator is also dropped
+        self.send_cmd(
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            p1=0,
+            mav=mav3,
+        )
+        self.assert_not_receive_message("COMMAND_ACK", mav=mav3, timeout=2)
+
+        # MAV_CMD_REQUEST_OPERATOR_CONTROL from non-operator still gets through
+        # but is FAILED because current owner (main GCS) has allow_takeover=0
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL,
+            p1=1, p2=0, p4=9, x=0,
+            mav=mav3,
+            want_result=mavutil.mavlink.MAV_RESULT_FAILED,
+        )
+
+        self.context_pop()
+        self.reboot_sitl()
+
     def tests1d(self):
         '''return list of all tests'''
         ret = ([
@@ -17736,6 +17974,7 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.UTMGlobalPosition,
             self.UTMGlobalPositionWaypoint,
             self.HomeAltResetTest,
+            self.MAV_CMD_REQUEST_OPERATOR_CONTROL,
         ])
         return ret
 

Tools/autotest/vehicle_test_suite.py

@@ -222,6 +222,9 @@ def __init__(self):
         # launched by restart_SITL_frame()) registered via
         # context_register_periph_child(); terminated on context_pop()
         self.periph_children = []
+        # mavlink connections opened via context_create_mavlink_connection();
+        # closed on context_pop()
+        self.mavlink_connections = []
 
 
 # https://stackoverflow.com/questions/616645/how-do-i-duplicate-sys-stdout-to-a-log-file-in-python
@@ -6760,6 +6763,12 @@ def context_register_periph_child(self, child):
         processes (used by restart_SITL_frame()).'''
         self.context_get().periph_children.append(child)
 
+    def context_create_mavlink_connection(self, port, **kwargs):
+        '''open a mavlink connection that is automatically closed on context_pop()'''
+        conn = mavutil.mavlink_connection(port, robust_parsing=True, **kwargs)
+        self.context_get().mavlink_connections.append(conn)
+        return conn
+
     def context_collect(self, msg_type):
         '''start collecting messages of type msg_type into context collection'''
         context = self.context_get()
@@ -6826,6 +6835,12 @@ def context_pop(self, process_interaction_allowed=True, hooks_already_removed=Fa
             except pexpect.ExceptionPexpect:
                 pass
 
+        for conn in reversed(dead.mavlink_connections):
+            try:
+                conn.close()
+            except Exception:
+                pass
+
         # restore any files snapshotted via context_backup_file()
         for path, data, mode in reversed(dead.backup_files):
             running_binary = getattr(self, 'binary', None)

libraries/GCS_MAVLink/GCS.cpp

@@ -262,6 +262,12 @@ MissionItemProtocol *GCS::missionitemprotocols[3];
 void GCS::init()
 {
     mavlink_system.sysid = sysid_this_mav();
+#if AP_MAVLINK_GCS_CONTROL_ENABLED
+    if (option_is_enabled(Option::GCS_SYSID_ENFORCE)) {
+        _operator_control_sysid = mav_gcs_sysid;
+        _operator_control_sysid_high = mav_gcs_sysid_high;
+    }
+#endif
 }
 
 /*
@@ -698,10 +704,87 @@ MAV_RESULT GCS::lua_command_int_packet(const mavlink_command_int_t &packet)
 */
 bool GCS::sysid_is_gcs(uint8_t _sysid) const
 {
+#if AP_MAVLINK_GCS_CONTROL_ENABLED
+    if (_operator_control_sysid != 0) {
+        if (_operator_control_sysid_high <= _operator_control_sysid) {
+            return _operator_control_sysid == _sysid;
+        }
+        return _sysid >= _operator_control_sysid && _sysid <= _operator_control_sysid_high;
+    }
+#endif
     if (mav_gcs_sysid_high <= mav_gcs_sysid) {
         return mav_gcs_sysid == _sysid;
     }
     return _sysid >= mav_gcs_sysid && _sysid <= mav_gcs_sysid_high;
 }
 
+#if AP_MAVLINK_GCS_CONTROL_ENABLED
+void GCS::set_operator_control(uint8_t oc_sysid, uint8_t oc_sysid_high, bool allow_takeover)
+{
+    _operator_control_sysid = oc_sysid;
+    _operator_control_sysid_high = oc_sysid_high;
+    _operator_control_allow_takeover = allow_takeover;
+    // clear secondary cache on any ownership change
+    memset(_secondary_gcs, 0, sizeof(_secondary_gcs));
+    send_message(MSG_CONTROL_STATUS);
+}
+
+void GCS::queue_operator_control_notification(uint8_t req_sysid, uint8_t req_sysid_high,
+                                               bool allow_takeover, float timeout)
+{
+    _oc_notification.pending       = true;
+    _oc_notification.req_sysid     = req_sysid;
+    _oc_notification.req_sysid_high = req_sysid_high;
+    _oc_notification.allow_takeover = allow_takeover;
+    _oc_notification.timeout       = timeout;
+    send_message(MSG_OPERATOR_CONTROL_NOTIFICATION);
+}
+
+void GCS::note_secondary_gcs_seen(uint8_t gcs_sysid)
+{
+    const uint32_t now_ms = AP_HAL::millis();
+    // update existing entry if present
+    for (auto &entry : _secondary_gcs) {
+        if (entry.gcs_sysid == gcs_sysid) {
+            entry.last_seen_ms = now_ms;
+            return;
+        }
+    }
+    // insert into first empty slot
+    for (auto &entry : _secondary_gcs) {
+        if (entry.gcs_sysid == 0) {
+            entry.gcs_sysid = gcs_sysid;
+            entry.last_seen_ms = now_ms;
+            return;
+        }
+    }
+    // replace oldest entry
+    uint8_t oldest_idx = 0;
+    uint32_t oldest_ms = _secondary_gcs[0].last_seen_ms;
+    for (uint8_t i = 1; i < ARRAY_SIZE(_secondary_gcs); i++) {
+        if (_secondary_gcs[i].last_seen_ms < oldest_ms) {
+            oldest_ms = _secondary_gcs[i].last_seen_ms;
+            oldest_idx = i;
+        }
+    }
+    _secondary_gcs[oldest_idx].gcs_sysid = gcs_sysid;
+    _secondary_gcs[oldest_idx].last_seen_ms = now_ms;
+}
+
+void GCS::get_secondary_gcs(uint8_t (&out)[10]) const
+{
+    const uint32_t now_ms = AP_HAL::millis();
+    uint8_t idx = 0;
+    for (const auto &entry : _secondary_gcs) {
+        if (idx >= ARRAY_SIZE(out)) {
+            break;
+        }
+        if (entry.gcs_sysid != 0 &&
+            now_ms - entry.last_seen_ms < GCS_OPERATOR_HEARTBEAT_TIMEOUT_MS) {
+            out[idx++] = entry.gcs_sysid;
+        }
+    }
+}
+#endif
+
 #endif  // HAL_GCS_ENABLED