Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

driver/mavcan.py: gracefully exit io thread when close is called #53

Merged
merged 1 commit into from
Dec 27, 2023
Merged

driver/mavcan.py: gracefully exit io thread when close is called #53

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
29 changes: 19 additions & 10 deletions dronecan/driver/mavcan.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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,
Expand All @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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()

Expand All @@ -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()
Expand Down Expand Up @@ -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))




Loading