From 628b087bedd6b8025bc4ff6664e1ec07c6e83190 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Wed, 10 Jun 2026 11:30:33 +0200 Subject: [PATCH 1/8] GCS_MAVLink: separate gcs_main from operator control range The operator control handler used _operator_control_sysid (param4, the range minimum) as both the sysid range bound and the gcs_main reported in CONTROL_STATUS. When a range is requested, param4 is the minimum id, not necessarily the requesting GCS. Add _operator_control_primary to track the actual requesting GCS (msg.sysid) separately from the range. get_operator_control_sysid() and sysid_is_primary_operator() now use _operator_control_primary, aligning CONTROL_STATUS.gcs_main with the protocol spec: System ID of GCS in control. --- libraries/GCS_MAVLink/GCS.cpp | 8 +++++--- libraries/GCS_MAVLink/GCS.h | 8 ++++---- libraries/GCS_MAVLink/GCS_Common.cpp | 10 +++++----- 3 files changed, 14 insertions(+), 12 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 780a3cf0761972..5ff14b7b7b298e 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -264,6 +264,7 @@ 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_primary = mav_gcs_sysid; _operator_control_sysid = mav_gcs_sysid; _operator_control_sysid_high = mav_gcs_sysid_high; } @@ -719,10 +720,11 @@ bool GCS::sysid_is_gcs(uint8_t _sysid) const } #if AP_MAVLINK_GCS_CONTROL_ENABLED -void GCS::set_operator_control(uint8_t oc_sysid, uint8_t oc_sysid_high, bool allow_takeover) +void GCS::set_operator_control(uint8_t primary, uint8_t range_low, uint8_t range_high, bool allow_takeover) { - _operator_control_sysid = oc_sysid; - _operator_control_sysid_high = oc_sysid_high; + _operator_control_primary = primary; + _operator_control_sysid = range_low; + _operator_control_sysid_high = range_high; _operator_control_allow_takeover = allow_takeover; // clear secondary cache on any ownership change memset(_secondary_gcs, 0, sizeof(_secondary_gcs)); diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 49bb2f7d9908ef..cd8015da01920f 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -1185,12 +1185,11 @@ class GCS bool sysid_is_gcs(uint8_t sysid) const; #if AP_MAVLINK_GCS_CONTROL_ENABLED - void set_operator_control(uint8_t sysid, uint8_t sysid_high, bool allow_takeover); - uint8_t get_operator_control_sysid() const { return _operator_control_sysid; } + void set_operator_control(uint8_t primary, uint8_t range_low, uint8_t range_high, bool allow_takeover); + uint8_t get_operator_control_sysid() const { return _operator_control_primary; } bool get_operator_control_allow_takeover() const { return _operator_control_allow_takeover; } - // true only when operator control is engaged and gcs_sysid is the primary (gcs_main) bool sysid_is_primary_operator(uint8_t gcs_sysid) const { - return _operator_control_sysid != 0 && gcs_sysid == _operator_control_sysid; + return _operator_control_primary != 0 && gcs_sysid == _operator_control_primary; } void note_secondary_gcs_seen(uint8_t gcs_sysid); void get_secondary_gcs(uint8_t (&out)[10]) const; @@ -1389,6 +1388,7 @@ class GCS uint32_t _sysid_gcs_last_seen_time_ms; #if AP_MAVLINK_GCS_CONTROL_ENABLED + uint8_t _operator_control_primary; uint8_t _operator_control_sysid; uint8_t _operator_control_sysid_high; bool _operator_control_allow_takeover; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 1a4a30a7b1274e..94fa31721daeb3 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2803,11 +2803,11 @@ void GCS::update_receive(void) const uint32_t now_ms = AP_HAL::millis(); if (now_ms - _operator_control_last_hb_check_ms >= 1000) { _operator_control_last_hb_check_ms = now_ms; - if (_operator_control_sysid != 0) { + if (_operator_control_primary != 0) { const uint32_t last_seen = sysid_mygcs_last_seen_time_ms(); if (last_seen != 0 && now_ms - last_seen > GCS_OPERATOR_HEARTBEAT_TIMEOUT_MS) { - set_operator_control(0, 0, false); + set_operator_control(0, 0, 0, false); } } } @@ -5713,20 +5713,20 @@ MAV_RESULT GCS_MAVLINK::handle_command_request_operator_control(const mavlink_co if (!gcs().sysid_is_gcs(msg.sysid)) { return MAV_RESULT_DENIED; } - gcs().set_operator_control(0, 0, false); + gcs().set_operator_control(0, 0, 0, false); return MAV_RESULT_ACCEPTED; } // no owner, or same GCS re-requesting: grant if (gcs().get_operator_control_sysid() == 0 || gcs().sysid_is_gcs(req_sysid)) { - gcs().set_operator_control(req_sysid, req_sysid_high, allow_takeover); + gcs().set_operator_control(msg.sysid, req_sysid, req_sysid_high, allow_takeover); return MAV_RESULT_ACCEPTED; } // different GCS; automatic takeover allowed? if (gcs().get_operator_control_allow_takeover()) { - gcs().set_operator_control(req_sysid, req_sysid_high, allow_takeover); + gcs().set_operator_control(msg.sysid, req_sysid, req_sysid_high, allow_takeover); return MAV_RESULT_ACCEPTED; } From 95998a83bbdb7a48791811341cf130be0f6fc5c1 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Tue, 14 Jul 2026 14:38:13 +0200 Subject: [PATCH 2/8] GCS_MAVLink: operator control protocol fixes Four fixes to the operator control request handling: - a secondary GCS within the operator control range was auto-granted control without going through the takeover path: sysid_is_gcs() matched any GCS in the range, so the same-GCS re-requesting branch granted control even when takeover was not allowed. Compare against the primary GCS instead, so secondaries must go through the takeover notification flow as the protocol requires - release was likewise accepted from any GCS in the range, letting a secondary strip control from the primary without a takeover; only the primary may release. The sysid_is_gcs() check is kept for the uncontrolled case so a no-op release is still only accepted from a recognized GCS - the takeover notification pending flag was set but never read or cleared, so a request for MSG_OPERATOR_CONTROL_NOTIFICATION outside the queued send path would transmit a stale notification. Replace it with a per-channel pending mask following the statustext pattern: queueing marks all active channels, each channel clears its bit after a successful send, and an ownership change invalidates any notification still queued - param3 of MAV_CMD_REQUEST_OPERATOR_CONTROL is specified as 3 to 60 seconds but was forwarded unchecked into the takeover notification, so an out-of-range requester timeout (including 0 from senders that do not set it) was displayed as-is by the owning GCS countdown --- libraries/GCS_MAVLink/GCS.cpp | 4 +++- libraries/GCS_MAVLink/GCS.h | 7 +++++-- libraries/GCS_MAVLink/GCS_Common.cpp | 24 +++++++++++++++++++----- 3 files changed, 27 insertions(+), 8 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 5ff14b7b7b298e..76f133ca8aabd2 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -728,13 +728,15 @@ void GCS::set_operator_control(uint8_t primary, uint8_t range_low, uint8_t range _operator_control_allow_takeover = allow_takeover; // clear secondary cache on any ownership change memset(_secondary_gcs, 0, sizeof(_secondary_gcs)); + // an ownership change makes any queued takeover notification stale + _oc_notification.chan_pending_mask = 0; 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.chan_pending_mask = GCS_MAVLINK::active_channel_mask(); _oc_notification.req_sysid = req_sysid; _oc_notification.req_sysid_high = req_sysid_high; _oc_notification.allow_takeover = allow_takeover; diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index cd8015da01920f..c21692002af0b5 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -1196,7 +1196,8 @@ class GCS void queue_operator_control_notification(uint8_t req_sysid, uint8_t req_sysid_high, bool allow_takeover, float timeout); struct OperatorControlNotification { - bool pending; + // channels which still have to send the notification + mavlink_channel_mask_t chan_pending_mask; uint8_t req_sysid; uint8_t req_sysid_high; bool allow_takeover; @@ -1205,7 +1206,9 @@ class GCS const OperatorControlNotification &get_operator_control_notification() const { return _oc_notification; } - void clear_operator_control_notification() { _oc_notification.pending = false; } + void clear_operator_control_notification(mavlink_channel_t chan) { + _oc_notification.chan_pending_mask &= ~(1U< Date: Tue, 14 Jul 2026 14:39:36 +0200 Subject: [PATCH 3/8] autotest: fix and stabilise the operator control test Update the test for the release restriction and timeout clamp (a secondary GCS in the range must be DENIED release; an out-of-range request timeout of 120 is clamped to 60 in the forwarded notification), and fix two reliability problems: - the takeover-notification check listened on self.mav, but waiting for the requester's COMMAND_ACK (run_cmd_int on a non-default connection) drains and discards everything arriving on self.mav, eating the notification before the listen loop starts; the test failed with "Did not receive REQUEST_OPERATOR_CONTROL notification" on every run. Listen on the owner's connection instead, which nothing drains, and assert the notification is addressed to the owner's system id - the operator-control heartbeat timeout is 5 seconds of sim-time but the framework paces its GCS heartbeats in wall-clock time; at the copter default speedup of 100 that is a 10+ sim-second gap between owner heartbeats, so the autopilot cleared ownership mid-test. Pin the test to speedup 4 --- Tools/autotest/arducopter.py | 32 +++++++++++++++++++++++++++----- 1 file changed, 27 insertions(+), 5 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index e2c3d471549227..53e40c4dbac7b9 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -14323,16 +14323,18 @@ def MAV_CMD_REQUEST_OPERATOR_CONTROL(self): # 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, + p1=1, p2=0, p3=120, p4=9, x=0, mav=mav3, want_result=mavutil.mavlink.MAV_RESULT_FAILED, ) - # notification is broadcast on all channels; self.mav is always connected. + # the notification is addressed to the current owner (mav2), so + # listen on their connection. self.mav is no good here: waiting for + # mav3's ACK above drains (discards) everything arriving on self.mav. # 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) + m = mav2.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: @@ -14341,10 +14343,19 @@ def MAV_CMD_REQUEST_OPERATOR_CONTROL(self): if notification is None: raise NotAchievedException( "Did not receive REQUEST_OPERATOR_CONTROL notification") + if notification.target_system != 7: + raise NotAchievedException( + "Expected notification addressed to owner sysid 7, got %u" % + notification.target_system) if int(notification.param4) != 9: raise NotAchievedException( "Expected notification param4=9 (requester sysid), got %u" % int(notification.param4)) + # the request timeout is specified as 3 to 60 seconds; 120 must be clamped + if int(notification.param3) != 60: + raise NotAchievedException( + "Expected notification param3 clamped to 60, got %u" % + int(notification.param3)) # mav2 sets allow_takeover=1; CONTROL_STATUS should reflect the new flag self.run_cmd_int( @@ -14416,11 +14427,19 @@ def MAV_CMD_REQUEST_OPERATOR_CONTROL(self): raise NotAchievedException( "Expected sysid 9 in gcs_secondary, got %s" % str(m.gcs_secondary)) - # mav3 (sysid=9) is within the range and can release + # mav3 (sysid=9) is a secondary within the range; only the primary + # may release, so this must be DENIED self.run_cmd_int( mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL, p1=0, p4=9, mav=mav3, + want_result=mavutil.mavlink.MAV_RESULT_DENIED, + ) + # the primary (mav2, sysid=7) releases + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL, + p1=0, p4=7, + mav=mav2, ) m = self.assert_receive_message("CONTROL_STATUS", timeout=3) if m.gcs_main != 0: @@ -17974,7 +17993,10 @@ def tests2b(self): # this block currently around 9.5mins here self.UTMGlobalPosition, self.UTMGlobalPositionWaypoint, self.HomeAltResetTest, - self.MAV_CMD_REQUEST_OPERATOR_CONTROL, + # run at low speedup: the operator-control heartbeat timeout is + # 5s of sim-time, and the framework's wall-clock-paced heartbeats + # would exceed that between drains at high speedup + Test(self.MAV_CMD_REQUEST_OPERATOR_CONTROL, speedup=4), ]) return ret From 6524eeab1afde135ce9c2292096726938bc03850 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Tue, 14 Jul 2026 14:41:24 +0200 Subject: [PATCH 4/8] GCS_MAVLink: operator control membership always comes from MAV_GCS_SYSID params Per the model agreed with Hamish on PR#33332 / mavlink#2535: - a control request can no longer redefine owner/secondary membership, which also removes the revert-on-release artifact. Membership is read directly from MAV_GCS_SYSID / MAV_GCS_SYSID_HI (the runtime copy of the range only existed to mirror them and could go stale if the parameters changed while controlled) - param5 (range high) was removed from the spec (mavlink#2535, merged): stop reading packet.x on requests and send 0 in param5 of the forwarded notification. param4 is kept on the notification (requester sysid, read by the GCS receiving it); on requests the requester is identified by the sender sysid, not param4 - MAV_GCS_SYSID_HI > MAV_GCS_SYSID configures multi-owner mode: the requester is validated against the param range and outsiders get MAV_RESULT_DENIED. In single-GCS mode any GCS may request control, subject to the takeover rules - the granted primary may therefore lie outside the configured range in single-GCS mode, so it is now recognized as our GCS while it holds control: its heartbeats are tracked for the operator timeout, its manual control and RC overrides are honored, its traffic holds off the GCS failsafe, and accept_packet() accepts it under GCS_SYSID_ENFORCE (previously such a primary was granted control it could not use, and under enforce its heartbeats were dropped so the grant silently reverted after the operator timeout) - gcs_secondary in CONTROL_STATUS stays all-zero in single-owner mode per the spec; previously the configured GCS's own heartbeats appeared as a phantom secondary whenever it was not the one in control --- libraries/GCS_MAVLink/GCS.cpp | 31 ++++++++------ libraries/GCS_MAVLink/GCS.h | 18 ++++++--- libraries/GCS_MAVLink/GCS_Common.cpp | 60 +++++++++++++++++++--------- 3 files changed, 73 insertions(+), 36 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 76f133ca8aabd2..02799d5633729a 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -265,8 +265,6 @@ void GCS::init() #if AP_MAVLINK_GCS_CONTROL_ENABLED if (option_is_enabled(Option::GCS_SYSID_ENFORCE)) { _operator_control_primary = mav_gcs_sysid; - _operator_control_sysid = mav_gcs_sysid; - _operator_control_sysid_high = mav_gcs_sysid_high; } #endif } @@ -706,11 +704,13 @@ 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; + // the primary operator need not be within the configured range + // (any GCS may be granted control in single-GCS mode) but must be + // accepted as our GCS while it holds control, or its manual + // control, its commands (under GCS_SYSID_ENFORCE) and the traffic + // holding off the GCS failsafe would all be ignored + if (sysid_is_primary_operator(_sysid)) { + return true; } #endif if (mav_gcs_sysid_high <= mav_gcs_sysid) { @@ -720,12 +720,15 @@ bool GCS::sysid_is_gcs(uint8_t _sysid) const } #if AP_MAVLINK_GCS_CONTROL_ENABLED -void GCS::set_operator_control(uint8_t primary, uint8_t range_low, uint8_t range_high, bool allow_takeover) +void GCS::set_operator_control(uint8_t primary, bool allow_takeover) { _operator_control_primary = primary; - _operator_control_sysid = range_low; - _operator_control_sysid_high = range_high; _operator_control_allow_takeover = allow_takeover; + if (primary != 0) { + // start the operator timeout from the grant; the new owner may + // not have sent a heartbeat yet + _operator_control_primary_last_seen_ms = AP_HAL::millis(); + } // clear secondary cache on any ownership change memset(_secondary_gcs, 0, sizeof(_secondary_gcs)); // an ownership change makes any queued takeover notification stale @@ -733,17 +736,21 @@ void GCS::set_operator_control(uint8_t primary, uint8_t range_low, uint8_t range send_message(MSG_CONTROL_STATUS); } -void GCS::queue_operator_control_notification(uint8_t req_sysid, uint8_t req_sysid_high, +void GCS::queue_operator_control_notification(uint8_t req_sysid, bool allow_takeover, float timeout) { _oc_notification.chan_pending_mask = GCS_MAVLINK::active_channel_mask(); _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_primary_gcs_seen() +{ + _operator_control_primary_last_seen_ms = AP_HAL::millis(); +} + void GCS::note_secondary_gcs_seen(uint8_t gcs_sysid) { const uint32_t now_ms = AP_HAL::millis(); diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index c21692002af0b5..749cbd5494cbc3 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -1184,22 +1184,28 @@ class GCS */ bool sysid_is_gcs(uint8_t sysid) const; + // configured GCS sysid range (MAV_GCS_SYSID / MAV_GCS_SYSID_HI); + // a high value greater than the low value configures multiple + // owner GCS + uint8_t sysid_gcs() const { return mav_gcs_sysid; } + uint8_t sysid_gcs_high() const { return mav_gcs_sysid_high; } + #if AP_MAVLINK_GCS_CONTROL_ENABLED - void set_operator_control(uint8_t primary, uint8_t range_low, uint8_t range_high, bool allow_takeover); + void set_operator_control(uint8_t primary, bool allow_takeover); uint8_t get_operator_control_sysid() const { return _operator_control_primary; } bool get_operator_control_allow_takeover() const { return _operator_control_allow_takeover; } bool sysid_is_primary_operator(uint8_t gcs_sysid) const { return _operator_control_primary != 0 && gcs_sysid == _operator_control_primary; } void note_secondary_gcs_seen(uint8_t gcs_sysid); + void note_primary_gcs_seen(); void get_secondary_gcs(uint8_t (&out)[10]) const; - void queue_operator_control_notification(uint8_t req_sysid, uint8_t req_sysid_high, + void queue_operator_control_notification(uint8_t req_sysid, bool allow_takeover, float timeout); struct OperatorControlNotification { // channels which still have to send the notification mavlink_channel_mask_t chan_pending_mask; uint8_t req_sysid; - uint8_t req_sysid_high; bool allow_takeover; float timeout; }; @@ -1392,10 +1398,12 @@ class GCS #if AP_MAVLINK_GCS_CONTROL_ENABLED uint8_t _operator_control_primary; - uint8_t _operator_control_sysid; - uint8_t _operator_control_sysid_high; bool _operator_control_allow_takeover; uint32_t _operator_control_last_hb_check_ms; + // last time traffic was seen from the primary operator; the + // primary may lie outside the MAV_GCS_SYSID range in single-GCS + // mode, so it cannot rely on _sysid_gcs_last_seen_time_ms + uint32_t _operator_control_primary_last_seen_ms; OperatorControlNotification _oc_notification; struct SecondaryGCS { uint8_t gcs_sysid; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 08e9a35542d6b4..b8e1175897625f 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2804,10 +2804,10 @@ void GCS::update_receive(void) if (now_ms - _operator_control_last_hb_check_ms >= 1000) { _operator_control_last_hb_check_ms = now_ms; if (_operator_control_primary != 0) { - const uint32_t last_seen = sysid_mygcs_last_seen_time_ms(); + const uint32_t last_seen = _operator_control_primary_last_seen_ms; if (last_seen != 0 && now_ms - last_seen > GCS_OPERATOR_HEARTBEAT_TIMEOUT_MS) { - set_operator_control(0, 0, 0, false); + set_operator_control(0, false); } } } @@ -4379,12 +4379,24 @@ void GCS_MAVLINK::handle_heartbeat(const mavlink_message_t &msg) if (gcs().sysid_is_gcs(msg.sysid)) { sysid_mygcs_seen(AP_HAL::millis()); #if AP_MAVLINK_GCS_CONTROL_ENABLED - // track secondary operators (sysids in the operator range that are not gcs_main) - if (msg.sysid != gcs().get_operator_control_sysid()) { + // track secondary operators (sysids in the operator range that + // are not gcs_main). With a single configured GCS there are no + // secondaries; gcs_secondary must stay all-zero in single-owner + // mode per the CONTROL_STATUS spec + if (gcs().sysid_gcs_high() > gcs().sysid_gcs() && + msg.sysid != gcs().get_operator_control_sysid()) { gcs().note_secondary_gcs_seen(msg.sysid); } #endif } +#if AP_MAVLINK_GCS_CONTROL_ENABLED + // the primary operator need not be within the configured GCS sysid + // range (any GCS may be granted control in single-GCS mode), so its + // heartbeats are tracked separately for the operator timeout + if (gcs().sysid_is_primary_operator(msg.sysid)) { + gcs().note_primary_gcs_seen(); + } +#endif } /* @@ -5697,15 +5709,20 @@ MAV_RESULT GCS_MAVLINK::handle_command_request_operator_control(const mavlink_co { const bool request_control = is_equal(packet.param1, 1.0f); const bool allow_takeover = is_equal(packet.param2, 1.0f); - const uint8_t req_sysid = (uint8_t)packet.param4; - const uint8_t req_sysid_high = (uint8_t)packet.x; - - // sender must fall within the requested sysid range - const bool sender_in_range = (req_sysid_high == 0) - ? (msg.sysid == req_sysid) - : (req_sysid_high >= req_sysid && - msg.sysid >= req_sysid && msg.sysid <= req_sysid_high); - if (!sender_in_range) { + + // the requester is identified by the command's sender. param4 + // (requester sysid) is only meaningful on the notification the + // vehicle forwards to the current owner, and param5 has been + // removed from the spec, so neither is read here. + const uint8_t gcs_sysid = gcs().sysid_gcs(); + const uint8_t gcs_sysid_high = gcs().sysid_gcs_high(); + + // MAV_GCS_SYSID_HI above MAV_GCS_SYSID configures multiple owner + // GCS; only a GCS within that range may use this command. With a + // single configured GCS there is no owner set to validate against, + // so any GCS may request control, subject to the takeover rules. + if (gcs_sysid_high > gcs_sysid && + (msg.sysid < gcs_sysid || msg.sysid > gcs_sysid_high)) { return MAV_RESULT_DENIED; } @@ -5720,27 +5737,32 @@ MAV_RESULT GCS_MAVLINK::handle_command_request_operator_control(const mavlink_co } else if (!gcs().sysid_is_gcs(msg.sysid)) { return MAV_RESULT_DENIED; } - gcs().set_operator_control(0, 0, 0, false); + gcs().set_operator_control(0, false); return MAV_RESULT_ACCEPTED; } - // no owner, or same primary GCS re-requesting (e.g. updating range): grant + // owner/secondary membership always comes from the MAV_GCS_SYSID + // parameters; a control request only ever transfers gcs_main and + // can never redefine membership + + // no owner, or same primary GCS re-requesting (e.g. updating + // allow_takeover): grant if (gcs().get_operator_control_sysid() == 0 || gcs().get_operator_control_sysid() == msg.sysid) { - gcs().set_operator_control(msg.sysid, req_sysid, req_sysid_high, allow_takeover); + gcs().set_operator_control(msg.sysid, allow_takeover); return MAV_RESULT_ACCEPTED; } // different GCS; automatic takeover allowed? if (gcs().get_operator_control_allow_takeover()) { - gcs().set_operator_control(msg.sysid, req_sysid, req_sysid_high, allow_takeover); + gcs().set_operator_control(msg.sysid, allow_takeover); return MAV_RESULT_ACCEPTED; } // notify the current owner; defer through the queued send path so it // goes out even when the TX buffer is momentarily full. param3 is // specified as 3 to 60 seconds - gcs().queue_operator_control_notification(req_sysid, req_sysid_high, allow_takeover, + gcs().queue_operator_control_notification(msg.sysid, allow_takeover, constrain_float(packet.param3, 3.0f, 60.0f)); return MAV_RESULT_FAILED; @@ -6710,7 +6732,7 @@ bool GCS_MAVLINK::send_operator_control_notification() notif.allow_takeover ? 1.0f : 0.0f, notif.timeout, (float)notif.req_sysid, - (float)notif.req_sysid_high, + 0.0f, // param5 removed from the spec 0.0f, 0.0f); gcs().clear_operator_control_notification(chan); From 92577ccc405f9fc65ae74a3bda4700dd52e8c05f Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Tue, 14 Jul 2026 14:41:24 +0200 Subject: [PATCH 5/8] autotest: operator control membership comes from params, not the request Update MAV_CMD_REQUEST_OPERATOR_CONTROL for the agreed model: - multi-owner mode is entered via MAV_GCS_SYSID/MAV_GCS_SYSID_HI instead of a range in the request (param5 removed from spec) - out-of-range requester is DENIED in multi-owner mode (request and release) - a granted request does not change gcs_secondary, release does not revert membership, and gcs_secondary stays all-zero in single-owner mode - the forwarded notification carries param5=0 - primary disconnect releases control even while a secondary keeps heartbeating - single-owner handover to an out-of-range GCS is exercised end-to-end under GCS_SYSID_ENFORCE: takeover accepted, commands ACKed, RC override honored from the primary and refused from an in-range non-primary, heartbeats keeping ownership alive across the operator timeout. The primary's heartbeats are sent from a message hook, mirroring the framework's own do_heartbeats(), and enforcement is switched off before context_pop so the unordered parameter restore cannot lock the test GCS out. Verified failing against the pre-fix GCS_MAVLink code and passing with it --- Tools/autotest/arducopter.py | 171 ++++++++++++++++++++++++++++++++--- 1 file changed, 156 insertions(+), 15 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 53e40c4dbac7b9..bea529309c3019 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -14289,14 +14289,25 @@ def MAV_CMD_REQUEST_OPERATOR_CONTROL(self): 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") + # single-owner mode (MAV_GCS_SYSID_HI <= MAV_GCS_SYSID): there are + # no secondaries, so gcs_secondary must be all-zero per the + # CONTROL_STATUS spec -- even though our own GCS (inside the + # configured range) has been heartbeating all along + if any(m.gcs_secondary): + raise NotAchievedException( + "Expected all-zero gcs_secondary in single-owner mode, got %s" % + str(m.gcs_secondary)) - # mav2 (sysid=7) requests single-GCS control + # mav2 (sysid=7) requests control. param4 is informational + # (requester sysid, only meaningful on the forwarded + # notification) and param5 was removed from the spec; the + # requester is identified by the sender sysid 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) + p4=7, # requester sysid (informational) + x=0, mav=mav2, ) m = self.assert_receive_message("CONTROL_STATUS", timeout=3) @@ -14356,6 +14367,11 @@ def MAV_CMD_REQUEST_OPERATOR_CONTROL(self): raise NotAchievedException( "Expected notification param3 clamped to 60, got %u" % int(notification.param3)) + # param5 was removed from the spec and must not be populated + if int(notification.param5) != 0: + raise NotAchievedException( + "Expected notification param5=0 (removed from spec), got %u" % + int(notification.param5)) # mav2 sets allow_takeover=1; CONTROL_STATUS should reflect the new flag self.run_cmd_int( @@ -14395,28 +14411,33 @@ def MAV_CMD_REQUEST_OPERATOR_CONTROL(self): 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 + # -- multi-owner mode: MAV_GCS_SYSID_HI > MAV_GCS_SYSID -- + # membership comes from the parameters; the request no longer + # carries a range (param5 was removed from the spec) + self.set_parameter("MAV_GCS_SYSID", 7) + self.set_parameter("MAV_GCS_SYSID_HI", 9) + + # out-of-range requester (self.mav, outside [7, 9]) -> DENIED self.run_cmd_int( mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL, - p1=1, p4=5, x=0, - mav=mav3, + p1=1, p2=0, p4=self.mav.source_system, want_result=mavutil.mavlink.MAV_RESULT_DENIED, ) - # range control: mav2 requests [7, 9] so both mav2 and mav3 are in control + # in-range mav2 (sysid=7) becomes gcs_main 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 + p1=1, p2=0, p4=7, 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) + "Expected gcs_main=7 in multi-owner mode, got %u" % m.gcs_main) - # heartbeats from mav3 (sysid=9, within range) should populate gcs_secondary + # the grant must not have changed membership: mav3 (sysid=9, in + # the param range) still shows up in gcs_secondary even though + # the request carried no range mav3.mav.heartbeat_send( mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_INVALID, @@ -14425,7 +14446,21 @@ def MAV_CMD_REQUEST_OPERATOR_CONTROL(self): 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)) + "Expected sysid 9 in gcs_secondary while controlled, got %s" % + str(m.gcs_secondary)) + # ...and out-of-range sysids must not (membership is the params, + # not whoever heartbeats) + if self.mav.source_system in m.gcs_secondary: + raise NotAchievedException( + "Out-of-range sysid %u must not appear in gcs_secondary" % + self.mav.source_system) + + # out-of-range GCS cannot release either + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL, + p1=0, p4=self.mav.source_system, + want_result=mavutil.mavlink.MAV_RESULT_DENIED, + ) # mav3 (sysid=9) is a secondary within the range; only the primary # may release, so this must be DENIED @@ -14444,9 +14479,25 @@ def MAV_CMD_REQUEST_OPERATOR_CONTROL(self): 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) + "Expected gcs_main=0 after release, got %u" % m.gcs_main) - # heartbeat disconnect: when all operators stop heartbeating, control releases + # release must not revert/clear membership: the param range is + # still in force, so in-range heartbeats keep populating + # 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 after release, got %s" % + str(m.gcs_secondary)) + + # heartbeat disconnect: when the primary stops heartbeating, + # control releases -- even though mav3 (a secondary in the + # param range) keeps heartbeating self.run_cmd_int( mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL, p1=1, p2=0, p4=7, x=0, @@ -14473,6 +14524,7 @@ def MAV_CMD_REQUEST_OPERATOR_CONTROL(self): mav2.close() mav3.close() self.set_parameter("MAV_GCS_SYSID", self.mav.source_system) + self.set_parameter("MAV_GCS_SYSID_HI", 0) # back to single-GCS mode self.set_parameter("MAV_OPTIONS", 1) # GCS_SYSID_ENFORCE bit self.reboot_sitl() @@ -14521,6 +14573,95 @@ def MAV_CMD_REQUEST_OPERATOR_CONTROL(self): want_result=mavutil.mavlink.MAV_RESULT_FAILED, ) + def mav3_heartbeat(): + mav3.mav.heartbeat_send( + mavutil.mavlink.MAV_TYPE_GCS, + mavutil.mavlink.MAV_AUTOPILOT_INVALID, + 0, 0, 0) + + def send_rc6_override(conn, pwm): + channels = [65535] * 18 + channels[5] = pwm + conn.mav.rc_channels_override_send(self.mav.target_system, 1, *channels) + + # single-owner handover to an out-of-range GCS: the granted primary + # need not be within MAV_GCS_SYSID; once granted it must be usable + # (packets accepted under GCS_SYSID_ENFORCE, manual control honored, + # heartbeats keeping ownership alive) + # mav3 must heartbeat continuously: once an out-of-range primary + # lapses past the 5s sim-time operator timeout its heartbeats are + # (correctly) dropped again and it cannot recover. Individual + # sends between steps are not enough on a slow machine, so send + # from a message hook -- like the framework's own do_heartbeats() + # -- which runs whenever messages are being processed + mav3_last_hb = [0.0] + + def mav3_heartbeat_hook(mav, msg): + now = time.time() + if now - mav3_last_hb[0] > 0.5: + mav3_last_hb[0] = now + mav3_heartbeat() + self.install_message_hook_context(mav3_heartbeat_hook) + self.set_parameter("SIM_SPEEDUP", 1) + # current owner (self.mav) re-requests with allow-takeover + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL, + p1=1, p2=1, p4=self.mav.source_system, x=0, + ) + # mav3 (out of range) takes over + mav3_heartbeat() + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL, + p1=1, p2=0, p4=9, x=0, + mav=mav3, + ) + m = self.poll_message("CONTROL_STATUS") + if m.gcs_main != 9: + raise NotAchievedException("out-of-range GCS was not granted takeover") + # commands from the out-of-range primary must be accepted (they used to + # be silently dropped by accept_packet) + mav3_heartbeat() + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_SET_MODE, + p1=mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + p2=4, # GUIDED + mav=mav3, + ) + # manual control (RC override) is honored from the primary + mav3_heartbeat() + send_rc6_override(mav3, 1700) + self.wait_rc_channel_value(6, 1700, timeout=3) + # but a non-primary in-range GCS must not be able to override + # (RC overrides expire after 3s, so do not re-send mav3's override here) + mav3_heartbeat() + send_rc6_override(self.mav, 1800) + self.delay_sim_time(1) + self.assert_rc_channel_value(6, 1700) + # the out-of-range primary keeps ownership across the 5s operator + # timeout by heartbeating (pre-fix, enforce dropped its heartbeats and + # the grant silently reverted) + tstart = self.get_sim_time() + while self.get_sim_time_cached() - tstart < 7: + mav3_heartbeat() + self.delay_sim_time(0.5) + m = self.poll_message("CONTROL_STATUS") + if m.gcs_main != 9: + raise NotAchievedException("out-of-range primary timed out despite heartbeating") + if any(m.gcs_secondary): + raise NotAchievedException("gcs_secondary populated in single-owner mode") + # mav3 releases to restore a sane state + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL, + p1=0, p4=9, + mav=mav3, + ) + + # drop enforcement before context_pop restores parameters: the + # restore is unordered, and once MAV_GCS_SYSID flips away from + # our sysid with enforce still on our own PARAM_SETs are + # dropped and the restore times out + self.set_parameter("MAV_OPTIONS", 0) + self.context_pop() self.reboot_sitl() From a17af6a7123972638407dca8971f6f8e21d7a457 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Tue, 14 Jul 2026 15:39:21 +0200 Subject: [PATCH 6/8] GCS_MAVLink: current primary always passes the operator range validation Changing MAV_GCS_SYSID/_HI at runtime so the current primary falls outside the configured range deadlocked the vehicle: the multi-owner range check denied the primary's release (and re-request), while its heartbeats kept the operator timeout alive, so control could never be freed. The current primary now bypasses the range validation so it can always release or adjust allow-takeover; a fresh request after it releases is still validated against the range. --- libraries/GCS_MAVLink/GCS_Common.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index b8e1175897625f..92d45ae88be29d 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -5721,7 +5721,12 @@ MAV_RESULT GCS_MAVLINK::handle_command_request_operator_control(const mavlink_co // GCS; only a GCS within that range may use this command. With a // single configured GCS there is no owner set to validate against, // so any GCS may request control, subject to the takeover rules. + // The current primary always passes: it must be able to release or + // adjust allow-takeover even if the configured range was changed + // while it held control, or it can neither release nor be released + // for as long as it keeps heartbeating. if (gcs_sysid_high > gcs_sysid && + !gcs().sysid_is_primary_operator(msg.sysid) && (msg.sysid < gcs_sysid || msg.sysid > gcs_sysid_high)) { return MAV_RESULT_DENIED; } From 0f349cb129419e21436cdac1045b27569bdf5586 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Tue, 14 Jul 2026 15:39:21 +0200 Subject: [PATCH 7/8] autotest: runtime range change must not deadlock the current primary Shrink MAV_GCS_SYSID/_HI under the current primary and verify it can still adjust allow-takeover and release; once released and out of range, a fresh request is DENIED. --- Tools/autotest/arducopter.py | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index bea529309c3019..07723f21663d38 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -14470,6 +14470,41 @@ def MAV_CMD_REQUEST_OPERATOR_CONTROL(self): mav=mav3, want_result=mavutil.mavlink.MAV_RESULT_DENIED, ) + + # changing the configured range so the current primary falls + # outside it must not deadlock: the primary can still adjust + # allow-takeover and release (its heartbeats keep the operator + # timeout alive, so nothing else could ever free the vehicle) + self.set_parameter("MAV_GCS_SYSID", 8) # range now [8, 9], primary is 7 + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL, + p1=1, p2=0, p4=7, x=0, + mav=mav2, + ) + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL, + p1=0, p4=7, + mav=mav2, + ) + m = self.assert_receive_message("CONTROL_STATUS", timeout=3) + if m.gcs_main != 0: + raise NotAchievedException( + "Expected gcs_main=0 after out-of-range primary release, got %u" % m.gcs_main) + # ...but now that it is out of range and no longer primary, a + # fresh request must be DENIED + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL, + p1=1, p2=0, p4=7, x=0, + mav=mav2, + want_result=mavutil.mavlink.MAV_RESULT_DENIED, + ) + self.set_parameter("MAV_GCS_SYSID", 7) + # restore mav2 as primary for the release-behaviour checks below + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL, + p1=1, p2=0, p4=7, x=0, + mav=mav2, + ) # the primary (mav2, sysid=7) releases self.run_cmd_int( mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL, From e69d52cd69a271be82fa13172e3a8405e624f4ec Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Tue, 14 Jul 2026 15:41:55 +0200 Subject: [PATCH 8/8] autotest: keep the secondary heartbeating through the primary timeout The disconnect test claimed control releases "even though mav3 keeps heartbeating", but mav3 sent no heartbeats during the wait, so the test passed identically under the old any-in-range-GCS liveness. Send them for real and assert the secondary survives the primary's timeout, pinning that the operator timeout tracks the primary specifically. --- Tools/autotest/arducopter.py | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 07723f21663d38..37d766c112bc9c 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -14542,11 +14542,27 @@ def MAV_CMD_REQUEST_OPERATOR_CONTROL(self): 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) + # keep the secondary heartbeating through the wait: the timeout + # must track the primary specifically -- under the old + # any-in-range-GCS liveness these heartbeats would mask the + # primary's disconnect and control would never release + tstart = self.get_sim_time() + while self.get_sim_time_cached() - tstart < 7: # > GCS_OPERATOR_HEARTBEAT_TIMEOUT_MS (5 s) + mav3.mav.heartbeat_send( + mavutil.mavlink.MAV_TYPE_GCS, + mavutil.mavlink.MAV_AUTOPILOT_INVALID, + 0, 0, 0, + ) + self.delay_sim_time(0.5) 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) + # the secondary was unaffected by the primary's timeout + if 9 not in m.gcs_secondary: + raise NotAchievedException( + "Expected sysid 9 still in gcs_secondary after primary timeout, got %s" % + str(m.gcs_secondary)) # reopen mav2 for the enforcement test mav2 = self.context_create_mavlink_connection( "tcp:localhost:%u" % self.adjust_ardupilot_port(5762),