Merge pull request #570 from dronekit/tcr-writethread

Splits outbound messages into its own thread.

Author: mrpollo

dronekit/__init__.py

@@ -1452,9 +1452,10 @@ def remove_message_listener(self, name, fn):
         """
         name = str(name)
         if name in self._message_listeners:
-            self._message_listeners[name].remove(fn)
-            if len(self._message_listeners[name]) == 0:
-                del self._message_listeners[name]
+            if fn in self._message_listeners[name]:
+                self._message_listeners[name].remove(fn)
+                if len(self._message_listeners[name]) == 0:
+                    del self._message_listeners[name]
 
     def notify_message_listeners(self, name, msg):
         for fn in self._message_listeners.get(name, []):
@@ -2605,7 +2606,11 @@ def clear(self):
 
         # Add home point again.
         self.wait_ready()
-        home = self._vehicle._wploader.wp(0)
+        home = None
+        try:
+            home = self._vehicle._wploader.wp(0)
+        except:
+            pass
         self._vehicle._wploader.clear()
         if home:
             self._vehicle._wploader.add(home, comment='Added by DroneKit')
@@ -2701,7 +2706,8 @@ def connect(ip,
             rate=4,
             baud=115200,
             heartbeat_timeout=30,
-            source_system=255):
+            source_system=255,
+            use_native=False):
     """
     Returns a :py:class:`Vehicle` object connected to the address specified by string parameter ``ip``. 
     Connection string parameters (``ip``) for different targets are listed in the :ref:`getting started guide <get_started_connecting>`.
@@ -2737,6 +2743,7 @@ def connect(ip,
     :param int heartbeat_timeout: Connection timeout value in seconds (default is 30s). 
         If a heartbeat is not detected within this time an exception will be raised.    
     :param int source_system: The MAVLink ID of the :py:class:`Vehicle` object returned by this method (by default 255).
+    :param bool use_native: Use precompiled MAVLink parser.
 
         .. note::
 
@@ -2755,7 +2762,7 @@ def connect(ip,
     if not vehicle_class:
         vehicle_class = Vehicle
 
-    handler = MAVConnection(ip, baud=baud, source_system=source_system)
+    handler = MAVConnection(ip, baud=baud, source_system=source_system, use_native=use_native)
     vehicle = vehicle_class(handler)
 
     if status_printer:

dronekit/mavlink.py

@@ -21,6 +21,11 @@
 
 
 class MAVWriter(object):
+    """
+    Indirection layer to take messages written to MAVlink and send them all
+    on the same thread.
+    """
+
     def __init__(self, queue):
         self.queue = queue
 
@@ -33,7 +38,7 @@ def read(self):
 
 
 class MAVConnection(object):
-    def __init__(self, ip, baud=115200, target_system=0, source_system=255):
+    def __init__(self, ip, baud=115200, target_system=0, source_system=255, use_native=False):
         self.master = mavutil.mavlink_connection(ip, baud=baud, source_system=source_system)
 
         # TODO get rid of "master" object as exposed,
@@ -42,7 +47,7 @@ def __init__(self, ip, baud=115200, target_system=0, source_system=255):
         self.master.mav = mavutil.mavlink.MAVLink(
             MAVWriter(self.out_queue),
             srcSystem=self.master.source_system,
-            use_native=False)
+            use_native=use_native)
 
         # Monkey-patch MAVLink object for fix_targets.
         sendfn = self.master.mav.send
@@ -72,7 +77,42 @@ def onexit():
 
         atexit.register(onexit)
 
-        def mavlink_thread():
+        def mavlink_thread_out():
+            # Huge try catch in case we see http://bugs.python.org/issue1856
+            try:
+                while self._alive:
+                    try:
+                        msg = self.out_queue.get(True, timeout=0.01)
+                        self.master.write(msg)
+                    except Empty:
+                        continue
+                    except socket.error as error:
+                        # If connection reset (closed), stop polling.
+                        if error.errno == ECONNABORTED:
+                            raise APIException('Connection aborting during read')
+                        raise
+                    except Exception as e:
+                        errprinter('>>> mav send error:', e)
+                        break
+            except APIException as e:
+                errprinter('>>> ' + str(e.message))
+                self._alive = False
+                self.master.close()
+                self._death_error = e
+
+            except Exception as e:
+                # http://bugs.python.org/issue1856
+                if not self._alive:
+                    pass
+                else:
+                    self._alive = False
+                    self.master.close()
+                    self._death_error = e
+
+            # Explicitly clear out buffer so .close closes.
+            self.out_queue = Queue()
+
+        def mavlink_thread_in():
             # Huge try catch in case we see http://bugs.python.org/issue1856
             try:
                 while True:
@@ -83,21 +123,6 @@ def mavlink_thread():
                     for fn in self.loop_listeners:
                         fn(self)
 
-                    while True:
-                        try:
-                            msg = self.out_queue.get_nowait()
-                            self.master.write(msg)
-                        except socket.error as error:
-                            # If connection reset (closed), stop polling.
-                            if error.errno == ECONNABORTED:
-                                raise APIException('Connection aborting during read')
-                            raise
-                        except Empty:
-                            break
-                        except Exception as e:
-                            errprinter('>>> mav send error:', e)
-                            break
-
                     while self._accept_input:
                         try:
                             msg = self.master.recv_msg()
@@ -140,9 +165,13 @@ def mavlink_thread():
                     self.master.close()
                     self._death_error = e
 
-        t = Thread(target=mavlink_thread)
+        t = Thread(target=mavlink_thread_in)
+        t.daemon = True
+        self.mavlink_thread_in = t
+
+        t = Thread(target=mavlink_thread_out)
         t.daemon = True
-        self.mavlink_thread = t
+        self.mavlink_thread_out = t
 
     def reset(self):
         self.out_queue = Queue()
@@ -173,8 +202,10 @@ def forward_message(self, fn):
         self.message_listeners.append(fn)
 
     def start(self):
-        if not self.mavlink_thread.is_alive():
-            self.mavlink_thread.start()
+        if not self.mavlink_thread_in.is_alive():
+            self.mavlink_thread_in.start()
+        if not self.mavlink_thread_out.is_alive():
+            self.mavlink_thread_out.start()
 
     def close(self):
         # TODO this can block forever if parameters continue to be added
@@ -189,13 +220,27 @@ def pipe(self, target):
         # vehicle -> self -> target
         @self.forward_message
         def callback(_, msg):
-            target.out_queue.put(msg.pack(target.master.mav))
+            try:
+                target.out_queue.put(msg.pack(target.master.mav))
+            except:
+                try:
+                    assert len(msg.get_msgbuf()) > 0
+                    target.out_queue.put(msg.get_msgbuf())
+                except:
+                    errprinter('>>> Could not pack this object on receive: %s' % type(msg))
 
         # target -> self -> vehicle
         @target.forward_message
         def callback(_, msg):
             msg = copy.copy(msg)
             target.fix_targets(msg)
-            self.out_queue.put(msg.pack(self.master.mav))
+            try:
+                self.out_queue.put(msg.pack(self.master.mav))
+            except:
+                try:
+                    assert len(msg.get_msgbuf()) > 0
+                    self.out_queue.put(msg.get_msgbuf())
+                except:
+                    errprinter('>>> Could not pack this object on forward: %s' % type(msg))
 
         return target