From 85beba03ee5d122d5b9bd21ab4c69669fc5f649f Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Fri, 1 Dec 2023 12:26:46 +1100 Subject: [PATCH] driver/mavcan.py: gracefully exit io thread when close is called --- dronecan/driver/mavcan.py | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/dronecan/driver/mavcan.py b/dronecan/driver/mavcan.py index f361d7e..2631f45 100644 --- a/dronecan/driver/mavcan.py +++ b/dronecan/driver/mavcan.py @@ -28,13 +28,13 @@ TX_QUEUE_SIZE = 1000 logger = getLogger(__name__) - +kill_process = False class ControlMessage(object): def __init__(self, command, data): self.command = command self.data = data -def io_process(url, bus, target_system, baudrate, tx_queue, rx_queue): +def io_process(url, bus, target_system, baudrate, tx_queue, rx_queue, exit_queue): os.environ['MAVLINK20'] = '1' target_component = 0 @@ -43,7 +43,7 @@ def io_process(url, bus, target_system, baudrate, tx_queue, rx_queue): filter_list = None signing_key = None last_loss_print_t = time.time() - + exit_proc = False def connect(): nonlocal conn, baudrate conn = mavutil.mavlink_connection(url, baud=baudrate, source_system=250, @@ -56,7 +56,11 @@ def connect(): conn.setup_signing(signing_key, sign_outgoing=True) def reconnect(): - while True: + nonlocal exit_proc + while True and not exit_proc: + if (not exit_queue.empty() and exit_queue.get() == "QUIT"): + exit_proc = True + return try: time.sleep(1) logger.info('reconnecting to %s' % url) @@ -114,7 +118,13 @@ def handle_control_message(m): enable_can_forward() while True: + if (not exit_queue.empty() and exit_queue.get() == "QUIT") or exit_proc: + conn.close() + return while not tx_queue.empty(): + if (not exit_queue.empty() and exit_queue.get() == "QUIT") or exit_proc: + conn.close() + return frame = tx_queue.get() if isinstance(frame, ControlMessage): handle_control_message(frame) @@ -186,10 +196,11 @@ def __init__(self, url, **kwargs): self.rx_queue = multiprocessing.Queue(maxsize=RX_QUEUE_SIZE) self.tx_queue = multiprocessing.Queue(maxsize=TX_QUEUE_SIZE) + self.exit_queue = multiprocessing.Queue(maxsize=1) self.proc = multiprocessing.Process(target=io_process, name='mavcan_io_process', args=(url, self.bus, self.target_system, baudrate, - self.tx_queue, self.rx_queue)) + self.tx_queue, self.rx_queue, self.exit_queue)) self.proc.daemon = True self.proc.start() @@ -199,7 +210,9 @@ def __init__(self, url, **kwargs): self.set_signing_passphrase(pass_phrase) def close(self): - pass + if self.proc is not None: + self.exit_queue.put_nowait("QUIT") + self.proc.join() def __del__(self): self.close() @@ -270,7 +283,3 @@ def set_signing_passphrase(self, passphrase): '''set MAVLink2 signing passphrase''' signing_key = self.passphrase_to_key(passphrase) self.tx_queue.put_nowait(ControlMessage('SigningKey', signing_key)) - - - -