merge from AdeelH/python-ardrone

ardrone/at.py

@@ -135,6 +135,7 @@ def anim(host, seq, anim, d):
     """
     at(host, 'ANIM', seq, [anim, d])
 
+
 def at(host, command, seq, params):
     """
     Parameters:

ardrone/drone.py

@@ -4,7 +4,6 @@
 
 import time
 import threading
-import multiprocessing
 
 import PIL.Image
 
@@ -28,18 +27,32 @@ def __init__(self, host='192.168.1.1'):
         self.lock = threading.Lock()
         self.speed = 0.2
         self.at(ardrone.at.config, 'general:navdata_demo', 'TRUE')
+        self.at(ardrone.at.config, 'control:control_level', '3')
         self.at(ardrone.at.config, 'control:altitude_max', '20000')
-        self.video_pipe, video_pipe_other = multiprocessing.Pipe()
-        self.nav_pipe, nav_pipe_other = multiprocessing.Pipe()
-        self.com_pipe, com_pipe_other = multiprocessing.Pipe()
-        self.network_process = ardrone.network.ARDroneNetworkProcess(self.host, nav_pipe_other, video_pipe_other, com_pipe_other)
-        self.network_process.start()
-        self.ipc_thread = ardrone.network.IPCThread(self)
-        self.ipc_thread.start()
-        self.image = PIL.Image.new('RGB', (640, 360))
-        self.navdata = dict()
+
+        self.image = None
+        self.navdata = None
+
+        self.video_thread = ardrone.network.VidThread(
+            self.host,
+            self.image_callback
+        )
+        self.navdata_thread = ardrone.network.NavThread(
+            self.host,
+            self.navdata_callback
+        )
+        self.video_thread.start()
+        self.navdata_thread.start()
+
         self.time = 0
 
+    def image_callback(self, im_data):
+        w, h, img = im_data
+        self.image = PIL.Image.frombuffer('RGB', (w, h), img, 'raw', 'RGB', 0, 1)
+
+    def navdata_callback(self, navdata):
+        self.navdata = navdata
+
     def takeoff(self):
         """Make the drone takeoff."""
         self.at(ardrone.at.ref, True)
@@ -140,10 +153,10 @@ def halt(self):
         """
         with self.lock:
             self.com_watchdog_timer.cancel()
-            self.ipc_thread.stop()
-            self.ipc_thread.join()
-            self.network_process.terminate()
-            self.network_process.join()
+            self.video_thread.stop()
+            self.video_thread.join()
+            self.navdata_thread.stop()
+            self.navdata_thread.join()
 
     def move(self, lr, fb, vv, va):
         """Makes the drone move (translate/rotate).
@@ -156,3 +169,13 @@ def move(self, lr, fb, vv, va):
         va -- angular speed: float [-1..1] negative: spin left, positive: spin
             right"""
         self.at(ardrone.at.pcmd, True, lr, fb, vv, va)
+
+    def move2(self, vv, va):
+        """Makes the drone move (up-down and rotate only) while trying
+        to stay above the same point on the ground.
+
+        Parameters:
+        vv -- vertical speed: float [-1..1] negative: go down, positive: rise
+        va -- angular speed: float [-1..1] negative: spin left, positive: spin
+            right"""
+        self.at(ardrone.at.pcmd, False, 0, 0, vv, va)

ardrone/network.py

@@ -6,106 +6,88 @@
 import socket
 import struct
 import threading
-import multiprocessing
-
-import PIL.Image
 
 import ardrone.constant
 import ardrone.navdata
 import ardrone.video
 
 
-class ARDroneNetworkProcess(multiprocessing.Process):
-    """ARDrone Network Process.
+class NavThread(threading.Thread):
+    """Inter Process Communication Thread.
 
-    This process collects data from the video and navdata port, converts the
-    data and sends it to the IPCThread.
+    This thread collects navdata from the navdata port and makes it available
+    to the ARDrone.
     """
 
-    def __init__(self, host, nav_pipe, video_pipe, com_pipe):
-        multiprocessing.Process.__init__(self)
-        self.nav_pipe = nav_pipe
-        self.video_pipe = video_pipe
-        self.com_pipe = com_pipe
+    def __init__(self, host, callback):
+        threading.Thread.__init__(self)
         self.host = host
+        self.callback = callback
+        self.stopping = False
 
     def run(self):
-        video_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-        video_socket.connect((self.host, ardrone.constant.VIDEO_PORT))
-
         nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
         nav_socket.setblocking(False)
         nav_socket.bind(('', ardrone.constant.NAVDATA_PORT))
         nav_socket.sendto(b'\x01\x00\x00\x00', (self.host, ardrone.constant.NAVDATA_PORT))
 
-        stopping = False
-        while not stopping:
-            inputready, outputready, exceptready = select.select([nav_socket, video_socket, self.com_pipe], [], [])
-            for i in inputready:
-                if i == video_socket:
-                    # get first few bytes of header
-                    data = video_socket.recv(12, socket.MSG_WAITALL)
-                    if len(data) != 12:
-                        continue
-
-                    # decode relevant portions of the header
-                    sig_p, sig_a, sig_v, sig_e, version, codec, header, payload = struct.unpack('4cBBHI', data)
-
-                    # check signature (and ignore packet otherwise)
-                    if sig_p != b'P' or sig_a != b'a' or sig_v != b'V' or sig_e != b'E':
-                        continue
-
-                    # get remaining frame
-                    data += video_socket.recv(header - 12 + payload, socket.MSG_WAITALL)
-
-                    try:
-                        # decode the frame
-                        image = ardrone.video.decode(data)
-                        self.video_pipe.send(image)
-                    except ardrone.video.DecodeError:
-                        pass
-                elif i == nav_socket:
-                    while 1:
-                        try:
-                            data = nav_socket.recv(65535)
-                        except IOError:
-                            # we consumed every packet from the socket and
-                            # continue with the last one
-                            break
-                    navdata = ardrone.navdata.decode(data)
-                    self.nav_pipe.send(navdata)
-                elif i == self.com_pipe:
-                    _ = self.com_pipe.recv()
-                    stopping = True
+        while not self.stopping:
+            inputready, outputready, exceptready = select.select([nav_socket], [], [])
+            if len(inputready) < 1:
+                continue
+            while True:
+                try:
+                    data = nav_socket.recv(65535)
+                except IOError:
+                    # we consumed every packet from the socket and continue with the last one
                     break
-        video_socket.close()
+            navdata = ardrone.navdata.decode(data)
+            self.callback(navdata)
         nav_socket.close()
 
+    def stop(self):
+        """Stop the IPCThread activity."""
+        self.stopping = True
 
-class IPCThread(threading.Thread):
+
+class VidThread(threading.Thread):
     """Inter Process Communication Thread.
 
-    This thread collects the data from the ARDroneNetworkProcess and forwards
-    it to the ARDrone.
+    This thread collects video from the video port and makes it available
+    to the ARDrone.
     """
 
-    def __init__(self, drone):
+    def __init__(self, host, callback):
         threading.Thread.__init__(self)
-        self.drone = drone
+        self.host = host
+        self.callback = callback
         self.stopping = False
 
     def run(self):
+        video_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        video_socket.connect((self.host, ardrone.constant.VIDEO_PORT))
+
         while not self.stopping:
-            inputready, outputready, exceptready = select.select([self.drone.video_pipe, self.drone.nav_pipe], [], [], 1)
-            for i in inputready:
-                if i == self.drone.video_pipe:
-                    while self.drone.video_pipe.poll():
-                        width, height, image = self.drone.video_pipe.recv()
-                    self.drone.image = PIL.Image.frombuffer('RGB', (width, height), image, 'raw', 'RGB', 0, 1)
-                elif i == self.drone.nav_pipe:
-                    while self.drone.nav_pipe.poll():
-                        navdata = self.drone.nav_pipe.recv()
-                    self.drone.navdata = navdata
+            inputready, outputready, exceptready = select.select([video_socket], [], [])
+            if len(inputready) < 1:
+                continue
+            # get first few bytes of header
+            data = video_socket.recv(12, socket.MSG_WAITALL)
+            if len(data) != 12:
+                continue
+            # decode relevant portions of the header
+            sig_p, sig_a, sig_v, sig_e, version, codec, header, payload = struct.unpack('4cBBHI', data)
+            # check signature (and ignore packet otherwise)
+            if sig_p != b'P' or sig_a != b'a' or sig_v != b'V' or sig_e != b'E':
+                continue
+            # get remaining frame
+            data += video_socket.recv(header - 12 + payload, socket.MSG_WAITALL)
+            try:
+                img = ardrone.video.decode(data)
+                self.callback(img)
+            except ardrone.video.DecodeError:
+                pass
+        video_socket.close()
 
     def stop(self):
         """Stop the IPCThread activity."""

ardrone/video.c

@@ -34,7 +34,7 @@ struct PaVE {
 	uint8_t reserved1[2]; //padding to align to 48 bytes
 	uint32_t advertised_size; //size of advertised frame
 	uint8_t reserved2[12]; //padding to align to 64 bytes
-} __attribute__ ((packed));
+} __attribute__ (packed);
 
 static PyObject * VideoDecodeError;