server2

13 files changed, 1941 insertions, 0 deletions

diff --git a/libardrone/__init__.py b/libardrone/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/libardrone/__init__.py
diff --git a/libardrone/ar2video.py b/libardrone/ar2video.py
new file mode 100644
index 0000000..06c94a1
--- /dev/null
+++ b/libardrone/ar2video.py
@@ -0,0 +1,48 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2011 Bastian Venthur
+# Copyright (c) 2013 Adrian Taylor
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+
+"""
+Video decoding for the AR.Drone 2.0.
+
+This is just H.264 encapsulated in a funny way.
+"""
+
+import h264decoder
+import paveparser
+
+
+class ARVideo2(object):
+    def __init__(self, drone, debug=False):
+        h264 = h264decoder.H264Decoder(self, drone.image_shape)
+        self.paveparser = paveparser.PaVEParser(h264)
+        self._drone = drone
+
+    """
+    Called by the H264 decoder when there's an image ready
+    """
+    def image_ready(self, image):
+        self._drone.set_image(image)
+
+    def write(self, data):
+        self.paveparser.write(data)
diff --git a/libardrone/arnetwork.py b/libardrone/arnetwork.py
new file mode 100644
index 0000000..45964dd
--- /dev/null
+++ b/libardrone/arnetwork.py
@@ -0,0 +1,140 @@
+# Copyright (c) 2011 Bastian Venthur
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+import logging
+
+"""
+This module provides access to the data provided by the AR.Drone.
+"""
+import threading
+import select
+import socket
+import multiprocessing
+import libardrone
+
+class ARDroneNetworkProcess(threading.Thread):
+    """ARDrone Network Process.
+
+    This process collects data from the video and navdata port, converts the
+    data and sends it to the IPCThread.
+    """
+
+    def __init__(self, com_pipe, is_ar_drone_2, drone):
+        threading.Thread.__init__(self)
+        self._drone = drone
+        self.com_pipe = com_pipe
+        self.is_ar_drone_2 = is_ar_drone_2
+        self.stopping = False
+        if is_ar_drone_2:
+            import ar2video
+            self.ar2video = ar2video.ARVideo2(self._drone, libardrone.DEBUG)
+        else:
+            import arvideo
+
+    def run(self):
+
+        def _connect():
+            logging.warn('Connection to ardrone')
+            if self.is_ar_drone_2:
+                video_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+                video_socket.connect(('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT))
+                video_socket.setblocking(0)
+            else:
+                video_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+                video_socket.setblocking(0)
+                video_socket.bind(('', libardrone.ARDRONE_VIDEO_PORT))
+                video_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT))
+
+            nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
+            nav_socket.setblocking(0)
+            nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT))
+            nav_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_NAVDATA_PORT))
+
+            control_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+            control_socket.connect(('192.168.1.1', libardrone.ARDRONE_CONTROL_PORT))
+            control_socket.setblocking(0)
+            logging.warn('Connection established')
+            return video_socket, nav_socket, control_socket
+
+        def _disconnect(video_socket, nav_socket, control_socket):
+            logging.warn('Disconnection to ardrone streams')
+            video_socket.close()
+            nav_socket.close()
+            control_socket.close()
+
+        video_socket, nav_socket, control_socket = _connect()
+
+        self.stopping = False
+        connection_lost = 1
+        reconnection_needed = False
+        while not self.stopping:
+            if reconnection_needed:
+                _disconnect(video_socket, nav_socket, control_socket)
+                video_socket, nav_socket, control_socket = _connect()
+                reconnection_needed = False
+            inputready, outputready, exceptready = select.select([nav_socket, video_socket, self.com_pipe, control_socket], [], [], 1.)
+            if len(inputready) == 0:
+                connection_lost += 1
+                reconnection_needed = True
+            for i in inputready:
+                if i == video_socket:
+                    while 1:
+                        try:
+                            data = video_socket.recv(65536)
+                            if self.is_ar_drone_2:
+                                self.ar2video.write(data)
+                        except IOError:
+                            # we consumed every packet from the socket and
+                            # continue with the last one
+                            break
+                        # Sending is taken care of by the decoder
+                    if not self.is_ar_drone_2:
+                        w, h, image, t = arvideo.read_picture(data)
+                        self._drone.set_image(image)
+                elif i == nav_socket:
+                    while 1:
+                        try:
+                            data = nav_socket.recv(500)
+                        except IOError:
+                            # we consumed every packet from the socket and
+                            # continue with the last one
+                            break
+                    navdata, has_information = libardrone.decode_navdata(data)
+                    if (has_information):
+                        self._drone.set_navdata(navdata)
+                elif i == self.com_pipe:
+                    _ = self.com_pipe.recv()
+                    self.stopping = True
+                    break
+                elif i == control_socket:
+                    reconnection_needed = False
+                    while not reconnection_needed:
+                        try:
+                            data = control_socket.recv(65536)
+                            if len(data) == 0:
+                                logging.warning('Received an empty packet on control socket')
+                                reconnection_needed = True
+                            else:
+                                logging.warning("Control Socket says : %s", data)
+                        except IOError:
+                            break
+        _disconnect(video_socket, nav_socket, control_socket)
+
+    def terminate(self):
+        self.stopping = True
diff --git a/libardrone/arvideo.py b/libardrone/arvideo.py
new file mode 100644
index 0000000..4ae011c
--- /dev/null
+++ b/libardrone/arvideo.py
@@ -0,0 +1,579 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2011 Bastian Venthur
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+
+"""
+Video decoding for the AR.Drone.
+
+This library uses psyco to speed-up the decoding process. It is however written
+in a way that it works also without psyco installed. On the author's
+development machine the speed up is from 2FPS w/o psyco to > 20 FPS w/ psyco.
+"""
+
+
+import array
+import cProfile
+import datetime
+import struct
+import sys
+
+try:
+    import psyco
+except ImportError:
+    print "Please install psyco for better video decoding performance."
+
+
+# from zig-zag back to normal
+ZIG_ZAG_POSITIONS = array.array('B',
+    ( 0,  1,  8, 16,  9,  2, 3, 10,
+     17, 24, 32, 25, 18, 11, 4,  5,
+     12, 19, 26, 33, 40, 48, 41, 34,
+     27, 20, 13,  6,  7, 14, 21, 28,
+     35, 42, 49, 56, 57, 50, 43, 36,
+     29, 22, 15, 23, 30, 37, 44, 51,
+     58, 59, 52, 45, 38, 31, 39, 46,
+     53, 60, 61, 54, 47, 55, 62, 63))
+
+# Inverse quantization
+IQUANT_TAB = array.array('B',
+    ( 3,  5,  7,  9, 11, 13, 15, 17,
+      5,  7,  9, 11, 13, 15, 17, 19,
+      7,  9, 11, 13, 15, 17, 19, 21,
+      9, 11, 13, 15, 17, 19, 21, 23,
+     11, 13, 15, 17, 19, 21, 23, 25,
+     13, 15, 17, 19, 21, 23, 25, 27,
+     15, 17, 19, 21, 23, 25, 27, 29,
+     17, 19, 21, 23, 25, 27, 29, 31))
+
+# Used for upscaling the 8x8 b- and r-blocks to 16x16
+SCALE_TAB = array.array('B', 
+    ( 0,  0,  1,  1,  2,  2,  3,  3,
+      0,  0,  1,  1,  2,  2,  3,  3,
+      8,  8,  9,  9, 10, 10, 11, 11,
+      8,  8,  9,  9, 10, 10, 11, 11,
+     16, 16, 17, 17, 18, 18, 19, 19,
+     16, 16, 17, 17, 18, 18, 19, 19,
+     24, 24, 25, 25, 26, 26, 27, 27,
+     24, 24, 25, 25, 26, 26, 27, 27,
+
+      4,  4,  5,  5,  6,  6,  7,  7,
+      4,  4,  5,  5,  6,  6,  7,  7,
+     12, 12, 13, 13, 14, 14, 15, 15,
+     12, 12, 13, 13, 14, 14, 15, 15,
+     20, 20, 21, 21, 22, 22, 23, 23,
+     20, 20, 21, 21, 22, 22, 23, 23,
+     28, 28, 29, 29, 30, 30, 31, 31,
+     28, 28, 29, 29, 30, 30, 31, 31,
+
+     32, 32, 33, 33, 34, 34, 35, 35,
+     32, 32, 33, 33, 34, 34, 35, 35,
+     40, 40, 41, 41, 42, 42, 43, 43,
+     40, 40, 41, 41, 42, 42, 43, 43,
+     48, 48, 49, 49, 50, 50, 51, 51,
+     48, 48, 49, 49, 50, 50, 51, 51,
+     56, 56, 57, 57, 58, 58, 59, 59,
+     56, 56, 57, 57, 58, 58, 59, 59,
+
+     36, 36, 37, 37, 38, 38, 39, 39,
+     36, 36, 37, 37, 38, 38, 39, 39,
+     44, 44, 45, 45, 46, 46, 47, 47,
+     44, 44, 45, 45, 46, 46, 47, 47,
+     52, 52, 53, 53, 54, 54, 55, 55,
+     52, 52, 53, 53, 54, 54, 55, 55,
+     60, 60, 61, 61, 62, 62, 63, 63,
+     60, 60, 61, 61, 62, 62, 63, 63))
+
+# Count leading zeros look up table
+CLZLUT = array.array('B',
+    (8, 7, 6, 6, 5, 5, 5, 5, 4, 4, 4, 4, 4, 4, 4, 4,
+     3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3,
+     2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
+     2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
+     1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+     1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+     1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+     1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0))
+
+# Map pixels from four 8x8 blocks to one 16x16
+MB_TO_GOB_MAP = array.array('B',
+    [  0,   1,   2,   3,   4,   5,   6,   7,
+      16,  17,  18,  19,  20,  21,  22,  23,
+      32,  33,  34,  35,  36,  37,  38,  39,
+      48,  49,  50,  51,  52,  53,  54,  55,
+      64,  65,  66,  67,  68,  69,  70,  71,
+      80,  81,  82,  83,  84,  85,  86,  87,
+      96,  97,  98,  99, 100, 101, 102, 103,
+     112, 113, 114, 115, 116, 117, 118, 119,
+       8,   9,  10,  11,  12,  13,  14,  15,
+      24,  25,  26,  27,  28,  29,  30,  31,
+      40,  41,  42,  43,  44,  45,  46,  47,
+      56,  57,  58,  59,  60,  61,  62,  63,
+      72,  73,  74,  75,  76,  77,  78,  79,
+      88,  89,  90,  91,  92,  93,  94,  95,
+     104, 105, 106, 107, 108, 109, 110, 111,
+     120, 121, 122, 123, 124, 125, 126, 127,
+     128, 129, 130, 131, 132, 133, 134, 135,
+     144, 145, 146, 147, 148, 149, 150, 151,
+     160, 161, 162, 163, 164, 165, 166, 167,
+     176, 177, 178, 179, 180, 181, 182, 183,
+     192, 193, 194, 195, 196, 197, 198, 199,
+     208, 209, 210, 211, 212, 213, 214, 215,
+     224, 225, 226, 227, 228, 229, 230, 231,
+     240, 241, 242, 243, 244, 245, 246, 247,
+     136, 137, 138, 139, 140, 141, 142, 143,
+     152, 153, 154, 155, 156, 157, 158, 159,
+     168, 169, 170, 171, 172, 173, 174, 175,
+     184, 185, 186, 187, 188, 189, 190, 191,
+     200, 201, 202, 203, 204, 205, 206, 207,
+     216, 217, 218, 219, 220, 221, 222, 223,
+     232, 233, 234, 235, 236, 237, 238, 239,
+     248, 249, 250, 251, 252, 253, 254, 255])
+MB_ROW_MAP = array.array('B', [i / 16 for i in MB_TO_GOB_MAP])
+MB_COL_MAP = array.array('B', [i % 16 for i in MB_TO_GOB_MAP])
+
+# An array of zeros. It is much faster to take the zeros from here than to
+# generate a new list when needed.
+ZEROS = array.array('i', [0 for i in range(256)])
+
+# Constants needed for the inverse discrete cosine transform.
+FIX_0_298631336 = 2446
+FIX_0_390180644 = 3196
+FIX_0_541196100 = 4433
+FIX_0_765366865 = 6270
+FIX_0_899976223 = 7373
+FIX_1_175875602 = 9633
+FIX_1_501321110 = 12299
+FIX_1_847759065 = 15137
+FIX_1_961570560 = 16069
+FIX_2_053119869 = 16819
+FIX_2_562915447 = 20995
+FIX_3_072711026 = 25172
+CONST_BITS = 13
+PASS1_BITS = 1
+F1 = CONST_BITS - PASS1_BITS - 1
+F2 = CONST_BITS - PASS1_BITS
+F3 = CONST_BITS + PASS1_BITS + 3
+
+# tuning parameter for get_block
+TRIES = 16
+MASK = 2**(TRIES*32)-1
+SHIFT = 32*(TRIES-1)
+
+
+def _first_half(data):
+    """Helper function used to precompute the zero values in a 12 bit datum.
+    """
+    # data has to be 12 bits wide
+    streamlen = 0
+    # count the zeros
+    zerocount = CLZLUT[data >> 4];
+    data = (data << (zerocount + 1)) & 0b111111111111
+    streamlen += zerocount + 1
+    # get number of remaining bits to read
+    toread = 0 if zerocount <= 1 else zerocount - 1
+    additional = data >> (12 - toread)
+    data = (data << toread) & 0b111111111111
+    streamlen += toread
+    # add as many zeros to out_list as indicated by additional bits
+    # if zerocount is 0, tmp = 0 else the 1 merged with additional bits
+    tmp = 0 if zerocount == 0 else (1 << toread) | additional
+    return [streamlen, tmp]
+
+
+def _second_half(data):
+    """Helper function to precompute the nonzeror values in a 15 bit datum.
+    """
+    # data has to be 15 bits wide
+    streamlen = 0
+    zerocount = CLZLUT[data >> 7]
+    data = (data << (zerocount + 1)) & 0b111111111111111
+    streamlen += zerocount + 1
+    # 01 == EOB
+    eob = False
+    if zerocount == 1:
+        eob = True
+        return [streamlen, None, eob]
+    # get number of remaining bits to read
+    toread = 0 if zerocount == 0 else zerocount - 1
+    additional = data >> (15 - toread)
+    data = (data << toread) & 0b111111111111111
+    streamlen += toread
+    tmp = (1 << toread) | additional
+    # get one more bit for the sign
+    tmp = -tmp if data >> (15 - 1) else tmp
+    tmp = int(tmp)
+    streamlen += 1
+    return [streamlen, tmp, eob]
+
+
+# Precompute all 12 and 15 bit values for the entropy decoding process
+FH = [_first_half(i) for i in range(2**12)]
+SH = [_second_half(i) for i in range(2**15)]
+
+
+class BitReader(object):
+    """Bitreader. Given a stream of data, it allows to read it bitwise."""
+
+    def __init__(self, packet):
+        self.packet = packet
+        self.offset = 0
+        self.bits_left = 0
+        self.chunk = 0
+        self.read_bits = 0
+
+    def read(self, nbits, consume=True):
+        """Read nbits and return the integervalue of the read bits.
+
+        If consume is False, it behaves like a 'peek' method (ie it reads the
+        bits but does not consume them.
+        """
+        # Read enough bits into chunk so we have at least nbits available
+        while nbits > self.bits_left:
+            try:
+                self.chunk = (self.chunk << 32) | struct.unpack_from('<I', self.packet, self.offset)[0]
+            except struct.error:
+                self.chunk <<= 32
+            self.offset += 4
+            self.bits_left += 32
+        # Get the first nbits bits from chunk (and remove them from chunk)
+        shift = self.bits_left - nbits
+        res = self.chunk >> shift
+        if consume:
+            self.chunk -= res << shift
+            self.bits_left -= nbits
+            self.read_bits += nbits
+        return res
+
+    def align(self):
+        """Byte align the data stream."""
+        shift = (8 - self.read_bits) % 8
+        self.read(shift)
+
+
+def inverse_dct(block):
+    """Inverse discrete cosine transform.
+    """
+    workspace = ZEROS[0:64]
+    data = ZEROS[0:64]
+    for pointer in range(8):
+        if (block[pointer + 8] == 0 and block[pointer + 16] == 0 and
+            block[pointer + 24] == 0 and block[pointer + 32] == 0 and
+            block[pointer + 40] == 0 and block[pointer + 48] == 0 and
+            block[pointer + 56] == 0):
+            dcval = block[pointer] << PASS1_BITS
+            for i in range(8):
+                workspace[pointer + i*8] = dcval
+            continue
+
+        z2 = block[pointer + 16]
+        z3 = block[pointer + 48]
+        z1 = (z2 + z3) * FIX_0_541196100
+        tmp2 = z1 + z3 * -FIX_1_847759065
+        tmp3 = z1 + z2 * FIX_0_765366865
+        z2 = block[pointer]
+        z3 = block[pointer + 32]
+        tmp0 = (z2 + z3) << CONST_BITS
+        tmp1 = (z2 - z3) << CONST_BITS
+        tmp10 = tmp0 + tmp3
+        tmp13 = tmp0 - tmp3
+        tmp11 = tmp1 + tmp2
+        tmp12 = tmp1 - tmp2
+        tmp0 = block[pointer + 56]
+        tmp1 = block[pointer + 40]
+        tmp2 = block[pointer + 24]
+        tmp3 = block[pointer + 8]
+        z1 = tmp0 + tmp3
+        z2 = tmp1 + tmp2
+        z3 = tmp0 + tmp2
+        z4 = tmp1 + tmp3
+        z5 = (z3 + z4) * FIX_1_175875602
+        tmp0 *= FIX_0_298631336
+        tmp1 *= FIX_2_053119869
+        tmp2 *= FIX_3_072711026
+        tmp3 *= FIX_1_501321110
+        z1 *= -FIX_0_899976223
+        z2 *= -FIX_2_562915447
+        z3 *= -FIX_1_961570560
+        z4 *= -FIX_0_390180644
+        z3 += z5
+        z4 += z5
+        tmp0 += z1 + z3
+        tmp1 += z2 + z4
+        tmp2 += z2 + z3
+        tmp3 += z1 + z4
+        workspace[pointer + 0] = ((tmp10 + tmp3 + (1 << F1)) >> F2)
+        workspace[pointer + 56] = ((tmp10 - tmp3 + (1 << F1)) >> F2)
+        workspace[pointer + 8] = ((tmp11 + tmp2 + (1 << F1)) >> F2)
+        workspace[pointer + 48] = ((tmp11 - tmp2 + (1 << F1)) >> F2)
+        workspace[pointer + 16] = ((tmp12 + tmp1 + (1 << F1)) >> F2)
+        workspace[pointer + 40] = ((tmp12 - tmp1 + (1 << F1)) >> F2)
+        workspace[pointer + 24] = ((tmp13 + tmp0 + (1 << F1)) >> F2)
+        workspace[pointer + 32] = ((tmp13 - tmp0 + (1 << F1)) >> F2)
+
+    for pointer in range(0, 64, 8):
+        z2 = workspace[pointer + 2]
+        z3 = workspace[pointer + 6]
+        z1 = (z2 + z3) * FIX_0_541196100
+        tmp2 = z1 + z3 * -FIX_1_847759065
+        tmp3 = z1 + z2 * FIX_0_765366865
+        tmp0 = (workspace[pointer] + workspace[pointer + 4]) << CONST_BITS
+        tmp1 = (workspace[pointer] - workspace[pointer + 4]) << CONST_BITS
+        tmp10 = tmp0 + tmp3
+        tmp13 = tmp0 - tmp3
+        tmp11 = tmp1 + tmp2
+        tmp12 = tmp1 - tmp2
+        tmp0 = workspace[pointer + 7]
+        tmp1 = workspace[pointer + 5]
+        tmp2 = workspace[pointer + 3]
+        tmp3 = workspace[pointer + 1]
+        z1 = tmp0 + tmp3
+        z2 = tmp1 + tmp2
+        z3 = tmp0 + tmp2
+        z4 = tmp1 + tmp3
+        z5 = (z3 + z4) * FIX_1_175875602
+        tmp0 *= FIX_0_298631336
+        tmp1 *= FIX_2_053119869
+        tmp2 *= FIX_3_072711026
+        tmp3 *= FIX_1_501321110
+        z1 *= -FIX_0_899976223
+        z2 *= -FIX_2_562915447
+        z3 *= -FIX_1_961570560
+        z4 *= -FIX_0_390180644
+        z3 += z5
+        z4 += z5
+        tmp0 += z1 + z3
+        tmp1 += z2 + z4
+        tmp2 += z2 + z3
+        tmp3 += z1 + z4
+        data[pointer + 0] = (tmp10 + tmp3) >> F3
+        data[pointer + 7] = (tmp10 - tmp3) >> F3
+        data[pointer + 1] = (tmp11 + tmp2) >> F3
+        data[pointer + 6] = (tmp11 - tmp2) >> F3
+        data[pointer + 2] = (tmp12 + tmp1) >> F3
+        data[pointer + 5] = (tmp12 - tmp1) >> F3
+        data[pointer + 3] = (tmp13 + tmp0) >> F3
+        data[pointer + 4] = (tmp13 - tmp0) >> F3
+
+    return data
+
+
+def get_pheader(bitreader):
+    """Read the picture header.
+
+    Returns the width and height of the image.
+    """
+    bitreader.align()
+    psc = bitreader.read(22)
+    assert(psc == 0b0000000000000000100000)
+    pformat = bitreader.read(2)
+    assert(pformat != 0b00)
+    if pformat == 1:
+        # CIF
+        width, height = 88, 72
+    else:
+        # VGA
+        width, height = 160, 120
+    presolution = bitreader.read(3)
+    assert(presolution != 0b000)
+    # double resolution presolution-1 times
+    width = width << presolution - 1
+    height = height << presolution - 1
+    #print "width/height:", width, height
+    ptype = bitreader.read(3)
+    pquant = bitreader.read(5)
+    pframe = bitreader.read(32)
+    return width, height
+
+
+def get_mb(bitreader, picture, width, offset):
+    """Get macro block.
+
+    This method does not return data but modifies the picture parameter in
+    place.
+    """
+    mbc = bitreader.read(1)
+    if mbc == 0:
+        mbdesc = bitreader.read(8)
+        assert(mbdesc >> 7 & 1)
+        if mbdesc >> 6 & 1:
+            mbdiff = bitreader.read(2)
+        y = get_block(bitreader, mbdesc & 1)
+        y.extend(get_block(bitreader, mbdesc >> 1 & 1))
+        y.extend(get_block(bitreader, mbdesc >> 2 & 1))
+        y.extend(get_block(bitreader, mbdesc >> 3 & 1))
+        cb = get_block(bitreader, mbdesc >> 4 & 1)
+        cr = get_block(bitreader, mbdesc >> 5 & 1)
+        # ycbcr to rgb
+        for i in range(256):
+            j = SCALE_TAB[i]
+            Y = y[i] - 16
+            B = cb[j] - 128
+            R = cr[j] - 128
+            r = (298 * Y           + 409 * R + 128) >> 8
+            g = (298 * Y - 100 * B - 208 * R + 128) >> 8
+            b = (298 * Y + 516 * B           + 128) >> 8
+            r = 0 if r < 0 else r
+            r = 255 if r > 255 else r
+            g = 0 if g < 0 else g
+            g = 255 if g > 255 else g
+            b = 0 if b < 0 else b
+            b = 255 if b > 255 else b
+            # re-order the pixels
+            row = MB_ROW_MAP[i]
+            col = MB_COL_MAP[i]
+            picture[offset + row*width + col] = ''.join((chr(r), chr(g), chr(b)))
+    else:
+        print "mbc was not zero"
+
+
+def get_block(bitreader, has_coeff):
+    """Read a 8x8 block from the data stream.
+
+    This method takes care of the huffman-, RLE, zig-zag and idct and returns a
+    list of 64 ints.
+    """
+    # read the first 10 bits in a 16 bit datum
+    out_list = ZEROS[0:64]
+    out_list[0] = int(bitreader.read(10)) * IQUANT_TAB[0]
+    if not has_coeff:
+        return inverse_dct(out_list)
+    i = 1
+    while 1:
+        _ = bitreader.read(32*TRIES, False)
+        streamlen = 0
+        #######################################################################
+        for j in range(TRIES):
+            data = (_ << streamlen) & MASK
+            data >>= SHIFT
+
+            l, tmp = FH[data >> 20]
+            streamlen += l
+            data = (data << l) & 0xffffffff
+            i += tmp
+
+            l, tmp, eob = SH[data >> 17]
+            streamlen += l
+            if eob:
+                bitreader.read(streamlen)
+                return inverse_dct(out_list)
+            j = ZIG_ZAG_POSITIONS[i]
+            out_list[j] = tmp*IQUANT_TAB[j]
+            i += 1
+        #######################################################################
+        bitreader.read(streamlen)
+    return inverse_dct(out_list)
+
+
+def get_gob(bitreader, picture, slicenr, width):
+    """Read a group of blocks.
+
+    The method does not return data, the picture parameter is modified in place
+    instead.
+    """
+    # the first gob has a special header
+    if slicenr > 0:
+        bitreader.align()
+        gobsc = bitreader.read(22)
+        if gobsc == 0b0000000000000000111111:
+            print "weeeee"
+            return False
+        elif (not (gobsc & 0b0000000000000000100000) or
+             (gobsc & 0b1111111111111111000000)):
+            print "Got wrong GOBSC, aborting.", bin(gobsc)
+            return False
+        _ = bitreader.read(5)
+    offset = slicenr*16*width
+    for i in range(width / 16):
+        get_mb(bitreader, picture, width, offset+16*i)
+
+
+def read_picture(data):
+    """Convert an AR.Drone image packet to rgb-string.
+
+    Returns: width, height, image and time to decode the image
+    """
+    bitreader = BitReader(data)
+    t = datetime.datetime.now()
+    width, height = get_pheader(bitreader)
+    slices = height / 16
+    blocks = width / 16
+    image = [0 for i in range(width*height)]
+    for i in range(0, slices):
+        get_gob(bitreader, image, i, width)
+    bitreader.align()
+    eos = bitreader.read(22)
+    assert(eos == 0b0000000000000000111111)
+    t2 = datetime.datetime.now()
+    return width, height, ''.join(image), (t2 - t).microseconds / 1000000.
+
+
+try:
+    psyco.bind(BitReader)
+    psyco.bind(get_block)
+    psyco.bind(get_gob)
+    psyco.bind(get_mb)
+    psyco.bind(inverse_dct)
+    psyco.bind(read_picture)
+except NameError:
+    print "Unable to bind video decoding methods with psyco. Proceeding anyways, but video decoding will be slow!"
+
+
+def main():
+    fh = open('framewireshark.raw', 'r')
+    #fh = open('videoframe.raw', 'r')
+    data = fh.read()
+    fh.close()
+    runs = 20
+    t = 0
+    for i in range(runs):
+        print '.',
+        width, height, image, ti = read_picture(data)
+        #show_image(image, width, height)
+        t += ti
+    print
+    print 'avg time:\t', t / runs, 'sec'
+    print 'avg fps:\t', 1 / (t / runs), 'fps'
+    if 'image' in sys.argv:
+        import pygame
+        pygame.init()
+        W, H = 320, 240
+        screen = pygame.display.set_mode((W, H))
+        surface = pygame.image.fromstring(image, (width, height), 'RGB')
+        screen.blit(surface, (0, 0))
+        pygame.display.flip()
+        raw_input()
+
+
+if __name__ == '__main__':
+    if 'profile' in sys.argv:
+        cProfile.run('main()')
+    else:
+        main()
+
diff --git a/libardrone/h264decoder.py b/libardrone/h264decoder.py
new file mode 100644
index 0000000..87d4625
--- /dev/null
+++ b/libardrone/h264decoder.py
@@ -0,0 +1,123 @@
+# -*- coding: utf-8 -*-
+# Copyright (c) 2013 Adrian Taylor
+# Inspired by equivalent node.js code by Felix Geisendörfer
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+import os
+
+
+"""
+H.264 video decoder for AR.Drone 2.0. Uses ffmpeg.
+"""
+
+import sys
+from subprocess import PIPE, Popen
+from threading  import Thread
+import time
+import libardrone
+import ctypes
+import numpy as np
+import sys
+
+
+try:
+    from Queue import Queue, Empty
+except ImportError:
+    from queue import Queue, Empty  # python 3.x
+
+ON_POSIX = 'posix' in sys.builtin_module_names
+
+
+def enqueue_output(out, outfileobject, frame_size):
+    frame_size_bytes = frame_size[0] * frame_size[1] * 3
+    while True:
+        buffer_str = out.read(frame_size_bytes)
+        im = np.frombuffer(buffer_str, count=frame_size_bytes, dtype=np.uint8)
+        im = im.reshape((frame_size[0], frame_size[1], 3))
+        outfileobject.image_ready(im)
+
+
+# Logic for making ffmpeg terminate on the death of this process
+def set_death_signal(signal):
+    libc = ctypes.CDLL('libc.so.6')
+    PR_SET_DEATHSIG = 1
+    libc.prctl(PR_SET_DEATHSIG, signal)
+
+
+def set_death_signal_int():
+    if sys.platform != 'darwin':
+        SIGINT = 2
+        SIGTERM = 15
+        set_death_signal(SIGINT)
+
+
+"""
+Usage: pass a listener, with a method 'data_ready' which will be called whenever there's output
+from ffmpeg. This will be called in an arbitrary thread. You can later call H264ToPng.get_data_if_any to retrieve
+said data.
+You should then call write repeatedly to write some encoded H.264 data.
+"""
+class H264Decoder(object):
+
+    def __init__(self, outfileobject=None, frame_size=(360, 640)):
+        if outfileobject is not None:
+
+            if (H264Decoder.which('ffmpeg') is None):
+                raise Exception("You need to install ffmpeg to be able to run ardrone")
+
+            p = Popen(["nice", "-n", "0", "ffmpeg", "-i", "-", "-f", "sdl",
+                       "-probesize", "2048", "-flags", "low_delay", "-f",
+                       "rawvideo", "-pix_fmt", 'rgb24', "-"],
+                      stdin=PIPE, stdout=PIPE, stderr=open('/dev/null', 'w'),
+                      bufsize=0, preexec_fn=set_death_signal_int)
+            t = Thread(target=enqueue_output, args=(p.stdout, outfileobject, frame_size))
+            t.daemon = True # thread dies with the program
+            t.start()
+        else:
+            if (H264Decoder.which('ffplay') is None):
+                raise Exception("You need to install ffmpeg and ffplay to be able to run ardrone in debug mode")
+
+            p = Popen(["nice", "-n", "15", "ffplay", "-probesize", "2048",
+                       "-flags", "low_delay", "-i", "-"],
+                      stdin=PIPE, stdout=open('/dev/null', 'w'),
+                      stderr=open('/dev/null', 'w'), bufsize=-1,
+                      preexec_fn=set_death_signal_int)
+
+        self.writefd = p.stdin
+
+    def write(self, data):
+        self.writefd.write(data)
+
+    @staticmethod
+    def which(program):
+        def is_exe(fpath):
+            return os.path.isfile(fpath) and os.access(fpath, os.X_OK)
+
+        fpath, fname = os.path.split(program)
+        if fpath:
+            if is_exe(program):
+                return program
+        else:
+            for path in os.environ["PATH"].split(os.pathsep):
+                path = path.strip('"')
+                exe_file = os.path.join(path, program)
+                if is_exe(exe_file):
+                    return exe_file
+
+        return None
diff --git a/libardrone/libardrone.py b/libardrone/libardrone.py
new file mode 100644
index 0000000..348ce0f
--- /dev/null
+++ b/libardrone/libardrone.py
@@ -0,0 +1,690 @@
+# Copyright (c) 2011 Bastian Venthur
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+"""
+Python library for the AR.Drone.
+
+V.1 This module was tested with Python 2.6.6 and AR.Drone vanilla firmware 1.5.1.
+V.2.alpha
+"""
+
+import logging
+import socket
+import struct
+import sys
+import threading
+import multiprocessing
+
+import arnetwork
+
+import time
+import numpy as np
+
+__author__ = "Bastian Venthur"
+
+ARDRONE_NAVDATA_PORT = 5554
+ARDRONE_VIDEO_PORT = 5555
+ARDRONE_COMMAND_PORT = 5556
+ARDRONE_CONTROL_PORT = 5559
+
+SESSION_ID = "943dac23"
+USER_ID = "36355d78"
+APP_ID = "21d958e4"
+
+DEBUG = False
+
+# 0: "Not defined"
+# 131072:  "Landed"
+# 393216:  "Taking-off-Floor"
+# 393217:  "Taking-off-Air"
+# 262144:  "Hovering"
+# 524288:  "Landing"
+# 458752:  "Stabilizing"
+# 196608:  "Moving"
+# 262153 and 196613 and 262155 and 196614 and 458753:  "Undefined"
+ctrl_state_dict={0:0, 131072:1, 393216:2, 393217:3, 262144:4, 524288:5, 458752:6, 196608:7, 262153:8, 196613:9, 262155:10, 196614:11, 458753: 12}
+
+
+class ARDrone(object):
+    """ARDrone Class.
+
+    Instanciate this class to control your drone and receive decoded video and
+    navdata.
+    Possible value for video codec (drone2):
+      NULL_CODEC    = 0,
+      UVLC_CODEC    = 0x20,       // codec_type value is used for START_CODE
+      P264_CODEC    = 0x40,
+      MP4_360P_CODEC = 0x80,
+      H264_360P_CODEC = 0x81,
+      MP4_360P_H264_720P_CODEC = 0x82,
+      H264_720P_CODEC = 0x83,
+      MP4_360P_SLRS_CODEC = 0x84,
+      H264_360P_SLRS_CODEC = 0x85,
+      H264_720P_SLRS_CODEC = 0x86,
+      H264_AUTO_RESIZE_CODEC = 0x87,    // resolution is automatically adjusted according to bitrate
+      MP4_360P_H264_360P_CODEC = 0x88,
+    """
+
+    def __init__(self, is_ar_drone_2=False, hd=False):
+
+        self.seq_nr = 1
+        self.timer_t = 0.2
+        self.com_watchdog_timer = threading.Timer(self.timer_t, self.commwdg)
+        self.lock = threading.Lock()
+        self.speed = 0.2
+        self.hd = hd
+        if (self.hd):
+            self.image_shape = (720, 1280, 3)
+        else:
+            self.image_shape = (360, 640, 3)
+
+        time.sleep(0.5)
+        self.config_ids_string = [SESSION_ID, USER_ID, APP_ID]
+        self.configure_multisession(SESSION_ID, USER_ID, APP_ID, self.config_ids_string)
+        self.set_session_id (self.config_ids_string, SESSION_ID)
+        time.sleep(0.5)
+        self.set_profile_id(self.config_ids_string, USER_ID)
+        time.sleep(0.5)
+        self.set_app_id(self.config_ids_string, APP_ID)
+        time.sleep(0.5)
+        self.set_video_bitrate_control_mode(self.config_ids_string, "1")
+        time.sleep(0.5)
+        self.set_video_bitrate(self.config_ids_string, "10000")
+        time.sleep(0.5)
+        self.set_max_bitrate(self.config_ids_string, "10000")
+        time.sleep(0.5)
+        self.set_fps(self.config_ids_string, "30")
+        time.sleep(0.5)
+        if (self.hd):
+            self.set_video_codec(self.config_ids_string, 0x83)
+        else:
+            self.set_video_codec(self.config_ids_string, 0x88)
+
+        self.last_command_is_hovering = True
+        self.com_pipe, com_pipe_other = multiprocessing.Pipe()
+
+        self.navdata = dict()
+        self.navdata[0] = dict(zip(['ctrl_state', 'battery', 'theta', 'phi', 'psi', 'altitude', 'vx', 'vy', 'vz', 'num_frames'], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
+
+        self.network_process = arnetwork.ARDroneNetworkProcess(com_pipe_other, is_ar_drone_2, self)
+        self.network_process.start()
+
+        self.image = np.zeros(self.image_shape, np.uint8)
+        self.time = 0
+
+        self.last_command_is_hovering = True
+
+        time.sleep(1.0)
+
+        self.at(at_config_ids , self.config_ids_string)
+        self.at(at_config, "general:navdata_demo", "TRUE")
+
+
+    def takeoff(self):
+        """Make the drone takeoff."""
+        self.at(at_ftrim)
+        self.at(at_config, "control:altitude_max", "20000")
+        self.at(at_ref, True)
+
+    def set_max_alt(self, alt):
+        self.at(at_config_ids , self.config_ids_string)
+        self.at(at_config, "control:altitude_max", alt)
+
+    def set_max_vz(self, speed):
+        self.at(at_config_ids , self.config_ids_string)
+        self.at(at_config, "control:control_vz_max", speed)
+
+    def set_max_angle(self, eul):
+        self.at(at_config_ids , self.config_ids_string)
+        self.at(at_config, "control:euler_angle_max", eul)
+
+    def set_max_rotspeed(self, speed):
+        self.at(at_config_ids , self.config_ids_string)
+        self.at(at_config, "control:control_yaw", speed)
+
+    def land(self):
+        """Make the drone land."""
+        self.at(at_ref, False)
+
+    def hover(self):
+        """Make the drone hover."""
+        self.at(at_pcmd, False, 0, 0, 0, 0)
+
+    def move_freely(self, hov, x,y, alt, rot):
+        self.at(at_pcmd, hov, x, y, alt, rot)
+
+    def move_left(self):
+        """Make the drone move left."""
+        self.at(at_pcmd, True, -self.speed, 0, 0, 0)
+
+    def move_right(self):
+        """Make the drone move right."""
+        self.at(at_pcmd, True, self.speed, 0, 0, 0)
+
+    def move_up(self):
+        """Make the drone rise upwards."""
+        self.at(at_pcmd, True, 0, 0, self.speed, 0)
+
+    def move_down(self):
+        """Make the drone decent downwards."""
+        self.at(at_pcmd, True, 0, 0, -self.speed, 0)
+
+    def move_forward(self):
+        """Make the drone move forward."""
+        self.at(at_pcmd, True, 0, -self.speed, 0, 0)
+
+    def move_backward(self):
+        """Make the drone move backwards."""
+        self.at(at_pcmd, True, 0, self.speed, 0, 0)
+
+    def turn_left(self):
+        """Make the drone rotate left."""
+        self.at(at_pcmd, True, 0, 0, 0, -self.speed)
+
+    def turn_right(self):
+        """Make the drone rotate right."""
+        self.at(at_pcmd, True, 0, 0, 0, self.speed)
+
+    def reset(self):
+        """Toggle the drone's emergency state."""
+        # Enter emergency mode
+        self.at(at_ref, False, True)
+        self.at(at_ref, False, False)
+        # Leave emergency mode
+        self.at(at_ref, False, True)
+
+    def trim(self):
+        """Flat trim the drone."""
+        self.at(at_ftrim)
+
+    def set_speed(self, speed):
+        """Set the drone's speed.
+
+        Valid values are floats from [0..1]
+        """
+        self.speed = speed
+
+    def set_camera_view(self, downward):
+        """
+        Set which video camera is used. If 'downward' is true,
+        downward camera will be viewed - otherwise frontwards.
+        """
+        channel = None
+        if downward:
+            channel = 0
+        else:
+            channel = 1
+        self.set_video_channel(self.config_ids_string, channel)
+
+    def at(self, cmd, *args, **kwargs):
+        """Wrapper for the low level at commands.
+
+        This method takes care that the sequence number is increased after each
+        at command and the watchdog timer is started to make sure the drone
+        receives a command at least every second.
+        """
+        self.lock.acquire()
+        self.com_watchdog_timer.cancel()
+        cmd(self.seq_nr, *args, **kwargs)
+        self.seq_nr += 1
+        self.com_watchdog_timer = threading.Timer(self.timer_t, self.commwdg)
+        self.com_watchdog_timer.start()
+        self.lock.release()
+
+    def configure_multisession(self, session_id, user_id, app_id, config_ids_string):
+        self.at(at_config, "custom:session_id", session_id)
+        self.at(at_config, "custom:profile_id", user_id)
+        self.at(at_config, "custom:application_id", app_id)
+
+    def set_session_id (self, config_ids_string, session_id):
+        self.at(at_config_ids , config_ids_string)
+        self.at(at_config, "custom:session_id", session_id)
+
+    def set_profile_id (self, config_ids_string, profile_id):
+        self.at(at_config_ids , config_ids_string)
+        self.at(at_config, "custom:profile_id", profile_id)
+
+    def set_app_id (self, config_ids_string, app_id):
+        self.at(at_config_ids , config_ids_string)
+        self.at(at_config, "custom:application_id", app_id)
+
+    def set_video_bitrate_control_mode (self, config_ids_string, mode):
+        self.at(at_config_ids , config_ids_string)
+        self.at(at_config, "video:bitrate_control_mode", mode)
+
+    def set_video_bitrate (self, config_ids_string, bitrate):
+        self.at(at_config_ids , config_ids_string)
+        self.at(at_config, "video:bitrate", bitrate)
+
+    def set_video_channel(self, config_ids_string, channel):
+        self.at(at_config_ids , config_ids_string)
+        self.at(at_config, "video:video_channel", channel)
+
+    def set_max_bitrate(self, config_ids_string, max_bitrate):
+        self.at(at_config_ids , config_ids_string)
+        self.at(at_config, "video:max_bitrate", max_bitrate)
+
+    def set_fps (self, config_ids_string, fps):
+        self.at(at_config_ids , config_ids_string)
+        self.at(at_config, "video:codec_fps", fps)
+
+    def set_video_codec (self, config_ids_string, codec):
+        self.at(at_config_ids , config_ids_string)
+        self.at(at_config, "video:video_codec", codec)
+
+    def commwdg(self):
+        """Communication watchdog signal.
+
+        This needs to be send regulary to keep the communication w/ the drone
+        alive.
+        """
+        self.at(at_comwdg)
+
+    def halt(self):
+        """Shutdown the drone.
+
+        This method does not land or halt the actual drone, but the
+        communication with the drone. You should call it at the end of your
+        application to close all sockets, pipes, processes and threads related
+        with this object.
+        """
+        self.lock.acquire()
+        self.com_watchdog_timer.cancel()
+        self.com_pipe.send('die!')
+        self.network_process.terminate()
+        self.network_process.join()
+        self.lock.release()
+
+    def get_image(self):
+        _im = np.copy(self.image)
+        return _im
+
+    def get_navdata(self):
+        return self.navdata
+
+    def set_navdata(self, navdata):
+        self.navdata = navdata
+        self.get_navdata()
+
+    def set_image(self, image):
+        if (image.shape == self.image_shape):
+            self.image = image
+        self.image = image
+
+    def apply_command(self, command):
+        available_commands = ["emergency",
+        "land", "takeoff", "move_left", "move_right", "move_down", "move_up",
+        "move_backward", "move_forward", "turn_left", "turn_right", "hover"]
+        if command not in available_commands:
+            logging.error("Command %s is not a recognized command" % command)
+
+        if command != "hover":
+            self.last_command_is_hovering = False
+
+        if (command == "emergency"):
+            self.reset()
+        elif (command == "land"):
+            self.land()
+            self.last_command_is_hovering = True
+        elif (command == "takeoff"):
+            self.takeoff()
+            self.last_command_is_hovering = True
+        elif (command == "move_left"):
+            self.move_left()
+        elif (command == "move_right"):
+            self.move_right()
+        elif (command == "move_down"):
+            self.move_down()
+        elif (command == "move_up"):
+            self.move_up()
+        elif (command == "move_backward"):
+            self.move_backward()
+        elif (command == "move_forward"):
+            self.move_forward()
+        elif (command == "turn_left"):
+            self.turn_left()
+        elif (command == "turn_right"):
+            self.turn_right()
+        elif (command == "hover" and not self.last_command_is_hovering):
+            self.hover()
+            self.last_command_is_hovering = True
+
+class ARDrone2(ARDrone):
+    def __init__(self, hd=False):
+        ARDrone.__init__(self, True, hd)
+
+###############################################################################
+### Low level AT Commands
+###############################################################################
+
+def at_ref(seq, takeoff, emergency=False):
+    """
+    Basic behaviour of the drone: take-off/landing, emergency stop/reset)
+
+    Parameters:
+    seq -- sequence number
+    takeoff -- True: Takeoff / False: Land
+    emergency -- True: Turn off the engines
+    """
+    p = 0b10001010101000000000000000000
+    if takeoff:
+        p += 0b1000000000
+    if emergency:
+        p += 0b0100000000
+    at("REF", seq, [p])
+
+def at_pcmd(seq, progressive, lr, fb, vv, va):
+    """
+    Makes the drone move (translate/rotate).
+
+    Parameters:
+    seq -- sequence number
+    progressive -- True: enable progressive commands, False: disable (i.e.
+        enable hovering mode)
+    lr -- left-right tilt: float [-1..1] negative: left, positive: right
+    rb -- front-back tilt: float [-1..1] negative: forwards, positive:
+        backwards
+    vv -- vertical speed: float [-1..1] negative: go down, positive: rise
+    va -- angular speed: float [-1..1] negative: spin left, positive: spin
+        right
+
+    The above float values are a percentage of the maximum speed.
+    """
+    p = 1 if progressive else 0
+    at("PCMD", seq, [p, float(lr), float(fb), float(vv), float(va)])
+
+def at_ftrim(seq):
+    """
+    Tell the drone it's lying horizontally.
+
+    Parameters:
+    seq -- sequence number
+    """
+    at("FTRIM", seq, [])
+
+def at_zap(seq, stream):
+    """
+    Selects which video stream to send on the video UDP port.
+
+    Parameters:
+    seq -- sequence number
+    stream -- Integer: video stream to broadcast
+    """
+    # FIXME: improve parameters to select the modes directly
+    at("ZAP", seq, [stream])
+
+def at_config(seq, option, value):
+    """Set configuration parameters of the drone."""
+    at("CONFIG", seq, [str(option), str(value)])
+
+def at_config_ids(seq, value):
+    """Set configuration parameters of the drone."""
+    at("CONFIG_IDS", seq, value)
+
+def at_ctrl(seq, num):
+    """Ask the parrot to drop its configuration file"""
+    at("CTRL", seq, [num, 0])
+
+def at_comwdg(seq):
+    """
+    Reset communication watchdog.
+    """
+    # FIXME: no sequence number
+    at("COMWDG", seq, [])
+
+def at_aflight(seq, flag):
+    """
+    Makes the drone fly autonomously.
+
+    Parameters:
+    seq -- sequence number
+    flag -- Integer: 1: start flight, 0: stop flight
+    """
+    at("AFLIGHT", seq, [flag])
+
+def at_pwm(seq, m1, m2, m3, m4):
+    """
+    Sends control values directly to the engines, overriding control loops.
+
+    Parameters:
+    seq -- sequence number
+    m1 -- front left command
+    m2 -- fright right command
+    m3 -- back right command
+    m4 -- back left command
+    """
+    # FIXME: what type do mx have?
+    raise NotImplementedError()
+
+def at_led(seq, anim, f, d):
+    """
+    Control the drones LED.
+
+    Parameters:
+    seq -- sequence number
+    anim -- Integer: animation to play
+    f -- ?: frequence in HZ of the animation
+    d -- Integer: total duration in seconds of the animation
+    """
+    pass
+
+def at_anim(seq, anim, d):
+    """
+    Makes the drone execute a predefined movement (animation).
+
+    Parameters:
+    seq -- sequcence number
+    anim -- Integer: animation to play
+    d -- Integer: total duration in sections of the animation
+    """
+    at("ANIM", seq, [anim, d])
+
+def at(command, seq, params):
+    """
+    Parameters:
+    command -- the command
+    seq -- the sequence number
+    params -- a list of elements which can be either int, float or string
+    """
+    param_str = ''
+    for p in params:
+        if type(p) == int:
+            param_str += ",%d" % p
+        elif type(p) == float:
+            param_str += ",%d" % f2i(p)
+        elif type(p) == str:
+            param_str += ',"' + p + '"'
+    msg = "AT*%s=%i%s\r" % (command, seq, param_str)
+    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock.sendto(msg.encode("utf-8"), ("192.168.1.1", ARDRONE_COMMAND_PORT))
+
+def f2i(f):
+    """Interpret IEEE-754 floating-point value as signed integer.
+
+    Arguments:
+    f -- floating point value
+    """
+    return struct.unpack('i', struct.pack('f', f))[0]
+
+###############################################################################
+### navdata
+###############################################################################
+def decode_navdata(packet):
+    """Decode a navdata packet."""
+    offset = 0
+    _ = struct.unpack_from("IIII", packet, offset)
+    drone_state = dict()
+    drone_state['fly_mask'] = _[1] & 1 # FLY MASK : (0) ardrone is landed, (1) ardrone is flying
+    drone_state['video_mask'] = _[1] >> 1 & 1 # VIDEO MASK : (0) video disable, (1) video enable
+    drone_state['vision_mask'] = _[1] >> 2 & 1 # VISION MASK : (0) vision disable, (1) vision enable */
+    drone_state['control_mask'] = _[1] >> 3 & 1 # CONTROL ALGO (0) euler angles control, (1) angular speed control */
+    drone_state['altitude_mask'] = _[1] >> 4 & 1 # ALTITUDE CONTROL ALGO : (0) altitude control inactive (1) altitude control active */
+    drone_state['user_feedback_start'] = _[1] >> 5 & 1 # USER feedback : Start button state */
+    drone_state['command_mask'] = _[1] >> 6 & 1 # Control command ACK : (0) None, (1) one received */
+    drone_state['fw_file_mask'] = _[1] >> 7 & 1 # Firmware file is good (1) */
+    drone_state['fw_ver_mask'] = _[1] >> 8 & 1 # Firmware update is newer (1) */
+    drone_state['fw_upd_mask'] = _[1] >> 9 & 1 # Firmware update is ongoing (1) */
+    drone_state['navdata_demo_mask'] = _[1] >> 10 & 1 # Navdata demo : (0) All navdata, (1) only navdata demo */
+    drone_state['navdata_bootstrap'] = _[1] >> 11 & 1 # Navdata bootstrap : (0) options sent in all or demo mode, (1) no navdata options sent */
+    drone_state['motors_mask'] = _[1] >> 12 & 1 # Motor status : (0) Ok, (1) Motors problem */
+    drone_state['com_lost_mask'] = _[1] >> 13 & 1 # Communication lost : (1) com problem, (0) Com is ok */
+    drone_state['vbat_low'] = _[1] >> 15 & 1 # VBat low : (1) too low, (0) Ok */
+    drone_state['user_el'] = _[1] >> 16 & 1 # User Emergency Landing : (1) User EL is ON, (0) User EL is OFF*/
+    drone_state['timer_elapsed'] = _[1] >> 17 & 1 # Timer elapsed : (1) elapsed, (0) not elapsed */
+    drone_state['angles_out_of_range'] = _[1] >> 19 & 1 # Angles : (0) Ok, (1) out of range */
+    drone_state['ultrasound_mask'] = _[1] >> 21 & 1 # Ultrasonic sensor : (0) Ok, (1) deaf */
+    drone_state['cutout_mask'] = _[1] >> 22 & 1 # Cutout system detection : (0) Not detected, (1) detected */
+    drone_state['pic_version_mask'] = _[1] >> 23 & 1 # PIC Version number OK : (0) a bad version number, (1) version number is OK */
+    drone_state['atcodec_thread_on'] = _[1] >> 24 & 1 # ATCodec thread ON : (0) thread OFF (1) thread ON */
+    drone_state['navdata_thread_on'] = _[1] >> 25 & 1 # Navdata thread ON : (0) thread OFF (1) thread ON */
+    drone_state['video_thread_on'] = _[1] >> 26 & 1 # Video thread ON : (0) thread OFF (1) thread ON */
+    drone_state['acq_thread_on'] = _[1] >> 27 & 1 # Acquisition thread ON : (0) thread OFF (1) thread ON */
+    drone_state['ctrl_watchdog_mask'] = _[1] >> 28 & 1 # CTRL watchdog : (1) delay in control execution (> 5ms), (0) control is well scheduled */
+    drone_state['adc_watchdog_mask'] = _[1] >> 29 & 1 # ADC Watchdog : (1) delay in uart2 dsr (> 5ms), (0) uart2 is good */
+    drone_state['com_watchdog_mask'] = _[1] >> 30 & 1 # Communication Watchdog : (1) com problem, (0) Com is ok */
+    drone_state['emergency_mask'] = _[1] >> 31 & 1 # Emergency landing : (0) no emergency, (1) emergency */
+    data = dict()
+    data['drone_state'] = drone_state
+    data['header'] = _[0]
+    data['seq_nr'] = _[2]
+    data['vision_flag'] = _[3]
+    offset += struct.calcsize("IIII")
+    has_flying_information = False
+    while 1:
+        try:
+            id_nr, size = struct.unpack_from("HH", packet, offset)
+            offset += struct.calcsize("HH")
+        except struct.error:
+            break
+        values = []
+        for i in range(size - struct.calcsize("HH")):
+            values.append(struct.unpack_from("c", packet, offset)[0])
+            offset += struct.calcsize("c")
+        # navdata_tag_t in navdata-common.h
+        if id_nr == 0:
+            has_flying_information = True
+            values = struct.unpack_from("IIfffifffI", "".join(values))
+            values = dict(zip(['ctrl_state', 'battery', 'theta', 'phi', 'psi', 'altitude', 'vx', 'vy', 'vz', 'num_frames'], values))
+            # convert the millidegrees into degrees and round to int, as they
+            try:
+                values['ctrl_state'] = ctrl_state_dict[values['ctrl_state']]
+            except KeyError:
+                values['ctrl_state'] = -1
+            # are not so precise anyways
+            for i in 'theta', 'phi', 'psi':
+                values[i] = int(values[i] / 1000)
+        data[id_nr] = values
+    return data, has_flying_information
+
+
+if __name__ == "__main__":
+    '''
+    For testing purpose only
+    '''
+    import termios
+    import fcntl
+    import os
+
+    fd = sys.stdin.fileno()
+
+    oldterm = termios.tcgetattr(fd)
+    newattr = termios.tcgetattr(fd)
+    newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO
+    termios.tcsetattr(fd, termios.TCSANOW, newattr)
+
+    oldflags = fcntl.fcntl(fd, fcntl.F_GETFL)
+    fcntl.fcntl(fd, fcntl.F_SETFL, oldflags | os.O_NONBLOCK)
+
+    drone = ARDrone(is_ar_drone_2=True)
+
+    import cv2
+    try:
+        startvideo = True
+        video_waiting = False
+        while 1:
+            time.sleep(.0001)
+            if startvideo:
+                try:
+                    cv2.imshow("Drone camera", cv2.cvtColor(drone.get_image(), cv2.COLOR_BGR2RGB))
+                    cv2.waitKey(1)
+                except:
+                    if not video_waiting:
+                        print("Video will display when ready")
+                    video_waiting = True
+                    pass
+
+            try:
+                c = sys.stdin.read(1)
+                c = c.lower()
+                print("Got character", c)
+                if c == 'a':
+                    drone.move_left()
+                if c == 'd':
+                    drone.move_right()
+                if c == 'w':
+                    drone.move_forward()
+                if c == 's':
+                    drone.move_backward()
+                if c == ' ':
+                    drone.land()
+                if c == '\n':
+                    drone.takeoff()
+                if c == 'q':
+                    drone.turn_left()
+                if c == 'e':
+                    drone.turn_right()
+                if c == '1':
+                    drone.move_up()
+                if c == '2':
+                    drone.hover()
+                if c == '3':
+                    drone.move_down()
+                if c == 't':
+                    drone.reset()
+                if c == 'x':
+                    drone.hover()
+                if c == 'y':
+                    drone.trim()
+                if c == 'i':
+                    startvideo = True
+                    try:
+                        navdata = drone.get_navdata()
+
+                        print('Emergency landing =', navdata['drone_state']['emergency_mask'])
+                        print('User emergency landing = ', navdata['drone_state']['user_el'])
+                        print('Navdata type= ', navdata['drone_state']['navdata_demo_mask'])
+                        print('Altitude= ', navdata[0]['altitude'])
+                        print('video enable= ', navdata['drone_state']['video_mask'])
+                        print('vision enable= ', navdata['drone_state']['vision_mask'])
+                        print('command_mask= ', navdata['drone_state']['command_mask'])
+                    except:
+                        pass
+
+                if c == 'j':
+                    print("Asking for configuration...")
+                    drone.at(at_ctrl, 5)
+                    time.sleep(0.5)
+                    drone.at(at_ctrl, 4)
+            except IOError:
+                pass
+    finally:
+        termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm)
+        fcntl.fcntl(fd, fcntl.F_SETFL, oldflags)
+        drone.halt()
+
diff --git a/libardrone/paveparser.output b/libardrone/paveparser.output
new file mode 100644
index 0000000..1943e6d
--- /dev/null
+++ b/libardrone/paveparser.output
diff --git a/libardrone/paveparser.py b/libardrone/paveparser.py
new file mode 100644
index 0000000..5e6fc08
--- /dev/null
+++ b/libardrone/paveparser.py
@@ -0,0 +1,159 @@
+# -*- coding: utf-8 -*-
+# Copyright (c) 2013 Adrian Taylor
+# Inspired by equivalent node.js code by Felix Geisendörfer
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import struct
+"""
+The AR Drone 2.0 allows a tcp client to receive H264 (MPEG4.10 AVC) video
+from the drone. However, the frames are wrapped by Parrot Video
+Encapsulation (PaVE), which this class parses.
+"""
+
+"""
+Usage: Pass in an output file object into the constructor, then call write on this.
+"""
+class PaVEParser(object):
+
+    HEADER_SIZE_SHORT = 64; # sometimes header is longer
+
+    def __init__(self, outfileobject):
+        self.buffer = ""
+        self.state = self.handle_header
+        self.outfileobject = outfileobject
+        self.misaligned_frames = 0
+        self.payloads = 0
+        self.drop_old_frames = True
+        self.align_on_iframe = True
+
+        if self.drop_old_frames:
+            self.state = self.handle_header_drop_frames
+
+    def write(self, data):
+        self.buffer += data
+        while True:
+            made_progress = self.state()
+            if not made_progress:
+                return
+
+    def handle_header(self):
+        if self.fewer_remaining_than(self.HEADER_SIZE_SHORT):
+            return False
+
+        (signature, version, video_codec, header_size, self.payload_size, encoded_stream_width,
+        encoded_stream_height, display_width, display_height, frame_number, timestamp, total_chunks,
+        chunk_index, frame_type, control, stream_byte_position_lw, stream_byte_position_uw,
+        stream_id, total_slices, slice_index, header1_size, header2_size,
+        reserved2, advertised_size, reserved3) = struct.unpack("<4sBBHIHHHHIIBBBBIIHBBBB2sI12s",
+                                                               self.buffer[0:self.HEADER_SIZE_SHORT])
+
+        if signature != "PaVE":
+            self.state = self.handle_misalignment
+            return True
+        self.buffer = self.buffer[header_size:]
+        self.state = self.handle_payload
+        return True
+
+    def handle_header_drop_frames(self):
+
+        eligible_index = self.buffer.find('PaVE')
+
+        if (eligible_index < 0):
+            return False
+        self.buffer = self.buffer[eligible_index:]
+
+        if self.fewer_remaining_than(self.HEADER_SIZE_SHORT):
+            return False
+
+        eligible_index = 0
+        current_index = eligible_index
+
+        while current_index != -1 and len(self.buffer[current_index:]) > self.HEADER_SIZE_SHORT:
+            (signature, version, video_codec, header_size, payload_size, encoded_stream_width,
+                encoded_stream_height, display_width, display_height, frame_number, timestamp, total_chunks,
+                chunk_index, frame_type, control, stream_byte_position_lw, stream_byte_position_uw,
+                stream_id, total_slices, slice_index, header1_size,
+                header2_size, reserved2, advertised_size,
+                reserved3) = struct.unpack("<4sBBHIHHHHIIBBBBIIHBBBB2sI12s",
+                                self.buffer[current_index:current_index + self.HEADER_SIZE_SHORT])
+
+            if (frame_type != 3 or current_index == 0):
+                eligible_index = current_index
+                self.payload_size = payload_size
+
+            offset = self.buffer[current_index + 1:].find('PaVE') + 1
+            if (offset == 0):
+                break
+
+            current_index += offset
+
+        self.buffer = self.buffer[eligible_index + header_size:]
+        self.state = self.handle_payload
+        return True
+
+
+    def handle_misalignment(self):
+        """Sometimes we start of in the middle of frame - look for the PaVE header."""
+        IFrame = False
+        if self.align_on_iframe:
+            while (not IFrame):
+                index = self.buffer.find('PaVE')
+                if index == -1:
+                    return False
+
+                self.buffer = self.buffer[index:]
+
+                if self.fewer_remaining_than(self.HEADER_SIZE_SHORT):
+                    return False
+
+                (signature, version, video_codec, header_size, self.payload_size, encoded_stream_width,
+                encoded_stream_height, display_width, display_height, frame_number, timestamp, total_chunks,
+                chunk_index, frame_type, control, stream_byte_position_lw, stream_byte_position_uw,
+                stream_id, total_slices, slice_index, header1_size, header2_size, reserved2, advertised_size,
+                reserved3) = struct.unpack("<4sBBHIHHHHIIBBBBIIHBBBB2sI12s", self.buffer[0:self.HEADER_SIZE_SHORT])
+
+                IFrame = (frame_type == 1 or frame_type == 2)
+                if not IFrame:
+                    self.buffer = self.buffer[header_size:]
+        else:
+            index = self.buffer.find('PaVE')
+            if index == -1:
+                return False
+            self.buffer = self.buffer[index:]
+
+        self.misaligned_frames += 1
+        self.state = self.handle_header
+
+        return True
+
+    def handle_payload(self):
+        if self.fewer_remaining_than(self.payload_size):
+            return False
+        self.state = self.handle_header
+        if self.drop_old_frames:
+            self.state = self.handle_header_drop_frames
+
+        self.outfileobject.write(self.buffer[0:self.payload_size])
+        self.buffer = self.buffer[self.payload_size:]
+        self.payloads += 1
+        return True
+
+    def fewer_remaining_than(self, desired_size):
+        return len(self.buffer) < desired_size
diff --git a/libardrone/test_h264_decoder.py b/libardrone/test_h264_decoder.py
new file mode 100644
index 0000000..1107fd7
--- /dev/null
+++ b/libardrone/test_h264_decoder.py
@@ -0,0 +1,17 @@
+import paveparser
+import mock
+import h264decoder
+import os
+
+
+def test_h264_decoder():
+    outfileobj = mock.Mock()
+    decoder = h264decoder.H264Decoder(outfileobj)
+    example_video_stream = open(os.path.join(os.path.dirname(__file__), 'paveparser.output'))
+    while True:
+        data = example_video_stream.read(1000)
+        if len(data) == 0:
+            break
+        decoder.write(data)
+
+    assert outfileobj.image_ready.called
diff --git a/libardrone/test_libardrone.py b/libardrone/test_libardrone.py
new file mode 100644
index 0000000..9b5bd5a
--- /dev/null
+++ b/libardrone/test_libardrone.py
@@ -0,0 +1,32 @@
+# Copyright (c) 2011 Bastian Venthur
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+
+import unittest
+
+import libardrone
+
+class LibardroneTestCase(unittest.TestCase):
+    def test_f2i(self):
+        self.assertEqual(libardrone.f2i(-0.8,), -1085485875)
+
+if __name__ == "__main__":
+    unittest.main()
+
diff --git a/libardrone/test_losing_connection.py b/libardrone/test_losing_connection.py
new file mode 100644
index 0000000..0765d76
--- /dev/null
+++ b/libardrone/test_losing_connection.py
@@ -0,0 +1,67 @@
+import select
+import socket
+import struct
+import time
+
+DRONE_IP = "192.168.1.1"
+ARDRONE_NAVDATA_PORT = 5554
+ARDRONE_COMMAND_PORT = 5556
+
+'''
+Small endless loop to test the robustness of the tcp ip connection (video streaming)
+Warning: This test does not stop, it raises an exception when the connection is lost or
+if something goes wrong (most likely the drone stops sending video data and
+send empty packets on the command port...
+'''
+
+def at(command, seq, params):
+    """
+    Parameters:
+    command -- the command
+    seq -- the sequence number
+    params -- a list of elements which can be either int, float or string
+    """
+    param_str = ''
+    for p in params:
+        if type(p) == int:
+            param_str += ",%d" % p
+        elif type(p) == float:
+            param_str += ",%d" % f2i(p)
+        elif type(p) == str:
+            param_str += ',"' + p + '"'
+    msg = "AT*%s=%i%s\r" % (command, seq, param_str)
+    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock.sendto(msg, ("192.168.1.1", ARDRONE_COMMAND_PORT))
+
+def f2i(f):
+    """Interpret IEEE-754 floating-point value as signed integer.
+    Arguments:
+    f -- floating point value
+    """
+    return struct.unpack('i', struct.pack('f', f))[0]
+
+
+if __name__ == '__main__':
+    nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
+    nav_socket.connect((DRONE_IP, ARDRONE_NAVDATA_PORT))
+    nav_socket.setblocking(0)
+    nav_socket.send("\x01\x00\x00\x00")
+
+    seq = 1
+    stopping = 1
+    while stopping < 100:
+        inputready, outputready, exceptready = select.select([nav_socket], [], [], 1)
+        seq += 1
+        at("COMWDG", seq, [])
+        if len(inputready) == 0:
+            print "Connection lost for the %d time !" % stopping
+            nav_socket.send("\x01\x00\x00\x00")
+            stopping += 1
+        for i in inputready:
+            while 1:
+                try:
+                    data = nav_socket.recv(500)
+                except IOError:
+                    break
+
+    raise Exception("Should not get here")
diff --git a/libardrone/test_paveparser.py b/libardrone/test_paveparser.py
new file mode 100644
index 0000000..49e65f9
--- /dev/null
+++ b/libardrone/test_paveparser.py
@@ -0,0 +1,20 @@
+import paveparser
+import mock
+import os
+
+
+def test_misalignment():
+    outfile = mock.Mock()
+    p = paveparser.PaVEParser(outfile)
+    example_video_stream = open(os.path.join(os.path.dirname(__file__), 'ardrone2_video_example.capture'))
+    while True:
+        data = example_video_stream.read(1000000)
+        if len(data) == 0:
+            break
+        p.write(data)
+
+    assert outfile.write.called
+    assert p.misaligned_frames < 3
+
+if __name__ == "__main__":
+    test_misalignment()
diff --git a/server2.py b/server2.py
new file mode 100644
index 0000000..2066231
--- /dev/null
+++ b/server2.py
@@ -0,0 +1,66 @@
+import cv2
+import os
+import socket
+import sys
+import time
+import struct
+
+
+
+
+
+
+server_address = '/home/flo/uds_socket'
+try:
+    os.unlink(server_address)
+except OSError:
+    if os.path.exists(server_address):
+        raise
+
+# Create a UDS socket
+sock = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
+
+# Bind the socket to the port
+print >>sys.stderr, 'starting up on %s' % server_address
+sock.bind(server_address)
+
+# Listen for incoming connections
+sock.listen(1)
+
+
+
+while True:
+    # Wait for a connection
+    print >>sys.stderr, 'waiting for a connection'
+    connection, client_address = sock.accept()
+    try:
+        print >>sys.stderr, 'connection from', client_address
+        
+        cap = cv2.VideoCapture("flight.avi")
+        logfile = open("flight.log", "r")
+        while True:
+            data = connection.recv(16)
+            if data:
+                if data=="get\n":
+                    status, frame = cap.read()
+                    values = logfile.readline().split()
+                    phi = float(values[0])
+                    theta = float(values[1])
+                    psi = float(values[2])
+                    batt = 100.0
+
+                    framestr = frame.tostring()
+                    lenframestr=len(framestr)
+                    connection.sendall(struct.pack(">i",lenframestr)+framestr+struct.pack("@dddd", phi, theta, psi, batt));
+                elif data[0:3] == "fly" and data[-1]=="\n":
+                    values = data[3:-1].split()
+                    print "fly ",values
+            else:
+                print >>sys.stderr, 'no more data from', client_address
+                break
+    except:
+        print "Dingens!!11!1!!!"
+    finally:
+        # Clean up the connection
+        connection.close()
+