pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/crtpstack.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/crtpstack.py

@@ -47,6 +47,7 @@ class CRTPPort:
     LOGGING = 0x05
     LOCALIZATION = 0x06
     COMMANDER_GENERIC = 0x07
+    SETPOINT_HL = 0x08
     PLATFORM = 0x0D
     DEBUGDRIVER = 0x0E
     LINKCTRL = 0x0F

pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/debugdriver.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/debugdriver.py

@@ -316,18 +316,18 @@ class DebugDriver(CRTPDriver):
         self._packet_handler._random_answer_delay = False
         self._packet_handler._random_toc_crcs = False
 
-        if (re.search('^debug://.*/1\Z', uri)):
+        if (re.search('^debug://.*/1$', uri)):
             self._packet_handler.inhibitAnswers = True
-        if (re.search('^debug://.*/110\Z', uri)):
+        if (re.search('^debug://.*/110$', uri)):
             self._packet_handler.bootloader = True
-        if (re.search('^debug://.*/2\Z', uri)):
+        if (re.search('^debug://.*/2$', uri)):
             self._packet_handler.doIncompleteLogTOC = True
-        if (re.search('^debug://.*/3\Z', uri)):
+        if (re.search('^debug://.*/3$', uri)):
             self._packet_handler._random_answer_delay = True
-        if (re.search('^debug://.*/4\Z', uri)):
+        if (re.search('^debug://.*/4$', uri)):
             self._packet_handler._random_answer_delay = True
             self._packet_handler._random_toc_crcs = True
-        if (re.search('^debug://.*/5\Z', uri)):
+        if (re.search('^debug://.*/5$', uri)):
             self._packet_handler._random_toc_crcs = True
 
         if len(self._fake_mems) == 0:
@@ -359,7 +359,7 @@ class DebugDriver(CRTPDriver):
                 FakeMemory(type=1, size=112, addr=0x1234567890ABCDEE,
                            data=[0x00 for a in range(112)]))
 
-        if (re.search('^debug://.*/6\Z', uri)):
+        if (re.search('^debug://.*/6$', uri)):
             logger.info('------------->Erasing memories on connect')
             for m in self._fake_mems:
                 m.erase()
@@ -572,19 +572,19 @@ class _PacketHandlingThread(Thread):
                                  ' it 0 !', pk.port)
 
                 if (pk.port == CRTPPort.LOGGING):
-                    l = self.fakeLogToc[varIndex]
+                    entry = self.fakeLogToc[varIndex]
                 if (pk.port == CRTPPort.PARAM):
-                    l = self.fakeParamToc[varIndex]
+                    entry = self.fakeParamToc[varIndex]
 
-                vartype = l['vartype']
-                if (pk.port == CRTPPort.PARAM and l['writable'] is True):
+                vartype = entry['vartype']
+                if (pk.port == CRTPPort.PARAM and entry['writable'] is True):
                     vartype = vartype | (0x10)
 
-                p.data = struct.pack('<BBB', cmd, l['varid'], vartype)
-                for ch in l['vargroup']:
+                p.data = struct.pack('<BBB', cmd, entry['varid'], vartype)
+                for ch in entry['vargroup']:
                     p.data.append(ord(ch))
                 p.data.append(0)
-                for ch in l['varname']:
+                for ch in entry['varname']:
                     p.data.append(ord(ch))
                 p.data.append(0)
                 if (self.doIncompleteLogTOC is False):

dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/prrtdriver.py

+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+import datetime
+import logging
+import re
+
+from .crtpstack import CRTPPacket
+from .exceptions import WrongUriType
+from cflib.crtp.crtpdriver import CRTPDriver
+
+prrt_installed = True
+try:
+    import prrt
+except ImportError:
+    prrt_installed = False
+
+__author__ = 'Bitcraze AB'
+__all__ = ['PrrtDriver']
+
+logger = logging.getLogger(__name__)
+
+MAX_PAYLOAD = 32
+DEFAULT_TARGET_DELAY = 0.05  # unit: s
+PRRT_LOCAL_PORT = 5000
+
+
+class PrrtDriver(CRTPDriver):
+
+    def __init__(self):
+        CRTPDriver.__init__(self)
+        self.prrt_socket = None
+        self.uri = ''
+        self.link_error_callback = None
+        self.packet_log = None
+        self.log_index = 0
+        logger.info('Initialized PRRT driver.')
+
+    def connect(self, uri, linkQualityCallback, linkErrorCallback):
+        # check if the URI is a PRRT URI
+        if not re.search('^prrt://', uri):
+            raise WrongUriType('Not a prrt URI')
+
+        # Check if it is a valid PRRT URI
+        uri_match = re.search(r'^prrt://((?:[\d]{1,3})\.(?:[\d]{1,3})\.'
+                              r'(?:[\d]{1,3})\.(?:[\d]{1,3})):([\d]{1,5})'
+                              r'(?:/([\d]{1,6}))?$', uri)
+        if not uri_match:
+            raise Exception('Invalid PRRT URI')
+
+        if not prrt_installed:
+            raise Exception('PRRT is missing')
+
+        self.uri = uri
+
+        self.link_error_callback = linkErrorCallback
+
+        address = uri_match.group(1)
+        port = int(uri_match.group(2))
+        target_delay_s = DEFAULT_TARGET_DELAY
+        if uri_match.group(3):
+            target_delay_s = int(uri_match.group(3)) / 1000
+
+        self.prrt_socket = prrt.PrrtSocket(('0.0.0.0', PRRT_LOCAL_PORT),
+                                           maximum_payload_size=MAX_PAYLOAD,
+                                           target_delay=target_delay_s)
+        self.prrt_socket.connect((address, port))
+
+    def send_packet(self, pk):
+        pk_bytes = bytearray([pk.get_header()]) + pk.data
+        self.prrt_socket.send(pk_bytes)
+
+    def receive_packet(self, wait=0):
+        try:
+            if wait == 0:
+                pk_bytes, _ = self.prrt_socket.receive_asap()
+            elif wait < 0:
+                pk_bytes, _ = self.prrt_socket.receive_asap_wait()
+            else:
+                deadline = datetime.datetime.utcnow() + datetime.timedelta(
+                    seconds=wait)
+                pk_bytes, _ = self.prrt_socket.receive_asap_timedwait(deadline)
+        except prrt.TimeoutException:
+            return None
+
+        if len(pk_bytes) <= 0:
+            return None
+
+        pk = CRTPPacket(pk_bytes[0], pk_bytes[1:])
+        return pk
+
+    def get_status(self):
+        return 'No information available'
+
+    def get_name(self):
+        return 'prrt'
+
+    def scan_interface(self, address):
+        default_uri = 'prrt://10.8.0.208:5000'
+        if prrt_installed:
+            return [[default_uri, ''], ]
+        return []
+
+    def close(self):
+        return

pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/radiodriver.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/radiodriver.py

@@ -39,6 +39,7 @@ import struct
 import sys
 import threading
 
+import cflib.drivers.crazyradio as crazyradio
 from .crtpstack import CRTPPacket
 from .exceptions import WrongUriType
 from cflib.crtp.crtpdriver import CRTPDriver
@@ -55,6 +56,9 @@ __all__ = ['RadioDriver']
 
 logger = logging.getLogger(__name__)
 
+_nr_of_retries = 100
+_nr_of_arc_retries = 3
+
 DEFAULT_ADDR_A = [0xe7, 0xe7, 0xe7, 0xe7, 0xe7]
 DEFAULT_ADDR = 0xE7E7E7E7E7
 
@@ -106,7 +110,7 @@ class _RadioManager():
             if _RadioManager._radios[self._devid].usage_counter == 0:
                 try:
                     _RadioManager._radios[self._devid].radio.close()
-                except:
+                except Exception:
                     pass
                 _RadioManager._radios[self._devid] = None
 
@@ -149,40 +153,11 @@ class RadioDriver(CRTPDriver):
         an error message.
         """
 
-        # check if the URI is a radio URI
-        if not re.search('^radio://', uri):
-            raise WrongUriType('Not a radio URI')
-
-        # Open the USB dongle
-        if not re.search('^radio://([0-9]+)((/([0-9]+))'
-                         '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$', uri):
-            raise WrongUriType('Wrong radio URI format!')
-
-        uri_data = re.search('^radio://([0-9]+)((/([0-9]+))'
-                             '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$', uri)
-
+        devid, channel, datarate, address = self.parse_uri(uri)
         self.uri = uri
 
-        channel = 2
-        if uri_data.group(4):
-            channel = int(uri_data.group(4))
-
-        datarate = Crazyradio.DR_2MPS
-        if uri_data.group(7) == '250K':
-            datarate = Crazyradio.DR_250KPS
-        if uri_data.group(7) == '1M':
-            datarate = Crazyradio.DR_1MPS
-        if uri_data.group(7) == '2M':
-            datarate = Crazyradio.DR_2MPS
-
-        address = DEFAULT_ADDR_A
-        if uri_data.group(9):
-            addr = str(uri_data.group(9))
-            new_addr = struct.unpack('<BBBBB', binascii.unhexlify(addr))
-            address = new_addr
-
         if self._radio_manager is None:
-            self._radio_manager = _RadioManager(int(uri_data.group(1)),
+            self._radio_manager = _RadioManager(devid,
                                                 channel,
                                                 datarate,
                                                 address)
@@ -191,7 +166,7 @@ class RadioDriver(CRTPDriver):
 
         with self._radio_manager as cradio:
             if cradio.version >= 0.4:
-                cradio.set_arc(10)
+                cradio.set_arc(_nr_of_arc_retries)
             else:
                 logger.warning('Radio version <0.4 will be obsoleted soon!')
 
@@ -211,6 +186,50 @@ class RadioDriver(CRTPDriver):
 
         self.link_error_callback = link_error_callback
 
+    @staticmethod
+    def parse_uri(uri):
+        # check if the URI is a radio URI
+        if not re.search('^radio://', uri):
+            raise WrongUriType('Not a radio URI')
+
+        # Open the USB dongle
+        if not re.search('^radio://([0-9a-fA-F]+)((/([0-9]+))'
+                         '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$', uri):
+            raise WrongUriType('Wrong radio URI format!')
+
+        uri_data = re.search('^radio://([0-9a-fA-F]+)((/([0-9]+))'
+                             '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$', uri)
+
+        if len(uri_data.group(1)) < 10 and uri_data.group(1).isdigit():
+            devid = int(uri_data.group(1))
+        else:
+            try:
+                devid = crazyradio.get_serials().index(
+                    uri_data.group(1).upper())
+            except ValueError:
+                raise Exception('Cannot find radio with serial {}'.format(
+                    uri_data.group(1)))
+
+        channel = 2
+        if uri_data.group(4):
+            channel = int(uri_data.group(4))
+
+        datarate = Crazyradio.DR_2MPS
+        if uri_data.group(7) == '250K':
+            datarate = Crazyradio.DR_250KPS
+        if uri_data.group(7) == '1M':
+            datarate = Crazyradio.DR_1MPS
+        if uri_data.group(7) == '2M':
+            datarate = Crazyradio.DR_2MPS
+
+        address = DEFAULT_ADDR_A
+        if uri_data.group(9):
+            addr = str(uri_data.group(9))
+            new_addr = struct.unpack('<BBBBB', binascii.unhexlify(addr))
+            address = new_addr
+
+        return devid, channel, datarate, address
+
     def receive_packet(self, time=0):
         """
         Receive a packet though the link. This call is blocking but will
@@ -389,8 +408,6 @@ class _RadioDriverThread(threading.Thread):
     Radio link receiver thread used to read data from the
     Crazyradio USB driver. """
 
-    TRIES_BEFORE_DISCON = 10
-
     def __init__(self, radio_manager, inQueue, outQueue,
                  link_quality_callback, link_error_callback, link):
         """ Create the object """
@@ -401,7 +418,7 @@ class _RadioDriverThread(threading.Thread):
         self._sp = False
         self._link_error_callback = link_error_callback
         self._link_quality_callback = link_quality_callback
-        self._retry_before_disconnect = _RadioDriverThread.TRIES_BEFORE_DISCON
+        self._retry_before_disconnect = _nr_of_retries
         self._retries = collections.deque()
         self._retry_sum = 0
 
@@ -497,8 +514,7 @@ class _RadioDriverThread(threading.Thread):
                         self._link_error_callback is not None):
                     self._link_error_callback('Too many packets lost')
                 continue
-            self._retry_before_disconnect = \
-                _RadioDriverThread.TRIES_BEFORE_DISCON
+            self._retry_before_disconnect = _nr_of_retries
 
             data = ackStatus.data
 
@@ -536,3 +552,13 @@ class _RadioDriverThread(threading.Thread):
                         dataOut.append(ord(X))
             else:
                 dataOut.append(0xFF)
+
+
+def set_retries_before_disconnect(nr_of_retries):
+    global _nr_of_retries
+    _nr_of_retries = nr_of_retries
+
+
+def set_retries(nr_of_arc_retries):
+    global _nr_of_arc_retries
+    _nr_of_arc_retries = nr_of_arc_retries

dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/serialdriver.py

+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2011-2013 Bitcraze AB
+#
+#  Crazyflie Nano Quadcopter Client
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+#  You should have received a copy of the GNU General Public License
+#  along with this program; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+#  MA  02110-1301, USA.
+"""
+An early serial link driver. This could still be used (after some fixing) to
+run high-speed CRTP with the Crazyflie. The UART can be run at 2Mbit.
+"""
+import logging
+import re
+import sys
+import threading
+
+from .crtpstack import CRTPPacket
+from .exceptions import WrongUriType
+from cflib.crtp.crtpdriver import CRTPDriver
+
+if sys.version_info < (3,):
+    import Queue as queue
+else:
+    import queue
+
+found_serial = True
+try:
+    import serial
+    import serial.tools.list_ports as list_ports
+except ImportError:
+    found_serial = False
+
+__author__ = 'Bitcraze AB'
+__all__ = ['SerialDriver']
+
+logger = logging.getLogger(__name__)
+
+MTU = 32
+START_BYTE1 = 0xbc
+START_BYTE2 = 0xcf
+SYSLINK_RADIO_RAW = 0x00
+
+
+def compute_cksum(list):
+    cksum0, cksum1 = 0, 0
+    for i in range(len(list)):
+        cksum0 = (cksum0 + list[i]) & 0xff
+        cksum1 = (cksum1 + cksum0) & 0xff
+    return bytearray([cksum0, cksum1])
+
+
+class SerialDriver(CRTPDriver):
+
+    def __init__(self):
+        CRTPDriver.__init__(self)
+        self.ser = None
+        self.uri = ''
+        self.link_error_callback = None
+        self.in_queue = None
+        self.out_queue = None
+        self._receive_thread = None
+        self._send_thread = None
+        logger.info('Initialized serial driver.')
+
+    def connect(self, uri, linkQualityCallback, linkErrorCallback):
+        # check if the URI is a serial URI
+        if not re.search('^serial://', uri):
+            raise WrongUriType('Not a serial URI')
+
+        # Check if it is a valid serial URI
+        uri_data = re.search('^serial://([a-zA-Z0-9]+)$', uri)
+        if not uri_data:
+            raise Exception('Invalid serial URI')
+
+        devices = [x.device for x in list_ports.comports()
+                   if x.name == uri_data.group(1)]
+        if not len(devices) == 1:
+            raise Exception('Could not identify device')
+        device = devices[0]
+
+        if not found_serial:
+            raise Exception('PySerial package is missing')
+
+        self.uri = uri
+
+        self.link_error_callback = linkErrorCallback
+
+        # Prepare the inter-thread communication queue
+        self.in_queue = queue.Queue()
+        self.out_queue = queue.Queue(1)
+
+        self.ser = serial.Serial(device, 512000, timeout=1)
+
+        # Launch the comm thread
+        self._receive_thread = _SerialReceiveThread(
+            self.ser, self.in_queue, linkQualityCallback, linkErrorCallback)
+        self._receive_thread.start()
+        self._send_thread = _SerialSendThread(
+            self.ser, self.out_queue, linkQualityCallback, linkErrorCallback)
+        self._send_thread.start()
+
+    def send_packet(self, pk):
+        try:
+            self.out_queue.put(pk, True, 2)
+        except queue.Full:
+            if self.link_error_callback:
+                self.link_error_callback(
+                    'RadioDriver: Could not send packet to copter')
+
+    def receive_packet(self, wait=0):
+        try:
+            if wait == 0:
+                pk = self.in_queue.get(False)
+            elif wait < 0:
+                pk = self.in_queue.get(True)
+            else:
+                pk = self.in_queue.get(True, wait)
+        except queue.Empty:
+            return None
+        return pk
+
+    def get_status(self):
+        return 'No information available'
+
+    def get_name(self):
+        return 'serial'
+
+    def scan_interface(self, address):
+        if found_serial:
+            devices_names = [x.name for x in list_ports.comports()]
+            return [('serial://' + x, '') for x in devices_names]
+        else:
+            return []
+
+    def close(self):
+        self._receive_thread.stop()
+        self._send_thread.stop()
+        try:
+            self._receive_thread.join()
+            self._send_thread.join()
+        except Exception:
+            pass
+        self.ser.close()
+
+
+class _SerialReceiveThread(threading.Thread):
+
+    def __init__(self, ser, inQueue, link_quality_callback,
+                 link_error_callback):
+        """ Create the object """
+        threading.Thread.__init__(self)
+        self.ser = ser
+        self.in_queue = inQueue
+        self._stop = False
+        self.link_error_callback = link_error_callback
+
+    def stop(self):
+        """ Stop the thread """
+        self._stop = True
+
+    def run(self):
+        """ Run the receiver thread """
+        READ_END = bytes([START_BYTE1, START_BYTE2])
+        received = bytearray(MTU + 4)
+        received_header = memoryview(received)[0:2]
+        while not self._stop:
+            try:
+                r = self.ser.read_until(READ_END)[-2:]
+                if len(r) != 2:
+                    continue
+
+                if r[0] != START_BYTE1 or r[1] != START_BYTE2:
+                    continue
+
+                r = self.ser.readinto(received_header)
+                if r != 2:
+                    continue
+
+                if not (0 < received_header[1] <= MTU):
+                    continue
+
+                expected = received_header[1] + 2
+
+                received_data_chk = memoryview(received)[2:2+expected]
+                r = self.ser.readinto(received_data_chk)
+                if r != expected:
+                    continue
+
+                # NOTE: end is (expected - 2) as the lenght of the data +2 for
+                # the header bytes
+                cksum = compute_cksum(memoryview(received)[:expected])
+                if cksum[0] != received_data_chk[-2] or \
+                        cksum[1] != received_data_chk[-1]:
+                    continue
+
+                pk = CRTPPacket(received[2], received[3:expected])
+                self.in_queue.put(pk)
+
+            except Exception as e:
+                import traceback
+                if self.link_error_callback:
+                    self.link_error_callback(
+                        'Error communicating with the Crazyflie!\n'
+                        'Exception:%s\n\n%s' % (e, traceback.format_exc()))
+
+
+class _SerialSendThread(threading.Thread):
+
+    def __init__(self, ser, outQueue, link_quality_callback,
+                 link_error_callback):
+        """ Create the object """
+        threading.Thread.__init__(self)
+        self.ser = ser
+        self.out_queue = outQueue
+        self._stop = False
+        self.link_error_callback = link_error_callback
+
+    def stop(self):
+        """ Stop the thread """
+        self._stop = True
+
+    def run(self):
+        """ Run the sender thread """
+        out_data = bytearray(MTU + 6)
+        out_data[0:3] = bytearray(
+            [START_BYTE1, START_BYTE2, SYSLINK_RADIO_RAW])
+
+        empty_packet = CRTPPacket(header=0xFF)
+        empty_packet_data_length = 0
+        empty_packet_data = bytearray(7)
+        empty_packet_data[0:5] = bytearray(
+            [START_BYTE1, START_BYTE2, SYSLINK_RADIO_RAW, 0x01,
+             empty_packet.header])
+        empty_packet_data[5:7] = compute_cksum(empty_packet_data[2:5])
+
+        while not self._stop:
+            try:
+                pk = self.out_queue.get(True, timeout=0.0003)
+                data = pk.data
+                len_data = len(data)
+                end_of_payload = 5 + len_data
+
+                out_data[3] = len_data + 1
+                out_data[4] = pk.header
+                out_data[5:end_of_payload] = data
+                out_data[end_of_payload:end_of_payload +
+                         2] = compute_cksum(out_data[2:end_of_payload])
+
+                written = self.ser.write(out_data[0:end_of_payload + 2])
+
+            except queue.Empty:
+                pk = empty_packet
+                len_data = empty_packet_data_length
+                written = self.ser.write(empty_packet_data)
+
+            if written != len_data + 7:
+                if self.link_error_callback:
+                    self.link_error_callback(
+                        'SerialDriver: Could only send {:d}B bytes of {:d}B '
+                        'packet to Crazyflie'.format(
+                            written, len_data + 7)
+                    )

pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/cfusb.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/drivers/cfusb.py

@@ -50,7 +50,7 @@ try:
         pyusb_backend = libusb0.get_backend()
     pyusb1 = True
 
-except:
+except Exception:
     pyusb1 = False
 
 
@@ -65,7 +65,8 @@ def _find_devices():
     if pyusb1:
         for d in usb.core.find(idVendor=USB_VID, idProduct=USB_PID, find_all=1,
                                backend=pyusb_backend):
-            ret.append(d)
+            if d.manufacturer == 'Bitcraze AB':
+                ret.append(d)
     else:
         busses = usb.busses()
         for bus in busses:
@@ -120,10 +121,9 @@ class CfUsb:
         if (pyusb1 is False):
             if self.handle:
                 self.handle.releaseInterface()
-                self.handle.reset()
         else:
             if self.dev:
-                self.dev.reset()
+                usb.util.dispose_resources(self.dev)
 
         self.handle = None
         self.dev = None
@@ -167,7 +167,7 @@ class CfUsb:
                     pass
                 else:
                     raise IOError('Crazyflie disconnected')
-            except AttributeError as e:
+            except AttributeError:
                 # pyusb < 1.0 doesn't implement getting the underlying error
                 # number and it seems as if it's not possible to detect
                 # if the cable is disconnected. So this detection is not

pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/crazyradio.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/drivers/crazyradio.py

@@ -63,11 +63,11 @@ try:
 
         pyusb_backend = libusb0.get_backend()
     pyusb1 = True
-except:
+except Exception:
     pyusb1 = False
 
 
-def _find_devices():
+def _find_devices(serial=None):
     """
     Returns a list of CrazyRadio devices currently connected to the computer
     """
@@ -76,6 +76,8 @@ def _find_devices():
     if pyusb1:
         for d in usb.core.find(idVendor=0x1915, idProduct=0x7777, find_all=1,
                                backend=pyusb_backend):
+            if serial is not None and serial == d.serial_number:
+                return d
             ret.append(d)
     else:
         busses = usb.busses()
@@ -83,11 +85,17 @@ def _find_devices():
             for device in bus.devices:
                 if device.idVendor == CRADIO_VID:
                     if device.idProduct == CRADIO_PID:
+                        if serial == device.serial_number:
+                            return device
                         ret += [device, ]
 
     return ret
 
 
+def get_serials():
+    return tuple(map(lambda d: d.serial_number, _find_devices()))
+
+
 class _radio_ack:
     ack = False
     powerDet = False
@@ -107,7 +115,7 @@ class Crazyradio:
     P_M6DBM = 2
     P_0DBM = 3
 
-    def __init__(self, device=None, devid=0):
+    def __init__(self, device=None, devid=0, serial=None):
         """ Create object and scan for USB dongle if no device is supplied """
 
         self.current_channel = None
@@ -116,9 +124,15 @@ class Crazyradio:
 
         if device is None:
             try:
-                device = _find_devices()[devid]
+                if serial is None:
+                    device = _find_devices()[devid]
+                else:
+                    device = _find_devices(serial)
             except Exception:
-                raise Exception('Cannot find a Crazyradio Dongle')
+                if serial is None:
+                    raise Exception('Cannot find a Crazyradio Dongle')
+                else:
+                    raise Exception('Cannot find Crazyradio {}'.format(serial))
 
         self.dev = device
 
@@ -155,10 +169,9 @@ class Crazyradio:
         if (pyusb1 is False):
             if self.handle:
                 self.handle.releaseInterface()
-                self.handle.reset()
         else:
             if self.dev:
-                self.dev.reset()
+                usb.util.dispose_resources(self.dev)
 
         self.handle = None
         self.dev = None

pps_ws/src/d_fall_pps/crazyradio/cflib/utils/__init__.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/positioning/__init__.py

-#!/usr/bin/env python
 # -*- coding: utf-8 -*-
 #
 #     ||          ____  _ __
@@ -7,9 +6,7 @@
 #  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
 #   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
 #
-#  Copyright (C) 2011-2013 Bitcraze AB
-#
-#  Crazyflie Nano Quadcopter Client
+#  Copyright (C) 2017 Bitcraze AB
 #
 #  This program is free software; you can redistribute it and/or
 #  modify it under the terms of the GNU General Public License
@@ -24,6 +21,3 @@
 #  along with this program; if not, write to the Free Software
 #  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
 #  MA  02110-1301, USA.
-"""
-Various utilities that is needed by the cflib.
-"""

dfall_ws/src/dfall_pkg/crazyradio/cflib/positioning/motion_commander.py

+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2017 Bitcraze AB
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+#  You should have received a copy of the GNU General Public License
+#  along with this program; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+#  MA  02110-1301, USA.
+"""
+The MotionCommander is used to make it easy to write scripts that moves the
+Crazyflie around. Some sort of positioning support is required, for instance
+the Flow deck.
+
+The motion commander uses velocity setpoints and does not have a notion of
+absolute position, the error in position will accumulate over time.
+
+The API contains a set of primitives that are easy to understand and use, such
+as "go forward" or "turn around".
+
+There are two flavors of primitives, one that is blocking and returns when
+a motion is completed, while the other starts a motion and returns immediately.
+In the second variation the user has to stop or change the motion when
+appropriate by issuing new commands.
+
+The MotionCommander can be used as context manager using the with keyword. In
+this mode of operation takeoff and landing is executed when the context is
+created/closed.
+"""
+import math
+import sys
+import time
+from threading import Thread
+
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+
+if sys.version_info < (3,):
+    from Queue import Queue, Empty
+else:
+    from queue import Queue, Empty
+
+
+class MotionCommander:
+    """The motion commander"""
+    VELOCITY = 0.2
+    RATE = 360.0 / 5
+
+    def __init__(self, crazyflie, default_height=0.3):
+        """
+        Construct an instance of a MotionCommander
+
+        :param crazyflie: a Crazyflie or SyncCrazyflie instance
+        :param default_height: the default height to fly at
+        """
+        if isinstance(crazyflie, SyncCrazyflie):
+            self._cf = crazyflie.cf
+        else:
+            self._cf = crazyflie
+
+        self.default_height = default_height
+
+        self._is_flying = False
+        self._thread = None
+
+    # Distance based primitives
+
+    def take_off(self, height=None, velocity=VELOCITY):
+        """
+        Takes off, that is starts the motors, goes straigt up and hovers.
+        Do not call this function if you use the with keyword. Take off is
+        done automatically when the context is created.
+
+        :param height: the height (meters) to hover at. None uses the default
+                       height set when constructed.
+        :param velocity: the velocity (meters/second) when taking off
+        :return:
+        """
+        if self._is_flying:
+            raise Exception('Already flying')
+
+        if not self._cf.is_connected():
+            raise Exception('Crazyflie is not connected')
+
+        self._is_flying = True
+        self._reset_position_estimator()
+
+        self._thread = _SetPointThread(self._cf)
+        self._thread.start()
+
+        if height is None:
+            height = self.default_height
+
+        self.up(height, velocity)
+
+    def land(self, velocity=VELOCITY):
+        """
+        Go straight down and turn off the motors.
+
+        Do not call this function if you use the with keyword. Landing is
+        done automatically when the context goes out of scope.
+
+        :param velocity: The velocity (meters/second) when going down
+        :return:
+        """
+        if self._is_flying:
+            self.down(self._thread.get_height(), velocity)
+
+            self._thread.stop()
+            self._thread = None
+
+            self._cf.commander.send_stop_setpoint()
+            self._is_flying = False
+
+    def __enter__(self):
+        self.take_off()
+        return self
+
+    def __exit__(self, exc_type, exc_val, exc_tb):
+        self.land()
+
+    def left(self, distance_m, velocity=VELOCITY):
+        """
+        Go left
+
+        :param distance_m: the distance to travel (meters)
+        :param velocity: the velocity of the motion (meters/second)
+        :return:
+        """
+        self.move_distance(0.0, distance_m, 0.0, velocity)
+
+    def right(self, distance_m, velocity=VELOCITY):
+        """
+        Go right
+
+        :param distance_m: the distance to travel (meters)
+        :param velocity: the velocity of the motion (meters/second)
+        :return:
+        """
+        self.move_distance(0.0, -distance_m, 0.0, velocity)
+
+    def forward(self, distance_m, velocity=VELOCITY):
+        """
+        Go forward
+
+        :param distance_m: the distance to travel (meters)
+        :param velocity: the velocity of the motion (meters/second)
+        :return:
+        """
+        self.move_distance(distance_m, 0.0, 0.0, velocity)
+
+    def back(self, distance_m, velocity=VELOCITY):
+        """
+        Go backwards
+
+        :param distance_m: the distance to travel (meters)
+        :param velocity: the velocity of the motion (meters/second)
+        :return:
+        """
+        self.move_distance(-distance_m, 0.0, 0.0, velocity)
+
+    def up(self, distance_m, velocity=VELOCITY):
+        """
+        Go up
+
+        :param distance_m: the distance to travel (meters)
+        :param velocity: the velocity of the motion (meters/second)
+        :return:
+        """
+        self.move_distance(0.0, 0.0, distance_m, velocity)
+
+    def down(self, distance_m, velocity=VELOCITY):
+        """
+        Go down
+
+        :param distance_m: the distance to travel (meters)
+        :param velocity: the velocity of the motion (meters/second)
+        :return:
+        """
+        self.move_distance(0.0, 0.0, -distance_m, velocity)
+
+    def turn_left(self, angle_degrees, rate=RATE):
+        """
+        Turn to the left, staying on the spot
+
+        :param angle_degrees: How far to turn (degrees)
+        :param rate: The trurning speed (degrees/second)
+        :return:
+        """
+        flight_time = angle_degrees / rate
+
+        self.start_turn_left(rate)
+        time.sleep(flight_time)
+        self.stop()
+
+    def turn_right(self, angle_degrees, rate=RATE):
+        """
+        Turn to the right, staying on the spot
+
+        :param angle_degrees: How far to turn (degrees)
+        :param rate: The trurning speed (degrees/second)
+        :return:
+        """
+        flight_time = angle_degrees / rate
+
+        self.start_turn_right(rate)
+        time.sleep(flight_time)
+        self.stop()
+
+    def circle_left(self, radius_m, velocity=VELOCITY, angle_degrees=360.0):
+        """
+        Go in circle, counter clock wise
+
+        :param radius_m: The radius of the circle (meters)
+        :param velocity: The velocity along the circle (meters/second)
+        :param angle_degrees: How far to go in the circle (degrees)
+        :return:
+        """
+        distance = 2 * radius_m * math.pi * angle_degrees / 360.0
+        flight_time = distance / velocity
+
+        self.start_circle_left(radius_m, velocity)
+        time.sleep(flight_time)
+        self.stop()
+
+    def circle_right(self, radius_m, velocity=VELOCITY, angle_degrees=360.0):
+        """
+        Go in circle, clock wise
+
+        :param radius_m: The radius of the circle (meters)
+        :param velocity: The velocity along the circle (meters/second)
+        :param angle_degrees: How far to go in the circle (degrees)
+        :return:
+        """
+        distance = 2 * radius_m * math.pi * angle_degrees / 360.0
+        flight_time = distance / velocity
+
+        self.start_circle_right(radius_m, velocity)
+        time.sleep(flight_time)
+        self.stop()
+
+    def move_distance(self, distance_x_m, distance_y_m, distance_z_m,
+                      velocity=VELOCITY):
+        """
+        Move in a straight line.
+        positive X is forward
+        positive Y is left
+        positive Z is up
+
+        :param distance_x_m: The distance to travel along the X-axis (meters)
+        :param distance_y_m: The distance to travel along the Y-axis (meters)
+        :param distance_z_m: The distance to travel along the Z-axis (meters)
+        :param velocity: the velocity of the motion (meters/second)
+        :return:
+        """
+        distance = math.sqrt(distance_x_m * distance_x_m +
+                             distance_y_m * distance_y_m +
+                             distance_z_m * distance_z_m)
+        flight_time = distance / velocity
+
+        velocity_x = velocity * distance_x_m / distance
+        velocity_y = velocity * distance_y_m / distance
+        velocity_z = velocity * distance_z_m / distance
+
+        self.start_linear_motion(velocity_x, velocity_y, velocity_z)
+        time.sleep(flight_time)
+        self.stop()
+
+    # Velocity based primitives
+
+    def start_left(self, velocity=VELOCITY):
+        """
+        Start moving left. This function returns immediately.
+
+        :param velocity: The velocity of the motion (meters/second)
+        :return:
+        """
+        self.start_linear_motion(0.0, velocity, 0.0)
+
+    def start_right(self, velocity=VELOCITY):
+        """
+        Start moving right. This function returns immediately.
+
+        :param velocity: The velocity of the motion (meters/second)
+        :return:
+        """
+        self.start_linear_motion(0.0, -velocity, 0.0)
+
+    def start_forward(self, velocity=VELOCITY):
+        """
+        Start moving forward. This function returns immediately.
+
+        :param velocity: The velocity of the motion (meters/second)
+        :return:
+        """
+        self.start_linear_motion(velocity, 0.0, 0.0)
+
+    def start_back(self, velocity=VELOCITY):
+        """
+        Start moving backwards. This function returns immediately.
+
+        :param velocity: The velocity of the motion (meters/second)
+        :return:
+        """
+        self.start_linear_motion(-velocity, 0.0, 0.0)
+
+    def start_up(self, velocity=VELOCITY):
+        """
+        Start moving up. This function returns immediately.
+
+        :param velocity: The velocity of the motion (meters/second)
+        :return:
+        """
+        self.start_linear_motion(0.0, 0.0, velocity)
+
+    def start_down(self, velocity=VELOCITY):
+        """
+        Start moving down. This function returns immediately.
+
+        :param velocity: The velocity of the motion (meters/second)
+        :return:
+        """
+        self.start_linear_motion(0.0, 0.0, -velocity)
+
+    def stop(self):
+        """
+        Stop any motion and hover.
+
+        :return:
+        """
+        self._set_vel_setpoint(0.0, 0.0, 0.0, 0.0)
+
+    def start_turn_left(self, rate=RATE):
+        """
+        Start turning left. This function returns immediately.
+
+        :param rate: The angular rate (degrees/second)
+        :return:
+        """
+        self._set_vel_setpoint(0.0, 0.0, 0.0, -rate)
+
+    def start_turn_right(self, rate=RATE):
+        """
+        Start turning right. This function returns immediately.
+
+        :param rate: The angular rate (degrees/second)
+        :return:
+        """
+        self._set_vel_setpoint(0.0, 0.0, 0.0, rate)
+
+    def start_circle_left(self, radius_m, velocity=VELOCITY):
+        """
+        Start a circular motion to the left. This function returns immediately.
+
+        :param radius_m: The radius of the circle (meters)
+        :param velocity: The velocity of the motion (meters/second)
+        :return:
+        """
+        circumference = 2 * radius_m * math.pi
+        rate = 360.0 * velocity / circumference
+
+        self._set_vel_setpoint(velocity, 0.0, 0.0, -rate)
+
+    def start_circle_right(self, radius_m, velocity=VELOCITY):
+        """
+        Start a circular motion to the right. This function returns immediately
+
+        :param radius_m: The radius of the circle (meters)
+        :param velocity: The velocity of the motion (meters/second)
+        :return:
+        """
+        circumference = 2 * radius_m * math.pi
+        rate = 360.0 * velocity / circumference
+
+        self._set_vel_setpoint(velocity, 0.0, 0.0, rate)
+
+    def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m):
+        """
+        Start a linear motion. This function returns immediately.
+
+        positive X is forward
+        positive Y is left
+        positive Z is up
+
+        :param velocity_x_m: The velocity along the X-axis (meters/second)
+        :param velocity_y_m: The velocity along the Y-axis (meters/second)
+        :param velocity_z_m: The velocity along the Z-axis (meters/second)
+        :return:
+        """
+        self._set_vel_setpoint(
+            velocity_x_m, velocity_y_m, velocity_z_m, 0.0)
+
+    def _set_vel_setpoint(self, velocity_x, velocity_y, velocity_z, rate_yaw):
+        if not self._is_flying:
+            raise Exception('Can not move on the ground. Take off first!')
+        self._thread.set_vel_setpoint(
+            velocity_x, velocity_y, velocity_z, rate_yaw)
+
+    def _reset_position_estimator(self):
+        self._cf.param.set_value('kalman.resetEstimation', '1')
+        time.sleep(0.1)
+        self._cf.param.set_value('kalman.resetEstimation', '0')
+        time.sleep(2)
+
+
+class _SetPointThread(Thread):
+    TERMINATE_EVENT = 'terminate'
+    UPDATE_PERIOD = 0.2
+    ABS_Z_INDEX = 3
+
+    def __init__(self, cf, update_period=UPDATE_PERIOD):
+        Thread.__init__(self)
+        self.update_period = update_period
+
+        self._queue = Queue()
+        self._cf = cf
+
+        self._hover_setpoint = [0.0, 0.0, 0.0, 0.0]
+
+        self._z_base = 0.0
+        self._z_velocity = 0.0
+        self._z_base_time = 0.0
+
+    def stop(self):
+        """
+        Stop the thread and wait for it to terminate
+
+        :return:
+        """
+        self._queue.put(self.TERMINATE_EVENT)
+        self.join()
+
+    def set_vel_setpoint(self, velocity_x, velocity_y, velocity_z, rate_yaw):
+        """Set the velocity setpoint to use for the future motion"""
+        self._queue.put((velocity_x, velocity_y, velocity_z, rate_yaw))
+
+    def get_height(self):
+        """
+        Get the current height of the Crazyflie.
+
+        :return: The height (meters)
+        """
+        return self._hover_setpoint[self.ABS_Z_INDEX]
+
+    def run(self):
+        while True:
+            try:
+                event = self._queue.get(block=True, timeout=self.update_period)
+                if event == self.TERMINATE_EVENT:
+                    return
+
+                self._new_setpoint(*event)
+            except Empty:
+                pass
+
+            self._update_z_in_setpoint()
+            self._cf.commander.send_hover_setpoint(*self._hover_setpoint)
+
+    def _new_setpoint(self, velocity_x, velocity_y, velocity_z, rate_yaw):
+        self._z_base = self._current_z()
+        self._z_velocity = velocity_z
+        self._z_base_time = time.time()
+
+        self._hover_setpoint = [velocity_x, velocity_y, rate_yaw, self._z_base]
+
+    def _update_z_in_setpoint(self):
+        self._hover_setpoint[self.ABS_Z_INDEX] = self._current_z()
+
+    def _current_z(self):
+        now = time.time()
+        return self._z_base + self._z_velocity * (now - self._z_base_time)

dfall_ws/src/dfall_pkg/crazyradio/cflib/positioning/position_hl_commander.py

+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2018 Bitcraze AB
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+#  You should have received a copy of the GNU General Public License
+#  along with this program; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+#  MA  02110-1301, USA.
+"""
+The PositionHlCommander is used to make it easy to write scripts that moves the
+Crazyflie around. Some sort of positioning support is required, for
+instance the Loco Positioning System. The implementation uses the High Level
+Commander and position setpoints.
+
+The API contains a set of primitives that are easy to understand and use, such
+as "go forward" or "turn around".
+
+The PositionHlCommander can be used as context manager using the with keyword.
+In this mode of operation takeoff and landing is executed when the context is
+created/closed.
+"""
+import math
+import time
+
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+
+
+class PositionHlCommander:
+    """The position High Level Commander"""
+
+    CONTROLLER_PID = 1
+    CONTROLLER_MELLINGER = 2
+
+    DEFAULT = None
+
+    def __init__(self, crazyflie,
+                 x=0.0, y=0.0, z=0.0,
+                 default_velocity=0.5,
+                 default_height=0.5,
+                 controller=CONTROLLER_PID):
+        """
+        Construct an instance of a PositionHlCommander
+
+        :param crazyflie: a Crazyflie or SyncCrazyflie instance
+        :param x: Initial position, x
+        :param y: Initial position, y
+        :param z: Initial position, z
+        :param default_velocity: the default velocity to use
+        :param default_height: the default height to fly at
+        :param controller: Which underlying controller to use
+        """
+        if isinstance(crazyflie, SyncCrazyflie):
+            self._cf = crazyflie.cf
+        else:
+            self._cf = crazyflie
+
+        self._default_velocity = default_velocity
+        self._default_height = default_height
+        self._controller = controller
+
+        self._x = x
+        self._y = y
+        self._z = z
+
+        self._is_flying = False
+
+    def take_off(self, height=DEFAULT, velocity=DEFAULT):
+        """
+        Takes off, that is starts the motors, goes straight up and hovers.
+        Do not call this function if you use the with keyword. Take off is
+        done automatically when the context is created.
+
+        :param height: the height (meters) to hover at. None uses the default
+                       height set when constructed.
+        :param velocity: the velocity (meters/second) when taking off
+        :return:
+        """
+        if self._is_flying:
+            raise Exception('Already flying')
+
+        if not self._cf.is_connected():
+            raise Exception('Crazyflie is not connected')
+
+        self._is_flying = True
+        self._reset_position_estimator()
+        self._activate_controller()
+        self._activate_high_level_commander()
+        self._hl_commander = self._cf.high_level_commander
+
+        height = self._height(height)
+
+        duration_s = height / self._velocity(velocity)
+        self._hl_commander.takeoff(height, duration_s)
+        time.sleep(duration_s)
+        self._z = height
+
+    def land(self, velocity=DEFAULT):
+        """
+        Go straight down and turn off the motors.
+
+        Do not call this function if you use the with keyword. Landing is
+        done automatically when the context goes out of scope.
+
+        :param velocity: The velocity (meters/second) when going down
+        :return:
+        """
+        if self._is_flying:
+            duration_s = self._z / self._velocity(velocity)
+            self._hl_commander.land(0, duration_s)
+            time.sleep(duration_s)
+            self._z = 0.0
+
+            self._hl_commander.stop()
+            self._is_flying = False
+
+    def __enter__(self):
+        self.take_off()
+        return self
+
+    def __exit__(self, exc_type, exc_val, exc_tb):
+        self.land()
+
+    def left(self, distance_m, velocity=DEFAULT):
+        """
+        Go left
+
+        :param distance_m: the distance to travel (meters)
+        :param velocity: the velocity of the motion (meters/second)
+        :return:
+        """
+        self.move_distance(0.0, distance_m, 0.0, velocity)
+
+    def right(self, distance_m, velocity=DEFAULT):
+        """
+        Go right
+
+        :param distance_m: the distance to travel (meters)
+        :param velocity: the velocity of the motion (meters/second)
+        :return:
+        """
+        self.move_distance(0.0, -distance_m, 0.0, velocity)
+
+    def forward(self, distance_m, velocity=DEFAULT):
+        """
+        Go forward
+
+        :param distance_m: the distance to travel (meters)
+        :param velocity: the velocity of the motion (meters/second)
+        :return:
+        """
+        self.move_distance(distance_m, 0.0, 0.0, velocity)
+
+    def back(self, distance_m, velocity=DEFAULT):
+        """
+        Go backwards
+
+        :param distance_m: the distance to travel (meters)
+        :param velocity: the velocity of the motion (meters/second)
+        :return:
+        """
+        self.move_distance(-distance_m, 0.0, 0.0, velocity)
+
+    def up(self, distance_m, velocity=DEFAULT):
+        """
+        Go up
+
+        :param distance_m: the distance to travel (meters)
+        :param velocity: the velocity of the motion (meters/second)
+        :return:
+        """
+        self.move_distance(0.0, 0.0, distance_m, velocity)
+
+    def down(self, distance_m, velocity=DEFAULT):
+        """
+        Go down
+
+        :param distance_m: the distance to travel (meters)
+        :param velocity: the velocity of the motion (meters/second)
+        :return:
+        """
+        self.move_distance(0.0, 0.0, -distance_m, velocity)
+
+    def move_distance(self, distance_x_m, distance_y_m, distance_z_m,
+                      velocity=DEFAULT):
+        """
+        Move in a straight line.
+        positive X is forward
+        positive Y is left
+        positive Z is up
+
+        :param distance_x_m: The distance to travel along the X-axis (meters)
+        :param distance_y_m: The distance to travel along the Y-axis (meters)
+        :param distance_z_m: The distance to travel along the Z-axis (meters)
+        :param velocity: the velocity of the motion (meters/second)
+        :return:
+        """
+
+        x = self._x + distance_x_m
+        y = self._y + distance_y_m
+        z = self._z + distance_z_m
+
+        self.go_to(x, y, z, velocity)
+
+    def go_to(self, x, y, z=DEFAULT, velocity=DEFAULT):
+        """
+        Go to a position
+
+        :param x: X coordinate
+        :param y: Y coordinate
+        :param z: Z coordinate
+        :param velocity: the velocity (meters/second)
+        :return:
+        """
+
+        z = self._height(z)
+
+        dx = x - self._x
+        dy = y - self._y
+        dz = z - self._z
+        distance = math.sqrt(dx * dx + dy * dy + dz * dz)
+
+        if distance > 0.0:
+            duration_s = distance / self._velocity(velocity)
+            self._hl_commander.go_to(x, y, z, 0, duration_s)
+            time.sleep(duration_s)
+
+            self._x = x
+            self._y = y
+            self._z = z
+
+    def set_default_velocity(self, velocity):
+        """
+        Set the default velocity to use in commands when no velocity is defined
+        :param velocity: The default velocity (meters/s)
+        :return:
+        """
+        self._default_velocity = velocity
+
+    def set_default_height(self, height):
+        """
+        Set the default height to use in commands when no height is defined
+
+        :param height: The default height (meters)
+        :return:
+        """
+        self._default_height = height
+
+    def set_controller(self, controller):
+        self._controller = controller
+
+    def get_position(self):
+        """
+        Get the current position
+        :return: (x, y, z)
+        """
+        return self._x, self._y, self._z
+
+    def _reset_position_estimator(self):
+        self._cf.param.set_value('kalman.initialX', '{:.2f}'.format(self._x))
+        self._cf.param.set_value('kalman.initialY', '{:.2f}'.format(self._y))
+        self._cf.param.set_value('kalman.initialZ', '{:.2f}'.format(self._z))
+
+        self._cf.param.set_value('kalman.resetEstimation', '1')
+        time.sleep(0.1)
+        self._cf.param.set_value('kalman.resetEstimation', '0')
+        time.sleep(2)
+
+    def _activate_high_level_commander(self):
+        self._cf.param.set_value('commander.enHighLevel', '1')
+
+    def _activate_controller(self):
+        value = str(self._controller)
+        self._cf.param.set_value('stabilizer.controller', value)
+
+    def _velocity(self, velocity):
+        if velocity is self.DEFAULT:
+            return self._default_velocity
+        return velocity
+
+    def _height(self, height):
+        if height is self.DEFAULT:
+            return self._default_height
+        return height

pps_ws/src/d_fall_pps/crazyradio/cflib/utils/callbacks.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/utils/callbacks.py

@@ -50,5 +50,6 @@ class Caller():
 
     def call(self, *args):
         """ Call the callbacks registered with the arguments args """
-        for cb in self.callbacks:
+        copy_of_callbacks = list(self.callbacks)
+        for cb in copy_of_callbacks:
             cb(*args)

pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/syncLogger.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/utils/multiranger.py

@@ -6,9 +6,7 @@
 #  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
 #   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
 #
-#  Copyright (C) 2016 Bitcraze AB
-#
-#  Crazyflie Nano Quadcopter Client
+#  Copyright (C) 2018 Bitcraze AB
 #
 #  This program is free software; you can redistribute it and/or
 #  modify it under the terms of the GNU General Public License
@@ -23,93 +21,95 @@
 #  along with this program; if not, write to the Free Software
 #  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
 #  MA  02110-1301, USA.
-"""
-This class provides synchronous access to log data from the Crazyflie.
-
-It acts as an iterator and returns the next value on each iteration.
-If no value is available it blocks until log data is available again.
-"""
-import sys
-
+from cflib.crazyflie.log import LogConfig
 from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
 
-if sys.version_info < (3,):
-    from Queue import Queue
-else:
-    from queue import Queue
-
 
-class SyncLogger:
-    DISCONNECT_EVENT = 'DISCONNECT_EVENT'
+class Multiranger:
+    FRONT = 'range.front'
+    BACK = 'range.back'
+    LEFT = 'range.left'
+    RIGHT = 'range.right'
+    UP = 'range.up'
+    DOWN = 'range.zrange'
 
-    def __init__(self, crazyflie, log_config):
-        """
-        Construct an instance of a SyncLogger
-
-        Takes an Crazyflie or SyncCrazyflie instance and a log configuration
-        """
+    def __init__(self, crazyflie, rate_ms=100, zranger=False):
         if isinstance(crazyflie, SyncCrazyflie):
             self._cf = crazyflie.cf
         else:
             self._cf = crazyflie
+        self._log_config = self._create_log_config(rate_ms)
 
-        self._log_config = log_config
-        self._cf.log.add_config(self._log_config)
+        self._up_distance = None
+        self._front_distance = None
+        self._back_distance = None
+        self._left_distance = None
+        self._right_distance = None
+        self._down_distance = None
 
-        self._queue = Queue()
+    def _create_log_config(self, rate_ms):
+        log_config = LogConfig('multiranger', rate_ms)
+        log_config.add_variable(self.FRONT)
+        log_config.add_variable(self.BACK)
+        log_config.add_variable(self.LEFT)
+        log_config.add_variable(self.RIGHT)
+        log_config.add_variable(self.UP)
+        log_config.add_variable(self.DOWN)
 
-        self._is_connected = False
+        log_config.data_received_cb.add_callback(self._data_received)
 
-    def connect(self):
-        if self._is_connected:
-            raise Exception('Already connected')
+        return log_config
 
-        self._cf.disconnected.add_callback(self._disconnected)
-        self._log_config.data_received_cb.add_callback(self._log_callback)
+    def start(self):
+        self._cf.log.add_config(self._log_config)
         self._log_config.start()
 
-        self._is_connected = True
-
-    def disconnect(self):
-        if self._is_connected:
-            self._log_config.stop()
-            self._log_config.delete()
-
-            self._log_config.data_received_cb.remove_callback(
-                self._log_callback)
-            self._cf.disconnected.remove_callback(self._disconnected)
+    def _convert_log_to_distance(self, data):
+        if data >= 8000:
+            return None
+        else:
+            return data / 1000.0
 
-            self._queue.empty()
+    def _data_received(self, timestamp, data, logconf):
+        self._up_distance = self._convert_log_to_distance(data[self.UP])
+        self._front_distance = self._convert_log_to_distance(data[self.FRONT])
+        self._back_distance = self._convert_log_to_distance(data[self.BACK])
+        self._left_distance = self._convert_log_to_distance(data[self.LEFT])
+        self._right_distance = self._convert_log_to_distance(data[self.RIGHT])
+        if self.DOWN in data:
+            self._down_distance = self._convert_log_to_distance(data[self.DOWN]
+                                                                )
 
-            self._is_connected = False
+    def stop(self):
+        self._log_config.delete()
 
-    def is_connected(self):
-        return self._is_connected
+    @property
+    def up(self):
+        return self._up_distance
 
-    def __iter__(self):
-        return self
+    @property
+    def left(self):
+        return self._left_distance
 
-    def __next__(self):
-        if not self._is_connected:
-            raise StopIteration
+    @property
+    def right(self):
+        return self._right_distance
 
-        data = self._queue.get()
+    @property
+    def front(self):
+        return self._front_distance
 
-        if data == self.DISCONNECT_EVENT:
-            raise StopIteration
+    @property
+    def back(self):
+        return self._back_distance
 
-        return data
+    @property
+    def down(self):
+        return self._down_distance
 
     def __enter__(self):
-        self.connect()
+        self.start()
         return self
 
     def __exit__(self, exc_type, exc_val, exc_tb):
-        self.disconnect()
-
-    def _log_callback(self, ts, data, logblock):
-        self._queue.put((ts, data, logblock))
-
-    def _disconnected(self, link_uri):
-        self._queue.put(self.DISCONNECT_EVENT)
-        self.disconnect()
+        self.stop()

dfall_ws/src/dfall_pkg/crazyradio/docs/_data/menu.yml

+- page_id: home
+- title: Install Instructions
+  subs:
+    - {page_id: install_from_source}
+    - {page_id: usd_permissions}
+- title: User guides
+  subs:
+    - {page_id: python_api}
+- title: Explanation Functionalities
+  subs:
+    - {page_id: crazyradio_lib}
+- title: Development
+  subs:
+    - {page_id: testing}
+    - {page_id: matlab}
+    - {page_id: eeprom}

dfall_ws/src/dfall_pkg/crazyradio/docs/crazyradio_lib.md

+---
+title: Python Crazyradio Library
+page_id: crazyradio_lib 
+---
+
+The python crazyradio lib can be found in the Crazyflie python library git repos
+<https://github.com/bitcraze/crazyflie-lib-python/blob/master/cflib/drivers/crazyradio.py>.
+It is a single file that implements the low level Crazyradio dongle
+functionalities.
+
+Theory of operation
+-------------------
+
+The Crazyradio is configured in PTX mode which means that it will start
+all comunication. If a device in PRX mode is on the same address,
+channel and datarate, the device will send back an ack packet that may
+contains data.
+
+       Crazyradio    Device
+          _             _
+          |             |
+          |  Packet     |
+          |------------>|
+          |        Ack  |
+          |<------------|
+          |             |
+
+This is an example on how to use the lib:
+
+``` {.python}
+import crazyradio
+
+cr = crazyradio.Crazyradio()
+cr.set_channel(90)
+cr.set_data_rate(cr.DR_2MPS)
+
+res = cr.send_packet([0xff, ])
+print res.ack                   # At true if an ack has been received
+print res.data                  # The ack payload data
+```
+
+API
+---
+
+### Crazyradio
+
+#### Fields
+
+None
+
+#### Methods
+
+##### [init]{.underline}(self. device=None, devid=0)
+
+|  Name         | `_ _init_ _` (Constructor)|
+|  -------------| ------------------------------------------------|
+|  Parameters   | (USBDevice) device, (int) devid|
+|  Returns      | None|
+|  Description  | Initialize the Crazyradio object. If device is not specified, a list of available Crazyradio is made and devId selects the Crazyradio used (by default the first one)|
+
+##### close(self)
+
+|  Name         | `close`|
+|  -------------| --------------------------------------------------------------------|
+|  Parameters   | None|
+|  Returns      | None|
+|  Description  | Close the USB device. Should be called before closing the program.|
+
+##### set\_channel(self, channel)
+
+ | Name         | `set_channel` |
+ | -------------| ---------------------------------------------------------------------|
+ | Parameters   | (int) channel |
+ | Returns      | None |
+ | Description  | Set the Crazyradio channel. Channel must be between 0 and 125. Channels are spaced by 1MHz starting at 2400MHz and ending at 2525MHz. |
+
+##### set\_address(self, address)
+
+|  Name         | `set_address`|
+|  -------------| ---------------------------|
+|  Parameters   | (list of int) address|
+|  Returns      | None|
+|  Description  | Set the Crazyradio address. The *address* is 5 bytes long. It should be a list of 5 bytes values.|
+
+##### set\_data\_rate(self, datarate)
+
+ | Name          |`set_datarate`|
+ | ------------- |------------------|
+ | Parameters    |(int) datarate|
+ | Returns       |None|
+ | Description   |Set the Crazyradio datarate. *Datarate* is one of `DR_250KPS`, `DR_1MPS` or `DR_2MPS`|
+
+##### set\_power(self, power)
+
+|  Name          |`set_power`|
+|  ------------- |----------------------------------------------------------------------------------|
+|  Parameters    |(int) power|
+|  Returns       |None|
+|  Description   |Set the Crazyradio transmit power. *Power* is one of `P_M18DBM`, `P_M12DBM`, `P_M6DBM` or `P_0DBM` respectively for -18dBm, -12dBm, -6dBm and 0dBm.|
+
+##### set\_arc(self, arc)
+
+ | Name         | `set_arc`|
+ | -------------| ----------------------------------------------------------------------------------|
+ | Parameters   | (int) arc|
+ | Returns      | None|
+ | Description  | Set the number of retry. 0\<*arc*\<15. See nRF24L01 documentation for more info.|
+
+##### set\_ard\_time(self, us)
+
+|  Name         | `set_ard_time`|
+|  -------------| ---------------------------------------------------------------------------------|
+|  Parameters   | (int) us|
+|  Returns      | None|
+|  Description  | Set the time to wait for an Ack in micro seconds. 250\<*us*\<4000. The wait time for an Ack packet corresponds to the time to receive the biggest expected Ack. See nRF24L01 documentation for more info.|
+
+##### set\_ard\_bytes(self, nbytes)
+
+|  Name         | `set_ard_bytes`|
+|  -------------| ---------------------------------------------------------------|
+|  Parameters   | (int) nbytes|
+|  Returns      | None|
+|  Description  | Set the time to wait for an Ack in number of ack payload bytes. The Crazyradio will calculate the correct time with the currently selected datarate.|
+
+##### set\_cont\_carrier(self, active)
+
+|  Name         | `set_cont_carrier`|
+|  -------------| --------------------------------------------------------|
+|  Parameters   | (bool) active|
+|  Returns      | None|
+|  Description  | Enable or disable the continious carrier mode. In continious carrier the Crazyradio transmit a constant sinus at the currently set frequency (channel) and power. This is a test mode that can affect other 2.4GHz devices (ex. wifi) it should only be used in a lab for test purposes.|
+
+##### scan\_channels(self, start, stop, packet)
+
+|  Name         | `scan_channels`|
+|  -------------| ----------------------------------|
+|  Parameters   | (int) start, (int) stop, (int) packet|
+|  Returns      | (list) List of channels that Acked the packet|
+|  Description  | Sends \\packet\\ to all channels from start to stop. Returns a list of the channels for which an ACK was received.|
+
+##### send\_packet(self, dataOut)
+
+|  Name         | `send_packet`|
+|  -------------| --------------------------------------------------------|
+|  Parameters   | (list or tuple) dataOut|
+|  Returns      | ([\_radio\_ack](#_radio_ack)) Ack status|
+|  Description  | Sends the packet *dataOut* on the configured channel and datarate. Waits for an ack and returns a \_radio\_ack object that contains the ack status and optional ack payload data.|
+
+### \_radio\_ack
+
+#### Fields
+
+|  (bool) ack       | At `True` if an Ack packet has been receive (ie. if the packet was received by the device)|
+ | ----------------- |------------------------------------------|
+ | (bool) powerDet  | Indicate the nRF24LU1 power detector status. See nRF24LU1 documentation for more information.|
+|  (int) retry     |  Number of retry before an ack was received|
+|  (tuple) data     | Data payload received in the Ack packet|
+
+#### Methods
+
+None