diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index e2c3d471549227..37d766c112bc9c 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) @@ -14323,16 +14334,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 +14354,24 @@ 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)) + # 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( @@ -14384,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, @@ -14414,20 +14446,93 @@ 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) - # mav3 (sysid=9) is within the range and can release + # 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 self.run_cmd_int( mavutil.mavlink.MAV_CMD_REQUEST_OPERATOR_CONTROL, p1=0, p4=9, 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, + 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 range release, got %u" % m.gcs_main) + "Expected gcs_main=0 after release, got %u" % m.gcs_main) + + # 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 all operators stop heartbeating, control releases + # 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, @@ -14437,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), @@ -14454,6 +14575,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() @@ -14502,6 +14624,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() @@ -17974,7 +18185,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 diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 780a3cf0761972..02799d5633729a 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -264,8 +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_sysid = mav_gcs_sysid; - _operator_control_sysid_high = mav_gcs_sysid_high; + _operator_control_primary = mav_gcs_sysid; } #endif } @@ -705,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) { @@ -719,27 +720,37 @@ 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, bool allow_takeover) { - _operator_control_sysid = oc_sysid; - _operator_control_sysid_high = oc_sysid_high; + _operator_control_primary = primary; _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 + _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, +void GCS::queue_operator_control_notification(uint8_t req_sysid, 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; _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 49bb2f7d9908ef..749cbd5494cbc3 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -1184,29 +1184,37 @@ 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 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, 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 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 { - 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; float timeout; }; 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<= 1000) { _operator_control_last_hb_check_ms = now_ms; - if (_operator_control_sysid != 0) { - const uint32_t last_seen = sysid_mygcs_last_seen_time_ms(); + if (_operator_control_primary != 0) { + 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, 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,42 +5709,66 @@ 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. + // 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; } if (!request_control) { - if (!gcs().sysid_is_gcs(msg.sysid)) { + // only the primary operator may release control; secondary GCS in + // the range must not be able to strip control from the primary + const uint8_t primary = gcs().get_operator_control_sysid(); + if (primary != 0) { + if (msg.sysid != primary) { + return MAV_RESULT_DENIED; + } + } else if (!gcs().sysid_is_gcs(msg.sysid)) { return MAV_RESULT_DENIED; } - gcs().set_operator_control(0, 0, false); + gcs().set_operator_control(0, false); return MAV_RESULT_ACCEPTED; } - // no owner, or same GCS re-requesting: 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().sysid_is_gcs(req_sysid)) { - gcs().set_operator_control(req_sysid, req_sysid_high, allow_takeover); + gcs().get_operator_control_sysid() == msg.sysid) { + 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(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 - gcs().queue_operator_control_notification(req_sysid, req_sysid_high, allow_takeover, packet.param3); + // goes out even when the TX buffer is momentarily full. param3 is + // specified as 3 to 60 seconds + gcs().queue_operator_control_notification(msg.sysid, allow_takeover, + constrain_float(packet.param3, 3.0f, 60.0f)); return MAV_RESULT_FAILED; } @@ -6686,6 +6722,10 @@ bool GCS_MAVLINK::send_available_mode_monitor() bool GCS_MAVLINK::send_operator_control_notification() { const GCS::OperatorControlNotification ¬if = gcs().get_operator_control_notification(); + if (!(notif.chan_pending_mask & (1U<