Ensure proper thread and link cleanup on failed Crazyflie connections / disconnects

cflib/bootloader/cloader.py

@@ -114,7 +114,7 @@ def reset_to_bootloader(self, target_id: int) -> bool:
                 self.link = cflib.crtp.get_link_driver(f'radio://0/0/2M/{address}?safelink=0')
                 time.sleep(0.5)
                 return True
-
+        self.link.close()
         return False
 
     def reset_to_firmware(self, target_id: int, boot_delay: float = 0.0) -> bool:

cflib/crazyflie/__init__.py

@@ -32,10 +32,10 @@
 """
 import datetime
 import logging
-import time
 import warnings
 from collections import namedtuple
 from threading import current_thread
+from threading import Event
 from threading import Lock
 from threading import Thread
 from threading import Timer
@@ -284,6 +284,14 @@ def close_link(self):
         if (self.link is not None):
             self.link.close()
             self.link = None
+        if self.incoming:
+            callbacks = list(self.incoming.cb)
+            if self.incoming.is_alive():
+                self.incoming.stop()
+                self.incoming.join(timeout=1)
+            self.incoming = _IncomingPacketHandler(self)
+            self.incoming.cb = callbacks
+            self.incoming.daemon = True
         self._answer_patterns = {}
         self.disconnected.call(self.link_uri)
         self.state = State.DISCONNECTED
@@ -397,6 +405,7 @@ def __init__(self, cf):
         self.daemon = True
         self.cf = cf
         self.cb = []
+        self._stop_event = Event()
 
     def add_port_callback(self, port, cb):
         """Add a callback for data that comes on a specific port"""
@@ -431,10 +440,14 @@ def remove_header_callback(self, cb, port, channel, port_mask=0xFF,
                     port_callback.callback == cb:
                 self.cb.remove(port_callback)
 
+    def stop(self):
+        """Signal the thread to stop."""
+        self._stop_event.set()
+
     def run(self):
-        while True:
+        while not self._stop_event.is_set():
             if self.cf.link is None:
-                time.sleep(1)
+                self._stop_event.wait(1)
                 continue
             pk = self.cf.link.receive_packet(1)
 

cflib/crazyflie/syncCrazyflie.py

@@ -92,6 +92,7 @@ def open_link(self):
         if not self._is_link_open:
             self._remove_callbacks()
             self._params_updated_event.clear()
+            self.cf.close_link()
             raise Exception(self._error_message)
 
     def wait_for_params(self):