diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index b166fb4cc32303..e2c3d471549227 100644 --- a/Tools/autotest/arducopter.py +++ b/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 diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 36b0300dad05c9..49d0b76d72101d 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/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) diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 81c0239119f936..780a3cf0761972 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/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 diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 790080013cff83..49bb2f7d9908ef 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -306,6 +306,7 @@ class GCS_MAVLINK mavlink_channel_t get_chan() const { return chan; } uint32_t get_last_heartbeat_time() const { return last_heartbeat_time; }; + uint32_t sysid_mygcs_last_seen_time_ms() const { return _sysid_gcs_last_seen_time_ms; } uint32_t last_heartbeat_time; // milliseconds @@ -1117,6 +1118,12 @@ class GCS_MAVLINK bool send_available_modes(); bool send_available_mode_monitor(); +#if AP_MAVLINK_GCS_CONTROL_ENABLED + MAV_RESULT handle_command_request_operator_control(const mavlink_command_int_t &packet, const mavlink_message_t &msg); + bool send_control_status(); + bool send_operator_control_notification(); +#endif + }; /// @class GCS @@ -1177,6 +1184,31 @@ 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; } + 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; + } + void note_secondary_gcs_seen(uint8_t gcs_sysid); + void get_secondary_gcs(uint8_t (&out)[10]) const; + void queue_operator_control_notification(uint8_t req_sysid, uint8_t req_sysid_high, + bool allow_takeover, float timeout); + struct OperatorControlNotification { + bool pending; + 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; } +#endif + // last time traffic was seen from my designated GCS. traffic // includes heartbeats and some manual control messages. uint32_t sysid_mygcs_last_seen_time_ms() const { @@ -1356,6 +1388,19 @@ class GCS // recent time that backend saw traffic from MAV_GCS_SYSID uint32_t _sysid_gcs_last_seen_time_ms; +#if AP_MAVLINK_GCS_CONTROL_ENABLED + uint8_t _operator_control_sysid; + uint8_t _operator_control_sysid_high; + bool _operator_control_allow_takeover; + uint32_t _operator_control_last_hb_check_ms; + OperatorControlNotification _oc_notification; + struct SecondaryGCS { + uint8_t gcs_sysid; + uint32_t last_seen_ms; + }; + SecondaryGCS _secondary_gcs[10]; +#endif + void service_statustext(void); #if HAL_MEM_CLASS <= HAL_MEM_CLASS_192 static const uint8_t _status_capacity = 10; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index d5a75d67eecd70..1a4a30a7b1274e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1203,6 +1203,9 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c #if AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED { MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, MSG_UTM_GLOBAL_POSITION}, #endif // AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED +#if AP_MAVLINK_GCS_CONTROL_ENABLED + { MAVLINK_MSG_ID_CONTROL_STATUS, MSG_CONTROL_STATUS}, +#endif // AP_MAVLINK_GCS_CONTROL_ENABLED }; for (uint8_t i=0; i= 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 (last_seen != 0 && + now_ms - last_seen > GCS_OPERATOR_HEARTBEAT_TIMEOUT_MS) { + set_operator_control(0, 0, false); + } + } + } + } +#endif } void GCS::send_mission_item_reached_message(uint16_t mission_index) @@ -4191,6 +4209,13 @@ void GCS_MAVLINK::handle_rc_channels_override(const mavlink_message_t &msg) if (!gcs().sysid_is_gcs(msg.sysid)) { return; // Only accept control from our gcs } +#if AP_MAVLINK_GCS_CONTROL_ENABLED + // manual control is exclusive to the primary operator (gcs_main); secondaries are denied + if (gcs().get_operator_control_sysid() != 0 && + !gcs().sysid_is_primary_operator(msg.sysid)) { + return; + } +#endif const uint32_t tnow = AP_HAL::millis(); @@ -4353,6 +4378,12 @@ void GCS_MAVLINK::handle_heartbeat(const mavlink_message_t &msg) // now... 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()) { + gcs().note_secondary_gcs_seen(msg.sysid); + } +#endif } } @@ -5661,6 +5692,52 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_follow(const mavlink_command_int_t &pa } #endif // AP_MAVLINK_FOLLOW_HANDLING_ENABLED +#if AP_MAVLINK_GCS_CONTROL_ENABLED +MAV_RESULT GCS_MAVLINK::handle_command_request_operator_control(const mavlink_command_int_t &packet, const mavlink_message_t &msg) +{ + 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) { + return MAV_RESULT_DENIED; + } + + if (!request_control) { + if (!gcs().sysid_is_gcs(msg.sysid)) { + return MAV_RESULT_DENIED; + } + gcs().set_operator_control(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); + 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); + 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); + + return MAV_RESULT_FAILED; +} +#endif // AP_MAVLINK_GCS_CONTROL_ENABLED + MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { switch (packet.command) { @@ -5894,6 +5971,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p case MAV_CMD_DO_FOLLOW: return handle_command_do_follow(packet, msg); #endif + +#if AP_MAVLINK_GCS_CONTROL_ENABLED + case MAV_CMD_REQUEST_OPERATOR_CONTROL: + return handle_command_request_operator_control(packet, msg); +#endif } return MAV_RESULT_UNSUPPORTED; @@ -6600,6 +6682,46 @@ bool GCS_MAVLINK::send_available_mode_monitor() return true; } +#if AP_MAVLINK_GCS_CONTROL_ENABLED +bool GCS_MAVLINK::send_operator_control_notification() +{ + const GCS::OperatorControlNotification ¬if = gcs().get_operator_control_notification(); + CHECK_PAYLOAD_SIZE2(COMMAND_LONG); + mavlink_msg_command_long_send( + chan, + gcs().get_operator_control_sysid(), + MAV_COMP_ID_AUTOPILOT1, + MAV_CMD_REQUEST_OPERATOR_CONTROL, + 0, + 1.0f, + notif.allow_takeover ? 1.0f : 0.0f, + notif.timeout, + (float)notif.req_sysid, + (float)notif.req_sysid_high, + 0.0f, + 0.0f); + return true; +} + +bool GCS_MAVLINK::send_control_status() +{ + CHECK_PAYLOAD_SIZE(CONTROL_STATUS); + + uint8_t flags = GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER; + if (gcs().get_operator_control_allow_takeover()) { + flags |= GCS_CONTROL_STATUS_FLAGS_TAKEOVER_ALLOWED; + } + uint8_t gcs_secondary[10] {}; + gcs().get_secondary_gcs(gcs_secondary); + mavlink_msg_control_status_send( + chan, + flags, + gcs().get_operator_control_sysid(), + gcs_secondary); + return true; +} +#endif // AP_MAVLINK_GCS_CONTROL_ENABLED + bool GCS_MAVLINK::try_send_message(const enum ap_message id) { bool ret = true; @@ -7026,6 +7148,16 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) ret = send_available_mode_monitor(); break; +#if AP_MAVLINK_GCS_CONTROL_ENABLED + case MSG_CONTROL_STATUS: + ret = send_control_status(); + break; + + case MSG_OPERATOR_CONTROL_NOTIFICATION: + ret = send_operator_control_notification(); + break; +#endif + #if AP_MAVLINK_MSG_FLIGHT_INFORMATION_ENABLED case MSG_FLIGHT_INFORMATION: CHECK_PAYLOAD_SIZE(FLIGHT_INFORMATION); @@ -7385,6 +7517,24 @@ bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status, return true; } +#if AP_MAVLINK_GCS_CONTROL_ENABLED + // always accept operator control requests so non-owners can request takeover + if (msg.msgid == MAVLINK_MSG_ID_COMMAND_INT) { + mavlink_command_int_t pkt; + mavlink_msg_command_int_decode(&msg, &pkt); + if (pkt.command == MAV_CMD_REQUEST_OPERATOR_CONTROL) { + return true; + } + } + if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + mavlink_command_long_t pkt; + mavlink_msg_command_long_decode(&msg, &pkt); + if (pkt.command == MAV_CMD_REQUEST_OPERATOR_CONTROL) { + return true; + } + } +#endif + if (!gcs().option_is_enabled(GCS::Option::GCS_SYSID_ENFORCE)) { return true; } @@ -7604,6 +7754,10 @@ uint64_t GCS_MAVLINK::capabilities() const } #endif +#if AP_MAVLINK_GCS_CONTROL_ENABLED + ret |= MAV_PROTOCOL_CAPABILITY_COMPONENT_ACCEPTS_GCS_CONTROL; +#endif + return ret; } @@ -7631,6 +7785,13 @@ void GCS_MAVLINK::handle_manual_control(const mavlink_message_t &msg) if (!gcs().sysid_is_gcs(msg.sysid)) { return; // only accept control from our gcs } +#if AP_MAVLINK_GCS_CONTROL_ENABLED + // manual control is exclusive to the primary operator (gcs_main); secondaries are denied + if (gcs().get_operator_control_sysid() != 0 && + !gcs().sysid_is_primary_operator(msg.sysid)) { + return; + } +#endif mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(&msg, &packet); diff --git a/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp b/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp index 38fdc1f4855619..df83a6f2e6226b 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp @@ -265,8 +265,11 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_POSITION_TARGET_GLOBAL_INT, #if APM_BUILD_TYPE(APM_BUILD_ArduSub) - MSG_NAMED_FLOAT + MSG_NAMED_FLOAT, #endif // APM_BUILD_TYPE(APM_BUILD_ArduSub) +#if AP_MAVLINK_GCS_CONTROL_ENABLED + MSG_CONTROL_STATUS, +#endif }; static const ap_message STREAM_POSITION_msgs[] = { diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h index e0dbc1929a8593..74b8d3003dae57 100644 --- a/libraries/GCS_MAVLink/GCS_config.h +++ b/libraries/GCS_MAVLink/GCS_config.h @@ -151,6 +151,15 @@ #define AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED (HAL_GCS_ENABLED && AP_AHRS_ENABLED && (HAL_PROGRAM_SIZE_LIMIT_KB > 2048)) #endif // AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED +#ifndef AP_MAVLINK_GCS_CONTROL_ENABLED +#define AP_MAVLINK_GCS_CONTROL_ENABLED (HAL_GCS_ENABLED && (HAL_PROGRAM_SIZE_LIMIT_KB > 2048)) +#endif // AP_MAVLINK_GCS_CONTROL_ENABLED + +#if AP_MAVLINK_GCS_CONTROL_ENABLED +// operator heartbeat timeout: release control when all owners silent for this long +#define GCS_OPERATOR_HEARTBEAT_TIMEOUT_MS 5000U +#endif + // deprecated 2025-02, replaced by MAV_CMD_DO_SET_GLOBAL_ORIGIN // ArduPilot 4.8 starts to warn if anyone uses this // ArduPilot 4.9 continues to warn if anyone uses this diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 99976df0a5f690..ba804d45047dff 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -116,5 +116,9 @@ enum ap_message : uint8_t { #if AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED MSG_UTM_GLOBAL_POSITION = 101, #endif // AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED +#if AP_MAVLINK_GCS_CONTROL_ENABLED + MSG_CONTROL_STATUS = 102, + MSG_OPERATOR_CONTROL_NOTIFICATION = 103, +#endif // AP_MAVLINK_GCS_CONTROL_ENABLED MSG_LAST // MSG_LAST must be the last entry in this enum }; diff --git a/modules/mavlink b/modules/mavlink index 035ffa630d54de..d655d89a06dca7 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 035ffa630d54de8e5fc0f4fafc32182287738819 +Subproject commit d655d89a06dca73042deeb18dcce3b47e155f10d