pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/bootloader/__init__.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/bootloader/__init__.py

@@ -177,7 +177,7 @@ class Bootloader:
                         file_to_flash['start_page'] = file_to_flash[
                             'target'].start_page
                         files_to_flash += (file_to_flash,)
-            except KeyError as e:
+            except KeyError:
                 print('Could not find a file for {} in {}'.format(
                     current_target, filename))
                 return False

pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/bootloader/cloader.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/bootloader/cloader.py

@@ -99,26 +99,32 @@ class Cloader:
         pk.data = (target_id, 0xFF)
         self.link.send_packet(pk)
 
-        pk = self.link.receive_packet(1)
-
-        while ((not pk or pk.header != 0xFF or
-                struct.unpack('<BB', pk.data[0:2]) != (target_id, 0xFF)
-                ) and retry_counter >= 0):
+        got_answer = False
+        while(not got_answer and retry_counter >= 0):
             pk = self.link.receive_packet(1)
-            retry_counter -= 1
-
-        if pk:
+            if pk and pk.header == 0xFF:
+                try:
+                    data = struct.unpack('<BB', pk.data[0:2])
+                    got_answer = data == (target_id, 0xFF)
+                except struct.error:
+                    # Failed unpacking, retry
+                    pass
+
+        if got_answer:
             new_address = (0xb1,) + struct.unpack('<BBBB', pk.data[2:6][::-1])
 
-            pk = CRTPPacket()
-            pk.set_header(0xFF, 0xFF)
-            pk.data = (target_id, 0xF0, 0x00)
-            self.link.send_packet(pk)
+            # The reset packet arrival cannot be checked.
+            # Send it more than one time to increase the chances it makes it.
+            for _ in range(10):
+                pk = CRTPPacket()
+                pk.set_header(0xFF, 0xFF)
+                pk.data = (target_id, 0xF0, 0x00)
+                self.link.send_packet(pk)
 
             addr = int(binascii.hexlify(
                 struct.pack('B' * 5, *new_address)), 16)
 
-            time.sleep(0.2)
+            time.sleep(1)
             self.link.close()
             time.sleep(0.2)
             self.link = cflib.crtp.get_link_driver(
@@ -359,6 +365,12 @@ class Cloader:
         # print "Write page", flashPage
         # print "Writing page [%d] and [%d] forward" % (flashPage, nPage)
         pk = None
+
+        # Flushing downlink ...
+        pk = self.link.receive_packet(0)
+        while pk is not None:
+            pk = self.link.receive_packet(0)
+
         retry_counter = 5
         # print "Flasing to 0x{:X}".format(addr)
         while ((not pk or pk.header != 0xFF or
@@ -369,7 +381,14 @@ class Cloader:
             pk.data = struct.pack('<BBHHH', addr, 0x18, page_buffer,
                                   target_page, page_count)
             self.link.send_packet(pk)
-            pk = self.link.receive_packet(1)
+
+            # Timeout for writing to flash is raised from 1s (used elsewhere
+            # in this module) to 2.5s because it may take more than a second
+            # to erase a page on the STM32F405.
+            #
+            # See https://github.com/bitcraze/crazyflie-lib-python/issues/98
+            # for more details.
+            pk = self.link.receive_packet(2.5)
             retry_counter -= 1
 
         if retry_counter < 0:

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

@@ -50,6 +50,7 @@ from .mem import Memory
 from .param import Param
 from .platformservice import PlatformService
 from .toccache import TocCache
+from cflib.crazyflie.high_level_commander import HighLevelCommander
 from cflib.utils.callbacks import Caller
 
 __author__ = 'Bitcraze AB'
@@ -105,9 +106,11 @@ class Crazyflie():
 
         self.incoming = _IncomingPacketHandler(self)
         self.incoming.setDaemon(True)
-        self.incoming.start()
+        if self.link:
+            self.incoming.start()
 
         self.commander = Commander(self)
+        self.high_level_commander = HighLevelCommander(self)
         self.loc = Localization(self)
         self.extpos = Extpos(self)
         self.log = Log(self)
@@ -155,6 +158,9 @@ class Crazyflie():
         """Start the connection setup by refreshing the TOCs"""
         logger.info('We are connected[%s], request connection setup',
                     self.link_uri)
+        self.platform.fetch_platform_informations(self._platform_info_fetched)
+
+    def _platform_info_fetched(self):
         self.log.refresh_toc(self._log_toc_updated_cb, self._toc_cache)
 
     def _param_toc_updated_cb(self):
@@ -223,6 +229,8 @@ class Crazyflie():
                 logger.warning(message)
                 self.connection_failed.call(link_uri, message)
             else:
+                if not self.incoming.isAlive():
+                    self.incoming.start()
                 # Add a callback so we can check that any data is coming
                 # back from the copter
                 self.packet_received.add_callback(
@@ -254,6 +262,11 @@ class Crazyflie():
         self._answer_patterns = {}
         self.disconnected.call(self.link_uri)
 
+    """Check if the communication link is open or not."""
+
+    def is_connected(self):
+        return self.connected_ts is not None
+
     def add_port_callback(self, port, cb):
         """Add a callback to cb on port"""
         self.incoming.add_port_callback(port, cb)

pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/commander.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/commander.py

@@ -39,6 +39,7 @@ TYPE_STOP = 0
 TYPE_VELOCITY_WORLD = 1
 TYPE_ZDISTANCE = 2
 TYPE_HOVER = 5
+TYPE_POSITION = 7
 
 
 class Commander():
@@ -127,3 +128,17 @@ class Commander():
         pk.data = struct.pack('<Bffff', TYPE_HOVER,
                               vx, vy, yawrate, zdistance)
         self._cf.send_packet(pk)
+
+    def send_position_setpoint(self, x, y, z, yaw):
+        """
+        Control mode where the position is sent as absolute x,y,z coordinate in
+        meter and the yaw is the absolute orientation.
+
+        x and y are in m
+        yaw is in degrees
+        """
+        pk = CRTPPacket()
+        pk.port = CRTPPort.COMMANDER_GENERIC
+        pk.data = struct.pack('<Bffff', TYPE_POSITION,
+                              x, y, z, yaw)
+        self._cf.send_packet(pk)

pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/extpos.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/extpos.py

@@ -7,7 +7,7 @@
 #  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
 #   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
 #
-#  Copyright (C) 2011-2013 Bitcraze AB
+#  Copyright (C) 2011-2020 Bitcraze AB
 #
 #  Crazyflie Nano Quadcopter Client
 #
@@ -50,3 +50,11 @@ class Extpos():
         """
 
         self._cf.loc.send_extpos([x, y, z])
+
+    def send_extpose(self, x, y, z, qx, qy, qz, qw):
+        """
+        Send the current Crazyflie X, Y, Z position and attitude as a
+        normalized quaternion. This is going to be forwarded to the
+        Crazyflie's position estimator.
+        """
+        self._cf.loc.send_extpose([x, y, z], [qx, qy, qz, qw])

dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/high_level_commander.py

+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2018-2020 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.
+"""
+Used for sending high level setpoints to the Crazyflie
+"""
+import struct
+
+from cflib.crtp.crtpstack import CRTPPacket
+from cflib.crtp.crtpstack import CRTPPort
+
+__author__ = 'Bitcraze AB'
+__all__ = ['HighLevelCommander']
+
+
+class HighLevelCommander():
+    """
+    Used for sending high level setpoints to the Crazyflie
+    """
+
+    COMMAND_SET_GROUP_MASK = 0
+    COMMAND_STOP = 3
+    COMMAND_GO_TO = 4
+    COMMAND_START_TRAJECTORY = 5
+    COMMAND_DEFINE_TRAJECTORY = 6
+    COMMAND_TAKEOFF_2 = 7
+    COMMAND_LAND_2 = 8
+
+    ALL_GROUPS = 0
+
+    TRAJECTORY_LOCATION_MEM = 1
+    TRAJECTORY_TYPE_POLY4D = 0
+
+    def __init__(self, crazyflie=None):
+        """
+        Initialize the object.
+        """
+        self._cf = crazyflie
+
+    def set_group_mask(self, group_mask=ALL_GROUPS):
+        """
+        Set the group mask that the Crazyflie belongs to
+
+        :param group_mask: mask for which groups this CF belongs to
+        """
+        self._send_packet(struct.pack('<BB',
+                                      self.COMMAND_SET_GROUP_MASK,
+                                      group_mask))
+
+    def takeoff(self, absolute_height_m, duration_s, group_mask=ALL_GROUPS,
+                yaw=0.0):
+        """
+        vertical takeoff from current x-y position to given height
+
+        :param absolute_height_m: absolut (m)
+        :param duration_s: time it should take until target height is
+                           reached (s)
+        :param group_mask: mask for which CFs this should apply to
+        :param yaw: yaw (rad). Use current yaw if set to None.
+        """
+        target_yaw = yaw
+        useCurrentYaw = False
+        if yaw is None:
+            target_yaw = 0.0
+            useCurrentYaw = True
+
+        self._send_packet(struct.pack('<BBff?f',
+                                      self.COMMAND_TAKEOFF_2,
+                                      group_mask,
+                                      absolute_height_m,
+                                      target_yaw,
+                                      useCurrentYaw,
+                                      duration_s))
+
+    def land(self, absolute_height_m, duration_s, group_mask=ALL_GROUPS,
+             yaw=0.0):
+        """
+        vertical land from current x-y position to given height
+
+        :param absolute_height_m: absolut (m)
+        :param duration_s: time it should take until target height is
+                           reached (s)
+        :param group_mask: mask for which CFs this should apply to
+        :param yaw: yaw (rad). Use current yaw if set to None.
+        """
+        target_yaw = yaw
+        useCurrentYaw = False
+        if yaw is None:
+            target_yaw = 0.0
+            useCurrentYaw = True
+
+        self._send_packet(struct.pack('<BBff?f',
+                                      self.COMMAND_LAND_2,
+                                      group_mask,
+                                      absolute_height_m,
+                                      target_yaw,
+                                      useCurrentYaw,
+                                      duration_s))
+
+    def stop(self, group_mask=ALL_GROUPS):
+        """
+        stops the current trajectory (turns off the motors)
+
+        :param group_mask: mask for which CFs this should apply to
+        :return:
+        """
+        self._send_packet(struct.pack('<BB',
+                                      self.COMMAND_STOP,
+                                      group_mask))
+
+    def go_to(self, x, y, z, yaw, duration_s, relative=False,
+              group_mask=ALL_GROUPS):
+        """
+        Go to an absolute or relative position
+
+        :param x: x (m)
+        :param y: y (m)
+        :param z: z (m)
+        :param yaw: yaw (radians)
+        :param duration_s: time it should take to reach the position (s)
+        :param relative: True if x, y, z is relative to the current position
+        :param group_mask: mask for which CFs this should apply to
+        """
+        self._send_packet(struct.pack('<BBBfffff',
+                                      self.COMMAND_GO_TO,
+                                      group_mask,
+                                      relative,
+                                      x, y, z,
+                                      yaw,
+                                      duration_s))
+
+    def start_trajectory(self, trajectory_id, time_scale=1.0, relative=False,
+                         reversed=False, group_mask=ALL_GROUPS):
+        """
+        starts executing a specified trajectory
+
+        :param trajectory_id: id of the trajectory (previously defined by
+               define_trajectory)
+        :param time_scale: time factor; 1.0 = original speed;
+                                        >1.0: slower;
+                                        <1.0: faster
+        :param relative: set to True, if trajectory should be shifted to
+               current setpoint
+        :param reversed: set to True, if trajectory should be executed in
+               reverse
+        :param group_mask: mask for which CFs this should apply to
+        :return:
+        """
+        self._send_packet(struct.pack('<BBBBBf',
+                                      self.COMMAND_START_TRAJECTORY,
+                                      group_mask,
+                                      relative,
+                                      reversed,
+                                      trajectory_id,
+                                      time_scale))
+
+    def define_trajectory(self, trajectory_id, offset, n_pieces):
+        """
+        Define a trajectory that has previously been uploaded to memory.
+
+        :param trajectory_id: The id of the trajectory
+        :param offset: offset in uploaded memory
+        :param n_pieces: Nr of pieces in the trajectory
+        :return:
+        """
+        self._send_packet(struct.pack('<BBBBIB',
+                                      self.COMMAND_DEFINE_TRAJECTORY,
+                                      trajectory_id,
+                                      self.TRAJECTORY_LOCATION_MEM,
+                                      self.TRAJECTORY_TYPE_POLY4D,
+                                      offset,
+                                      n_pieces))
+
+    def _send_packet(self, data):
+        pk = CRTPPacket()
+        pk.port = CRTPPort.SETPOINT_HL
+        pk.data = data
+        self._cf.send_packet(pk)

pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/localization.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/localization.py

@@ -7,7 +7,7 @@
 #  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
 #   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
 #
-#  Copyright (C) 2017 Bitcraze AB
+#  Copyright (C) 2017-2020 Bitcraze AB
 #
 #  Crazyflie Nano Quadcopter Client
 #
@@ -56,9 +56,15 @@ class Localization():
     GENERIC_CH = 1
 
     # Location message types for generig channel
-    RANGE_STREAM_REPORT = 0x00
-    RANGE_STREAM_REPORT_FP16 = 0x01
-    LPS_SHORT_LPP_PACKET = 0x02
+    RANGE_STREAM_REPORT = 0
+    RANGE_STREAM_REPORT_FP16 = 1
+    LPS_SHORT_LPP_PACKET = 2
+    EMERGENCY_STOP = 3
+    EMERGENCY_STOP_WATCHDOG = 4
+    COMM_GNSS_NMEA = 6
+    COMM_GNSS_PROPRIETARY = 7
+    EXT_POSE = 8
+    EXT_POSE_PACKED = 9
 
     def __init__(self, crazyflie=None):
         """
@@ -110,6 +116,22 @@ class Localization():
         pk.data = struct.pack('<fff', pos[0], pos[1], pos[2])
         self._cf.send_packet(pk)
 
+    def send_extpose(self, pos, quat):
+        """
+        Send the current Crazyflie pose (position [x, y, z] and
+        attitude quaternion [qx, qy, qz, qw]). This is going to be forwarded
+        to the Crazyflie's position estimator.
+        """
+
+        pk = CRTPPacket()
+        pk.port = CRTPPort.LOCALIZATION
+        pk.channel = self.GENERIC_CH
+        pk.data = struct.pack('<Bfffffff',
+                              self.EXT_POSE,
+                              pos[0], pos[1], pos[2],
+                              quat[0], quat[1], quat[2], quat[3])
+        self._cf.send_packet(pk)
+
     def send_short_lpp_packet(self, dest_id, data):
         """
         Send ultra-wide-band LPP packet to dest_id

pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/log.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/log.py

@@ -79,8 +79,10 @@ CHAN_SETTINGS = 1
 CHAN_LOGDATA = 2
 
 # Commands used when accessing the Table of Contents
-CMD_TOC_ELEMENT = 0
-CMD_TOC_INFO = 1
+CMD_TOC_ELEMENT = 0  # original version: up to 255 entries
+CMD_TOC_INFO = 1    # original version: up to 255 entries
+CMD_GET_ITEM_V2 = 2  # version 2: up to 16k entries
+CMD_GET_INFO_V2 = 3  # version 2: up to 16k entries
 
 # Commands used when accessing the Log configurations
 CMD_CREATE_BLOCK = 0
@@ -89,6 +91,8 @@ CMD_DELETE_BLOCK = 2
 CMD_START_LOGGING = 3
 CMD_STOP_LOGGING = 4
 CMD_RESET_LOGGING = 5
+CMD_CREATE_BLOCK_V2 = 6
+CMD_APPEND_BLOCK_V2 = 7
 
 # Possible states when receiving TOC
 IDLE = 'IDLE'
@@ -150,8 +154,11 @@ class LogConfig(object):
         self.added_cb = Caller()
         self.err_no = 0
 
+        # These 3 variables are set by the log subsystem when the bock is added
         self.id = 0
         self.cf = None
+        self.useV2 = False
+
         self.period = int(period_in_ms / 10)
         self.period_in_ms = period_in_ms
         self._added = False
@@ -214,7 +221,10 @@ class LogConfig(object):
         """Save the log configuration in the Crazyflie"""
         pk = CRTPPacket()
         pk.set_header(5, CHAN_SETTINGS)
-        pk.data = (CMD_CREATE_BLOCK, self.id)
+        if self.useV2:
+            pk.data = (CMD_CREATE_BLOCK_V2, self.id)
+        else:
+            pk.data = (CMD_CREATE_BLOCK, self.id)
         for var in self.variables:
             if (var.is_toc_variable() is False):  # Memory location
                 logger.debug('Logging to raw memory %d, 0x%04X',
@@ -228,9 +238,18 @@ class LogConfig(object):
                              self.cf.log.toc.get_element_id(
                                  var.name), var.get_storage_and_fetch_byte())
                 pk.data.append(var.get_storage_and_fetch_byte())
-                pk.data.append(self.cf.log.toc.get_element_id(var.name))
+                if self.useV2:
+                    ident = self.cf.log.toc.get_element_id(var.name)
+                    pk.data.append(ident & 0x0ff)
+                    pk.data.append((ident >> 8) & 0x0ff)
+                else:
+                    pk.data.append(self.cf.log.toc.get_element_id(var.name))
         logger.debug('Adding log block id {}'.format(self.id))
-        self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))
+        if self.useV2:
+            self.cf.send_packet(pk, expected_reply=(
+                CMD_CREATE_BLOCK_V2, self.id))
+        else:
+            self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))
 
     def start(self):
         """Start the logging for this entry"""
@@ -337,21 +356,21 @@ class LogTocElement:
             raise KeyError(
                 'Type [%d] not found in LogTocElement.types!' % ident)
 
-    def __init__(self, data=None):
+    def __init__(self, ident=0, data=None):
         """TocElement creator. Data is the binary payload of the element."""
 
+        self.ident = ident
+
         if (data):
-            naming = data[2:]
+            naming = data[1:]
             zt = bytearray((0, ))
             self.group = naming[:naming.find(zt)].decode('ISO-8859-1')
             self.name = naming[naming.find(zt) + 1:-1].decode('ISO-8859-1')
 
-            self.ident = data[0]
-
-            self.ctype = LogTocElement.get_cstring_from_id(data[1])
-            self.pytype = LogTocElement.get_unpack_string_from_id(data[1])
+            self.ctype = LogTocElement.get_cstring_from_id(data[0])
+            self.pytype = LogTocElement.get_unpack_string_from_id(data[0])
 
-            self.access = data[1] & 0x10
+            self.access = data[0] & 0x10
 
 
 class Log():
@@ -386,6 +405,8 @@ class Log():
 
         self._config_id_counter = 1
 
+        self._useV2 = False
+
     def add_config(self, logconf):
         """Add a log configuration to the logging framework.
 
@@ -436,6 +457,7 @@ class Log():
             logconf.valid = True
             logconf.cf = self.cf
             logconf.id = self._config_id_counter
+            logconf.useV2 = self._useV2
             self._config_id_counter = (self._config_id_counter + 1) % 255
             self.log_blocks.append(logconf)
             self.block_added_cb.call(logconf)
@@ -448,6 +470,8 @@ class Log():
     def refresh_toc(self, refresh_done_callback, toc_cache):
         """Start refreshing the table of loggale variables"""
 
+        self._useV2 = self.cf.platform.get_protocol_version() >= 4
+
         self._toc_cache = toc_cache
         self._refresh_callback = refresh_done_callback
         self.toc = None
@@ -473,7 +497,7 @@ class Log():
             id = payload[0]
             error_status = payload[1]
             block = self._find_block(id)
-            if (cmd == CMD_CREATE_BLOCK):
+            if cmd == CMD_CREATE_BLOCK or cmd == CMD_CREATE_BLOCK_V2:
                 if (block is not None):
                     if error_status == 0 or error_status == errno.EEXIST:
                         if not block.added:

pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/mem.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/mem.py

@@ -72,6 +72,10 @@ class MemoryElement(object):
     TYPE_1W = 1
     TYPE_DRIVER_LED = 0x10
     TYPE_LOCO = 0x11
+    TYPE_TRAJ = 0x12
+    TYPE_LOCO2 = 0x13
+    TYPE_LH = 0x14
+    TYPE_MEMORY_TESTER = 0x15
 
     def __init__(self, id, type, size, mem_handler):
         """Initialize the element with default values"""
@@ -91,10 +95,18 @@ class MemoryElement(object):
             return 'LED driver'
         if t == MemoryElement.TYPE_LOCO:
             return 'Loco Positioning'
+        if t == MemoryElement.TYPE_TRAJ:
+            return 'Trajectory'
+        if t == MemoryElement.TYPE_LOCO2:
+            return 'Loco Positioning 2'
+        if t == MemoryElement.TYPE_LH:
+            return 'Lighthouse positioning'
+        if t == MemoryElement.TYPE_MEMORY_TESTER:
+            return 'Memory tester'
         return 'Unknown'
 
     def new_data(self, mem, addr, data):
-        logger.info('New data, but not OW mem')
+        logger.debug('New data, but not OW mem')
 
     def __str__(self):
         """Generate debug string for memory"""
@@ -139,7 +151,8 @@ class LEDDriverMemory(MemoryElement):
     def new_data(self, mem, addr, data):
         """Callback for when new memory data has been fetched"""
         if mem.id == self.id:
-            logger.info("Got new data from the LED driver, but we don't care.")
+            logger.debug(
+                "Got new data from the LED driver, but we don't care.")
 
     def write_data(self, write_finished_cb):
         """Write the saved LED-ring data to the Crazyflie"""
@@ -166,13 +179,13 @@ class LEDDriverMemory(MemoryElement):
         if not self._update_finished_cb:
             self._update_finished_cb = update_finished_cb
             self.valid = False
-            logger.info('Updating content of memory {}'.format(self.id))
+            logger.debug('Updating content of memory {}'.format(self.id))
             # Start reading the header
             self.mem_handler.read(self, 0, 16)
 
     def write_done(self, mem, addr):
         if self._write_finished_cb and mem.id == self.id:
-            logger.info('Write to LED driver done')
+            logger.debug('Write to LED driver done')
             self._write_finished_cb(self, addr)
             self._write_finished_cb = None
 
@@ -198,7 +211,7 @@ class I2CElement(MemoryElement):
                 done = False
                 # Check for header
                 if data[0:4] == EEPROM_TOKEN:
-                    logger.info('Got new data: {}'.format(data))
+                    logger.debug('Got new data: {}'.format(data))
                     [self.elements['version'],
                      self.elements['radio_channel'],
                      self.elements['radio_speed'],
@@ -222,7 +235,7 @@ class I2CElement(MemoryElement):
                 self.elements['radio_address'] = int(
                     radio_address_upper) << 32 | radio_address_lower
 
-                logger.info(self.elements)
+                logger.debug(self.elements)
                 data = self.datav0 + data
                 done = True
 
@@ -267,7 +280,7 @@ class I2CElement(MemoryElement):
         if not self._update_finished_cb:
             self._update_finished_cb = update_finished_cb
             self.valid = False
-            logger.info('Updating content of memory {}'.format(self.id))
+            logger.debug('Updating content of memory {}'.format(self.id))
             # Start reading the header
             self.mem_handler.read(self, 0, 16)
 
@@ -366,10 +379,9 @@ class OWElement(MemoryElement):
 
         # Now generate the elements part
         elem = bytearray()
-        logger.info(list(self.elements.keys()))
+        logger.debug(list(self.elements.keys()))
         for element in reversed(list(self.elements.keys())):
             elem_string = self.elements[element]
-            # logger.info(">>>> {}".format(elem_string))
             key_encoding = self._rev_element_mapping[element]
             elem += struct.pack('BB', key_encoding, len(elem_string))
             elem += bytearray(elem_string.encode('ISO-8859-1'))
@@ -399,13 +411,12 @@ class OWElement(MemoryElement):
         if not self._update_finished_cb:
             self._update_finished_cb = update_finished_cb
             self.valid = False
-            logger.info('Updating content of memory {}'.format(self.id))
+            logger.debug('Updating content of memory {}'.format(self.id))
             # Start reading the header
             self.mem_handler.read(self, 0, 11)
 
     def _parse_and_check_header(self, data):
         """Parse and check the CRC of the header part of the memory"""
-        # logger.info("Should parse header: {}".format(data))
         (start, self.pins, self.vid, self.pid, crc) = struct.unpack('<BIBBB',
                                                                     data)
         test_crc = crc32(data[:-1]) & 0x0ff
@@ -424,6 +435,7 @@ class OWElement(MemoryElement):
 
 
 class AnchorData:
+    """Holds data for one anchor"""
 
     def __init__(self, position=(0.0, 0.0, 0.0), is_valid=False):
         self.position = position
@@ -490,7 +502,7 @@ class LocoMemory(MemoryElement):
             self.anchor_data = []
             self.nr_of_anchors = 0
             self.valid = False
-            logger.info('Updating content of memory {}'.format(self.id))
+            logger.debug('Updating content of memory {}'.format(self.id))
 
             # Start reading the header
             self.mem_handler.read(self, LocoMemory.MEM_LOCO_INFO,
@@ -505,6 +517,388 @@ class LocoMemory(MemoryElement):
         self.mem_handler.read(self, addr, LocoMemory.MEM_LOCO_PAGE_LEN)
 
 
+class AnchorData2:
+    """Holds data for one anchor"""
+
+    def __init__(self, position=(0.0, 0.0, 0.0), is_valid=False):
+        self.position = position
+        self.is_valid = is_valid
+
+    def set_from_mem_data(self, data):
+        x, y, z, self.is_valid = struct.unpack('<fff?', data)
+        self.position = (x, y, z)
+
+
+class LocoMemory2(MemoryElement):
+    """Memory interface for accessing data from the Loco Positioning system
+       version 2"""
+
+    SIZE_OF_FLOAT = 4
+
+    # MAX_NR_OF_ANCHORS should be set to the number of anchors
+    # supported by the firmware. Preferably short enough to fit into one packet
+    MAX_NR_OF_ANCHORS = 16
+    ID_LIST_LEN = 1 + MAX_NR_OF_ANCHORS
+
+    ADR_ID_LIST = 0x0000
+    ADR_ACTIVE_ID_LIST = 0x1000
+    ADR_ANCHOR_BASE = 0x2000
+
+    ANCHOR_PAGE_SIZE = 0x0100
+    PAGE_LEN = (3 * SIZE_OF_FLOAT) + 1
+
+    def __init__(self, id, type, size, mem_handler):
+        super(LocoMemory2, self).__init__(id=id, type=type, size=size,
+                                          mem_handler=mem_handler)
+        self._update_ids_finished_cb = None
+        self._update_active_ids_finished_cb = None
+        self._update_data_finished_cb = None
+        self._currently_fetching_index = -1
+
+        self.anchor_ids = []
+        self.active_anchor_ids = []
+        self.anchor_data = {}
+        self.nr_of_anchors = 0
+        self.ids_valid = False
+        self.active_ids_valid = False
+        self.data_valid = False
+
+    def new_data(self, mem, addr, data):
+        """Callback for when new memory data has been fetched"""
+        if mem.id == self.id:
+            if addr == LocoMemory2.ADR_ID_LIST:
+                self._handle_id_list_data(data)
+            elif addr == LocoMemory2.ADR_ACTIVE_ID_LIST:
+                self._handle_active_id_list_data(data)
+            else:
+                id = int((addr - LocoMemory2.ADR_ANCHOR_BASE) /
+                         LocoMemory2.ANCHOR_PAGE_SIZE)
+                self._handle_anchor_data(id, data)
+
+    def update_id_list(self, update_ids_finished_cb):
+        """Request an update of the id list"""
+        if not self._update_ids_finished_cb:
+            self._update_ids_finished_cb = update_ids_finished_cb
+            self.anchor_ids = []
+            self.active_anchor_ids = []
+            self.anchor_data = {}
+
+            self.nr_of_anchors = 0
+            self.ids_valid = False
+            self.data_valid = False
+
+            logger.debug('Updating ids of memory {}'.format(self.id))
+
+            # Start reading the header
+            self.mem_handler.read(self, LocoMemory2.ADR_ID_LIST,
+                                  LocoMemory2.ID_LIST_LEN)
+
+    def update_active_id_list(self, update_active_ids_finished_cb):
+        """Request an update of the active id list"""
+        if not self._update_active_ids_finished_cb:
+            self._update_active_ids_finished_cb = update_active_ids_finished_cb
+            self.active_anchor_ids = []
+
+            self.active_ids_valid = False
+
+            logger.debug('Updating active ids of memory {}'.format(self.id))
+
+            # Start reading the header
+            self.mem_handler.read(self, LocoMemory2.ADR_ACTIVE_ID_LIST,
+                                  LocoMemory2.ID_LIST_LEN)
+
+    def update_data(self, update_data_finished_cb):
+        """Request an update of the anchor data"""
+        if not self._update_data_finished_cb and self.nr_of_anchors > 0:
+            self._update_data_finished_cb = update_data_finished_cb
+            self.anchor_data = {}
+
+            self.data_valid = False
+            self._nr_of_anchors_to_fetch = self.nr_of_anchors
+
+            logger.debug('Updating anchor data of memory {}'.format(self.id))
+
+            # Start reading the first anchor
+            self._currently_fetching_index = 0
+            self._request_page(self.anchor_ids[self._currently_fetching_index])
+
+    def disconnect(self):
+        self._update_ids_finished_cb = None
+        self._update_data_finished_cb = None
+
+    def _handle_id_list_data(self, data):
+        self.nr_of_anchors = data[0]
+        for i in range(self.nr_of_anchors):
+            self.anchor_ids.append(data[1 + i])
+        self.ids_valid = True
+
+        if self._update_ids_finished_cb:
+            self._update_ids_finished_cb(self)
+            self._update_ids_finished_cb = None
+
+    def _handle_active_id_list_data(self, data):
+        count = data[0]
+        for i in range(count):
+            self.active_anchor_ids.append(data[1 + i])
+        self.active_ids_valid = True
+
+        if self._update_active_ids_finished_cb:
+            self._update_active_ids_finished_cb(self)
+            self._update_active_ids_finished_cb = None
+
+    def _handle_anchor_data(self, id, data):
+        anchor = AnchorData2()
+        anchor.set_from_mem_data(data)
+        self.anchor_data[id] = anchor
+
+        self._currently_fetching_index += 1
+        if self._currently_fetching_index < self.nr_of_anchors:
+            self._request_page(self.anchor_ids[self._currently_fetching_index])
+        else:
+            self.data_valid = True
+            if self._update_data_finished_cb:
+                self._update_data_finished_cb(self)
+                self._update_data_finished_cb = None
+
+    def _request_page(self, page):
+        addr = LocoMemory2.ADR_ANCHOR_BASE + \
+            LocoMemory2.ANCHOR_PAGE_SIZE * page
+        self.mem_handler.read(self, addr, LocoMemory2.PAGE_LEN)
+
+
+class Poly4D:
+    class Poly:
+        def __init__(self, values=[0.0] * 8):
+            self.values = values
+
+    def __init__(self, duration, x=None, y=None, z=None, yaw=None):
+        self.duration = duration
+        self.x = x if x else self.Poly()
+        self.y = y if y else self.Poly()
+        self.z = z if z else self.Poly()
+        self.yaw = yaw if yaw else self.Poly()
+
+
+class TrajectoryMemory(MemoryElement):
+    """
+    Memory interface for trajectories used by the high level commander
+    """
+
+    def __init__(self, id, type, size, mem_handler):
+        """Initialize trajectory memory"""
+        super(TrajectoryMemory, self).__init__(id=id, type=type, size=size,
+                                               mem_handler=mem_handler)
+        self._write_finished_cb = None
+
+        # A list of Poly4D objects to write to the Crazyflie
+        self.poly4Ds = []
+
+    def write_data(self, write_finished_cb):
+        """Write trajectory data to the Crazyflie"""
+        self._write_finished_cb = write_finished_cb
+        data = bytearray()
+
+        for poly4D in self.poly4Ds:
+            data += struct.pack('<ffffffff', *poly4D.x.values)
+            data += struct.pack('<ffffffff', *poly4D.y.values)
+            data += struct.pack('<ffffffff', *poly4D.z.values)
+            data += struct.pack('<ffffffff', *poly4D.yaw.values)
+            data += struct.pack('<f', poly4D.duration)
+
+        self.mem_handler.write(self, 0x00, data, flush_queue=True)
+
+    def write_done(self, mem, addr):
+        if self._write_finished_cb and mem.id == self.id:
+            logger.debug('Write trajectory data done')
+            self._write_finished_cb(self, addr)
+            self._write_finished_cb = None
+
+    def disconnect(self):
+        self._write_finished_cb = None
+
+
+class LighthouseBsGeometry:
+    """Container for geometry data of one Lighthouse base station"""
+
+    SIZE_FLOAT = 4
+    SIZE_VECTOR = 3 * SIZE_FLOAT
+    SIZE_GEOMETRY = (1 + 3) * SIZE_VECTOR
+    SIZE_DATA = 2 * SIZE_GEOMETRY
+
+    def __init__(self):
+        self.origin = [0.0, 0.0, 0.0]
+        self.rotation_matrix = [
+            [0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0],
+        ]
+
+    def set_from_mem_data(self, data):
+        self.origin = self._read_vector(
+            data[0 * self.SIZE_VECTOR:1 * self.SIZE_VECTOR])
+        self.rotation_matrix = [
+            self._read_vector(data[1 * self.SIZE_VECTOR:2 * self.SIZE_VECTOR]),
+            self._read_vector(data[2 * self.SIZE_VECTOR:3 * self.SIZE_VECTOR]),
+            self._read_vector(data[3 * self.SIZE_VECTOR:4 * self.SIZE_VECTOR]),
+        ]
+
+    def add_mem_data(self, data):
+        self._add_vector(data, self.origin)
+        self._add_vector(data, self.rotation_matrix[0])
+        self._add_vector(data, self.rotation_matrix[1])
+        self._add_vector(data, self.rotation_matrix[2])
+
+    def _add_vector(self, data, vector):
+        data += struct.pack('<fff', vector[0], vector[1], vector[2])
+
+    def _read_vector(self, data):
+        x, y, z = struct.unpack('<fff', data)
+        return [x, y, z]
+
+    def dump(self):
+        print('origin:', self.origin)
+        print('rotation matrix: ', self.rotation_matrix)
+
+
+class LighthouseMemory(MemoryElement):
+    """
+    Memory interface for lighthouse configuration data
+    """
+
+    def __init__(self, id, type, size, mem_handler):
+        """Initialize Lighthouse memory"""
+        super(LighthouseMemory, self).__init__(id=id, type=type, size=size,
+                                               mem_handler=mem_handler)
+
+        self._update_finished_cb = None
+        self._write_finished_cb = None
+
+        # Geometry data for two base stations
+        self.geometry_data = [
+            LighthouseBsGeometry(),
+            LighthouseBsGeometry(),
+        ]
+
+    def new_data(self, mem, addr, data):
+        """Callback for when new memory data has been fetched"""
+        if mem.id == self.id:
+            if addr == 0:
+                self.geometry_data[0].set_from_mem_data(
+                    data[0:LighthouseBsGeometry.SIZE_GEOMETRY])
+                self.geometry_data[1].set_from_mem_data(
+                    data[LighthouseBsGeometry.SIZE_GEOMETRY:])
+
+                if self._update_finished_cb:
+                    self._update_finished_cb(self)
+                    self._update_finished_cb = None
+
+    def update(self, update_finished_cb):
+        """Request an update of the memory content"""
+        if not self._update_finished_cb:
+            self._update_finished_cb = update_finished_cb
+            logger.debug('Updating content of memory {}'.format(self.id))
+            self.mem_handler.read(self, 0, LighthouseBsGeometry.SIZE_DATA)
+
+    def write_data(self, write_finished_cb):
+        """Write geometry data to the Crazyflie"""
+        self._write_finished_cb = write_finished_cb
+        data = bytearray()
+
+        for bs in self.geometry_data:
+            bs.add_mem_data(data)
+
+        self.mem_handler.write(self, 0x00, data, flush_queue=True)
+
+    def write_done(self, mem, addr):
+        if self._write_finished_cb and mem.id == self.id:
+            logger.debug('Write of geometry data done')
+            self._write_finished_cb(self, addr)
+            self._write_finished_cb = None
+
+    def disconnect(self):
+        self._update_finished_cb = None
+        self._write_finished_cb = None
+
+    def dump(self):
+        for data in self.geometry_data:
+            data.dump()
+
+
+class MemoryTester(MemoryElement):
+    """
+    Memory interface for testing the memory sub system, end to end.
+
+    Usage
+    1. To verify reading:
+      * Call read_data()
+      * Wait for the callback to be called
+      * Verify that readValidationSucess is True
+
+    2. To verify writing:
+      * Set the parameter 'memTst.resetW' in the CF
+      * call write_data()
+      * Wait for the callback
+      * Read the log var 'memTst.errCntW' from the CF and validate that it
+        is 0
+    """
+
+    def __init__(self, id, type, size, mem_handler):
+        """Initialize Memory tester"""
+        super(MemoryTester, self).__init__(id=id, type=type, size=size,
+                                           mem_handler=mem_handler)
+
+        self._update_finished_cb = None
+        self._write_finished_cb = None
+
+        self.readValidationSucess = True
+
+    def new_data(self, mem, start_address, data):
+        """Callback for when new memory data has been fetched"""
+        if mem.id == self.id:
+            for i in range(len(data)):
+                actualValue = struct.unpack('<B', data[i:i + 1])[0]
+                expectedValue = (start_address + i) & 0xff
+
+                if (actualValue != expectedValue):
+                    address = start_address + i
+                    self.readValidationSucess = False
+                    logger.error(
+                        'Error in data - expected: {}, actual: {}, address:{}',
+                        expectedValue, actualValue, address)
+
+                if self._update_finished_cb:
+                    self._update_finished_cb(self)
+                    self._update_finished_cb = None
+
+    def read_data(self, start_address, size, update_finished_cb):
+        """Request an update of the memory content"""
+        if not self._update_finished_cb:
+            self._update_finished_cb = update_finished_cb
+            logger.debug('Reading memory {}'.format(self.id))
+            self.mem_handler.read(self, start_address, size)
+
+    def write_data(self, start_address, size, write_finished_cb):
+        """Write data to the Crazyflie"""
+        self._write_finished_cb = write_finished_cb
+        data = bytearray()
+
+        for i in range(size):
+            value = (start_address + i) & 0xff
+            data += struct.pack('<B', value)
+
+        self.mem_handler.write(self, start_address, data, flush_queue=True)
+
+    def write_done(self, mem, addr):
+        if self._write_finished_cb and mem.id == self.id:
+            logger.debug('Write of data finished')
+            self._write_finished_cb(self, addr)
+            self._write_finished_cb = None
+
+    def disconnect(self):
+        self._update_finished_cb = None
+        self._write_finished_cb = None
+
+
 class _ReadRequest:
     """
     Class used to handle memory reads that will split up the read in multiple
@@ -527,7 +921,7 @@ class _ReadRequest:
         self._request_new_chunk()
 
     def resend(self):
-        logger.info('Sending write again...')
+        logger.debug('Sending write again...')
         self._request_new_chunk()
 
     def _request_new_chunk(self):
@@ -539,7 +933,7 @@ class _ReadRequest:
         if new_len > _ReadRequest.MAX_DATA_LENGTH:
             new_len = _ReadRequest.MAX_DATA_LENGTH
 
-        logger.info('Requesting new chunk of {}bytes at 0x{:X}'.format(
+        logger.debug('Requesting new chunk of {}bytes at 0x{:X}'.format(
             new_len, self._current_addr))
 
         # Request the data for the next address
@@ -597,7 +991,7 @@ class _WriteRequest:
         self._write_new_chunk()
 
     def resend(self):
-        logger.info('Sending write again...')
+        logger.debug('Sending write again...')
         self.cf.send_packet(
             self._sent_packet, expected_reply=self._sent_reply, timeout=1)
 
@@ -610,7 +1004,7 @@ class _WriteRequest:
         if new_len > _WriteRequest.MAX_DATA_LENGTH:
             new_len = _WriteRequest.MAX_DATA_LENGTH
 
-        logger.info('Writing new chunk of {}bytes at 0x{:X}'.format(
+        logger.debug('Writing new chunk of {}bytes at 0x{:X}'.format(
             new_len, self._current_addr))
 
         data = self._data[:new_len]
@@ -641,7 +1035,7 @@ class _WriteRequest:
             self._write_new_chunk()
             return False
         else:
-            logger.info('This write request is done')
+            logger.debug('This write request is done')
             return True
 
 
@@ -661,7 +1055,6 @@ class Memory():
 
     def __init__(self, crazyflie=None):
         """Instantiate class and connect callbacks"""
-        self.mems = []
         # Called when new memories have been added
         self.mem_added_cb = Caller()
         # Called when new data has been read
@@ -672,18 +1065,20 @@ class Memory():
         self.cf = crazyflie
         self.cf.add_port_callback(CRTPPort.MEM, self._new_packet_cb)
         self.cf.disconnected.add_callback(self._disconnected)
+        self._write_requests_lock = Lock()
 
+        self._clear_state()
+
+    def _clear_state(self):
+        self.mems = []
         self._refresh_callback = None
         self._fetch_id = 0
         self.nbr_of_mems = 0
         self._ow_mem_fetch_index = 0
         self._elem_data = ()
         self._read_requests = {}
-        self._read_requests_lock = Lock()
         self._write_requests = {}
-        self._write_requests_lock = Lock()
         self._ow_mems_left_to_update = []
-
         self._getting_count = False
 
     def _mem_update_done(self, mem):
@@ -694,7 +1089,7 @@ class Memory():
         if mem.id in self._ow_mems_left_to_update:
             self._ow_mems_left_to_update.remove(mem.id)
 
-        logger.info(mem)
+        logger.debug(mem)
 
         if len(self._ow_mems_left_to_update) == 0:
             if self._refresh_callback:
@@ -778,7 +1173,7 @@ class Memory():
         self.nbr_of_mems = 0
         self._getting_count = False
 
-        logger.info('Requesting number of memories')
+        logger.debug('Requesting number of memories')
         pk = CRTPPacket()
         pk.set_header(CRTPPort.MEM, CHAN_INFO)
         pk.data = (CMD_INFO_NBR,)
@@ -786,12 +1181,7 @@ class Memory():
 
     def _disconnected(self, uri):
         """The link to the Crazyflie has been broken. Reset state"""
-        for m in self.mems:
-            try:
-                m.disconnect()
-            except Exception as e:
-                logger.info(
-                    'Error when resetting after disconnect: {}'.format(e))
+        self._clear_state()
 
     def _new_packet_cb(self, packet):
         """Callback for newly arrived packets for the memory port"""
@@ -809,7 +1199,7 @@ class Memory():
                 if self.nbr_of_mems > 0:
                     if not self._getting_count:
                         self._getting_count = True
-                        logger.info('Requesting first id')
+                        logger.debug('Requesting first id')
                         pk = CRTPPacket()
                         pk.set_header(CRTPPort.MEM, CHAN_INFO)
                         pk.data = (CMD_INFO_DETAILS, 0)
@@ -864,26 +1254,47 @@ class Memory():
                     elif mem_type == MemoryElement.TYPE_DRIVER_LED:
                         mem = LEDDriverMemory(id=mem_id, type=mem_type,
                                               size=mem_size, mem_handler=self)
-                        logger.info(mem)
+                        logger.debug(mem)
                         self.mem_read_cb.add_callback(mem.new_data)
                         self.mem_write_cb.add_callback(mem.write_done)
                     elif mem_type == MemoryElement.TYPE_LOCO:
                         mem = LocoMemory(id=mem_id, type=mem_type,
                                          size=mem_size, mem_handler=self)
-                        logger.info(mem)
+                        logger.debug(mem)
+                        self.mem_read_cb.add_callback(mem.new_data)
+                    elif mem_type == MemoryElement.TYPE_TRAJ:
+                        mem = TrajectoryMemory(id=mem_id, type=mem_type,
+                                               size=mem_size, mem_handler=self)
+                        logger.debug(mem)
+                        self.mem_write_cb.add_callback(mem.write_done)
+                    elif mem_type == MemoryElement.TYPE_LOCO2:
+                        mem = LocoMemory2(id=mem_id, type=mem_type,
+                                          size=mem_size, mem_handler=self)
+                        logger.debug(mem)
                         self.mem_read_cb.add_callback(mem.new_data)
+                    elif mem_type == MemoryElement.TYPE_LH:
+                        mem = LighthouseMemory(id=mem_id, type=mem_type,
+                                               size=mem_size, mem_handler=self)
+                        logger.debug(mem)
+                        self.mem_read_cb.add_callback(mem.new_data)
+                        self.mem_write_cb.add_callback(mem.write_done)
+                    elif mem_type == MemoryElement.TYPE_MEMORY_TESTER:
+                        mem = MemoryTester(id=mem_id, type=mem_type,
+                                           size=mem_size, mem_handler=self)
+                        logger.debug(mem)
+                        self.mem_read_cb.add_callback(mem.new_data)
+                        self.mem_write_cb.add_callback(mem.write_done)
                     else:
                         mem = MemoryElement(id=mem_id, type=mem_type,
                                             size=mem_size, mem_handler=self)
-                        logger.info(mem)
+                        logger.debug(mem)
                     self.mems.append(mem)
                     self.mem_added_cb.call(mem)
-                    # logger.info(mem)
 
                     self._fetch_id = mem_id + 1
 
                 if self.nbr_of_mems - 1 >= self._fetch_id:
-                    logger.info(
+                    logger.debug(
                         'Requesting information about memory {}'.format(
                             self._fetch_id))
                     pk = CRTPPacket()
@@ -892,7 +1303,7 @@ class Memory():
                     self.cf.send_packet(pk, expected_reply=(
                         CMD_INFO_DETAILS, self._fetch_id))
                 else:
-                    logger.info(
+                    logger.debug(
                         'Done getting all the memories, start reading the OWs')
                     ows = self.get_mems(MemoryElement.TYPE_1W)
                     # If there are any OW mems start reading them, otherwise
@@ -907,7 +1318,7 @@ class Memory():
         if chan == CHAN_WRITE:
             id = cmd
             (addr, status) = struct.unpack('<IB', payload[0:5])
-            logger.info(
+            logger.debug(
                 'WRITE: Mem={}, addr=0x{:X}, status=0x{}'.format(
                     id, addr, status))
             # Find the read request
@@ -925,7 +1336,8 @@ class Memory():
                         if len(self._write_requests[id]) > 0:
                             self._write_requests[id][0].start()
                 else:
-                    logger.info('Status {}: write resending...'.format(status))
+                    logger.debug(
+                        'Status {}: write resending...'.format(status))
                     wreq.resend()
                 self._write_requests_lock.release()
 
@@ -933,11 +1345,11 @@ class Memory():
             id = cmd
             (addr, status) = struct.unpack('<IB', payload[0:5])
             data = struct.unpack('B' * len(payload[5:]), payload[5:])
-            logger.info('READ: Mem={}, addr=0x{:X}, status=0x{}, '
-                        'data={}'.format(id, addr, status, data))
+            logger.debug('READ: Mem={}, addr=0x{:X}, status=0x{}, '
+                         'data={}'.format(id, addr, status, data))
             # Find the read request
             if id in self._read_requests:
-                logger.info(
+                logger.debug(
                     'READING: We are still interested in request for '
                     'mem {}'.format(id))
                 rreq = self._read_requests[id]
@@ -946,5 +1358,5 @@ class Memory():
                         self._read_requests.pop(id, None)
                         self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data)
                 else:
-                    logger.info('Status {}: resending...'.format(status))
+                    logger.debug('Status {}: resending...'.format(status))
                     rreq.resend()

pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/param.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/param.py

@@ -63,13 +63,9 @@ TOC_CHANNEL = 0
 READ_CHANNEL = 1
 WRITE_CHANNEL = 2
 
-# TOC access command
-TOC_RESET = 0
-TOC_GETNEXT = 1
-TOC_GETCRC32 = 2
+# One element entry in the TOC
 
 
-# One element entry in the TOC
 class ParamTocElement:
     """An element in the Log TOC."""
 
@@ -88,10 +84,11 @@ class ParamTocElement:
              0x06: ('float', '<f'),
              0x07: ('double', '<d')}
 
-    def __init__(self, data=None):
+    def __init__(self, ident=0, data=None):
         """TocElement creator. Data is the binary payload of the element."""
+        self.ident = ident
         if (data):
-            strs = struct.unpack('s' * len(data[2:]), data[2:])
+            strs = struct.unpack('s' * len(data[1:]), data[1:])
             if sys.version_info < (3,):
                 strs = ('{}' * len(strs)).format(*strs).split('\0')
             else:
@@ -102,12 +99,7 @@ class ParamTocElement:
             self.group = strs[0]
             self.name = strs[1]
 
-            if type(data[0]) == str:
-                self.ident = ord(data[0])
-            else:
-                self.ident = data[0]
-
-            metadata = data[1]
+            metadata = data[0]
             if type(metadata) == str:
                 metadata = ord(metadata)
 
@@ -133,12 +125,14 @@ class Param():
         self.toc = Toc()
 
         self.cf = crazyflie
+        self._useV2 = False
         self.param_update_callbacks = {}
         self.group_update_callbacks = {}
         self.all_update_callback = Caller()
         self.param_updater = None
 
-        self.param_updater = _ParamUpdater(self.cf, self._param_updated)
+        self.param_updater = _ParamUpdater(
+            self.cf, self._useV2, self._param_updated)
         self.param_updater.start()
 
         self.cf.disconnected.add_callback(self._disconnected)
@@ -169,10 +163,16 @@ class Param():
 
     def _param_updated(self, pk):
         """Callback with data for an updated parameter"""
-        var_id = pk.data[0]
+        if self._useV2:
+            var_id = struct.unpack('<H', pk.data[:2])[0]
+        else:
+            var_id = pk.data[0]
         element = self.toc.get_element_by_id(var_id)
         if element:
-            s = struct.unpack(element.pytype, pk.data[1:])[0]
+            if self._useV2:
+                s = struct.unpack(element.pytype, pk.data[2:])[0]
+            else:
+                s = struct.unpack(element.pytype, pk.data[1:])[0]
             s = s.__str__()
             complete_name = '%s.%s' % (element.group, element.name)
 
@@ -233,6 +233,7 @@ class Param():
         """
         Initiate a refresh of the parameter TOC.
         """
+        self._useV2 = self.cf.platform.get_protocol_version() >= 4
         toc_fetcher = TocFetcher(self.cf, ParamTocElement,
                                  CRTPPort.PARAM, self.toc,
                                  refresh_done_callback, toc_cache)
@@ -271,8 +272,17 @@ class Param():
             varid = element.ident
             pk = CRTPPacket()
             pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL)
-            pk.data = struct.pack('<B', varid)
-            pk.data += struct.pack(element.pytype, eval(value))
+            if self._useV2:
+                pk.data = struct.pack('<H', varid)
+            else:
+                pk.data = struct.pack('<B', varid)
+
+            try:
+                value_nr = eval(value)
+            except TypeError:
+                value_nr = value
+
+            pk.data += struct.pack(element.pytype, value_nr)
             self.param_updater.request_param_setvalue(pk)
 
 
@@ -280,12 +290,13 @@ class _ParamUpdater(Thread):
     """This thread will update params through a queue to make sure that we
     get back values"""
 
-    def __init__(self, cf, updated_callback):
+    def __init__(self, cf, useV2, updated_callback):
         """Initialize the thread"""
         Thread.__init__(self)
         self.setDaemon(True)
         self.wait_lock = Lock()
         self.cf = cf
+        self._useV2 = useV2
         self.updated_callback = updated_callback
         self.request_queue = Queue()
         self.cf.add_port_callback(CRTPPort.PARAM, self._new_packet_cb)
@@ -300,7 +311,7 @@ class _ParamUpdater(Thread):
         # we didn't get back due to a disconnect for example.
         try:
             self.wait_lock.release()
-        except:
+        except Exception:
             pass
 
     def request_param_setvalue(self, pk):
@@ -311,21 +322,30 @@ class _ParamUpdater(Thread):
     def _new_packet_cb(self, pk):
         """Callback for newly arrived packets"""
         if pk.channel == READ_CHANNEL or pk.channel == WRITE_CHANNEL:
-            var_id = pk.data[0]
+            if self._useV2:
+                var_id = struct.unpack('<H', pk.data[:2])[0]
+                if pk.channel == READ_CHANNEL:
+                    pk.data = pk.data[:2] + pk.data[3:]
+            else:
+                var_id = pk.data[0]
             if (pk.channel != TOC_CHANNEL and self._req_param == var_id and
                     pk is not None):
                 self.updated_callback(pk)
                 self._req_param = -1
                 try:
                     self.wait_lock.release()
-                except:
+                except Exception:
                     pass
 
     def request_param_update(self, var_id):
         """Place a param update request on the queue"""
+        self._useV2 = self.cf.platform.get_protocol_version() >= 4
         pk = CRTPPacket()
         pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
-        pk.data = struct.pack('<B', var_id)
+        if self._useV2:
+            pk.data = struct.pack('<H', var_id)
+        else:
+            pk.data = struct.pack('<B', var_id)
         logger.debug('Requesting request to update param [%d]', var_id)
         self.request_queue.put(pk)
 
@@ -334,7 +354,13 @@ class _ParamUpdater(Thread):
             pk = self.request_queue.get()  # Wait for request update
             self.wait_lock.acquire()
             if self.cf.link:
-                self._req_param = pk.data[0]
-                self.cf.send_packet(pk, expected_reply=(tuple(pk.data[0:2])))
+                if self._useV2:
+                    self._req_param = struct.unpack('<H', pk.data[:2])[0]
+                    self.cf.send_packet(
+                        pk, expected_reply=(tuple(pk.data[:2])))
+                else:
+                    self._req_param = pk.data[0]
+                    self.cf.send_packet(
+                        pk, expected_reply=(tuple(pk.data[:1])))
             else:
                 self.wait_lock.release()

pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/platformservice.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/platformservice.py

@@ -27,12 +27,26 @@
 """
 Used for sending control setpoints to the Crazyflie
 """
+import logging
+
 from cflib.crtp.crtpstack import CRTPPacket
 from cflib.crtp.crtpstack import CRTPPort
 
 __author__ = 'Bitcraze AB'
 __all__ = ['PlatformService']
 
+logger = logging.getLogger(__name__)
+
+PLATFORM_COMMAND = 0
+VERSION_COMMAND = 1
+
+PLATFORM_SET_CONT_WAVE = 0
+
+VERSION_GET_PROTOCOL = 0
+VERSION_GET_FIRMWARE = 1
+
+LINKSERVICE_SOURCE = 1
+
 
 class PlatformService():
     """
@@ -45,12 +59,72 @@ class PlatformService():
         """
         self._cf = crazyflie
 
+        self._cf.add_port_callback(CRTPPort.PLATFORM,
+                                   self._platform_callback)
+        self._cf.add_port_callback(CRTPPort.LINKCTRL,
+                                   self._crt_service_callback)
+
+        # Request protocol version.
+        # The semaphore makes sure that other module will wait for the version
+        # to be received before using it.
+        self._has_protocol_version = False
+        self._protocolVersion = -1
+        self._callback = None
+
+    def fetch_platform_informations(self, callback):
+        """
+        Fetch platform info from the firmware
+        Should be called at the earliest in the connection sequence
+        """
+
+        self._protocolVersion = -1
+        self._callback = callback
+
+        self._request_protocol_version()
+
     def set_continous_wave(self, enabled):
         """
         Enable/disable the client side X-mode. When enabled this recalculates
         the setpoints before sending them to the Crazyflie.
         """
         pk = CRTPPacket()
-        pk.set_header(CRTPPort.PLATFORM, 0)
+        pk.set_header(CRTPPort.PLATFORM, PLATFORM_COMMAND)
         pk.data = (0, enabled)
         self._cf.send_packet(pk)
+
+    def get_protocol_version(self):
+        """
+        Return version of the CRTP protocol
+        """
+        return self._protocolVersion
+
+    def _request_protocol_version(self):
+        # Sending a sink request to detect if the connected Crazyflie
+        # supports protocol versioning
+        pk = CRTPPacket()
+        pk.set_header(CRTPPort.LINKCTRL, LINKSERVICE_SOURCE)
+        pk.data = (0,)
+        self._cf.send_packet(pk)
+
+    def _crt_service_callback(self, pk):
+        if pk.channel == LINKSERVICE_SOURCE:
+            # If the sink contains a magic string, get the protocol version,
+            # otherwise -1
+            if pk.data[:18].decode('utf8') == 'Bitcraze Crazyflie':
+                pk = CRTPPacket()
+                pk.set_header(CRTPPort.PLATFORM, VERSION_COMMAND)
+                pk.data = (VERSION_GET_PROTOCOL, )
+                self._cf.send_packet(pk)
+            else:
+                self._protocolVersion = -1
+                logger.info('Procotol version: {}'.format(
+                    self.get_protocol_version()))
+                self._callback()
+
+    def _platform_callback(self, pk):
+        if pk.channel == VERSION_COMMAND and \
+                pk.data[0] == VERSION_GET_PROTOCOL:
+            self._protocolVersion = pk.data[1]
+            logger.info('Procotol version: {}'.format(
+                self.get_protocol_version()))
+            self._callback()

pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/swarm.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/swarm.py

@@ -23,15 +23,34 @@
 #  MA  02110-1301, USA.
 from threading import Thread
 
+from cflib.crazyflie import Crazyflie
 from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
 
 
 class _Factory:
+    """
+    Default Crazyflie factory class.
+    """
 
     def construct(self, uri):
         return SyncCrazyflie(uri)
 
 
+class CachedCfFactory:
+    """
+    Factory class that creates Crazyflie instances with TOC caching
+    to reduce connection time.
+    """
+
+    def __init__(self, ro_cache=None, rw_cache=None):
+        self.ro_cache = ro_cache
+        self.rw_cache = rw_cache
+
+    def construct(self, uri):
+        cf = Crazyflie(ro_cache=self.ro_cache, rw_cache=self.rw_cache)
+        return SyncCrazyflie(uri, cf=cf)
+
+
 class Swarm:
     """
     Runs a swarm of Crazyflies. It implements a functional-ish style of

pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/syncCrazyflie.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/syncCrazyflie.py

@@ -50,18 +50,17 @@ class SyncCrazyflie:
         self._is_link_open = False
         self._error_message = None
 
-        self.cf.connected.add_callback(self._connected)
-        self.cf.connection_failed.add_callback(self._connection_failed)
-        self.cf.disconnected.add_callback(self._disconnected)
-
     def open_link(self):
         if (self.is_link_open()):
             raise Exception('Link already open')
 
+        self._add_callbacks()
+
         print('Connecting to %s' % self._link_uri)
         self.cf.open_link(self._link_uri)
         self._connect_event.wait()
         if not self._is_link_open:
+            self._remove_callbacks()
             raise Exception(self._error_message)
 
     def __enter__(self):
@@ -70,6 +69,7 @@ class SyncCrazyflie:
 
     def close_link(self):
         self.cf.close_link()
+        self._remove_callbacks()
         self._is_link_open = False
 
     def __exit__(self, exc_type, exc_val, exc_tb):
@@ -94,4 +94,21 @@ class SyncCrazyflie:
         self._connect_event.set()
 
     def _disconnected(self, link_uri):
+        self._remove_callbacks()
         self._is_link_open = False
+
+    def _add_callbacks(self):
+        self.cf.connected.add_callback(self._connected)
+        self.cf.connection_failed.add_callback(self._connection_failed)
+        self.cf.disconnected.add_callback(self._disconnected)
+
+    def _remove_callbacks(self):
+        def remove_callback(container, callback):
+            try:
+                container.remove_callback(callback)
+            except ValueError:
+                pass
+
+        remove_callback(self.cf.connected, self._connected)
+        remove_callback(self.cf.connection_failed, self._connection_failed)
+        remove_callback(self.cf.disconnected, self._disconnected)

pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/syncLogger.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/syncLogger.py

@@ -6,7 +6,7 @@
 #  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
 #   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
 #
-#  Copyright (C) 2016 Bitcraze AB
+#  Copyright (C) 2016-2020 Bitcraze AB
 #
 #  Crazyflie Nano Quadcopter Client
 #
@@ -27,7 +27,7 @@
 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.
+If no value is available it blocks until log data is received again.
 """
 import sys
 
@@ -46,15 +46,18 @@ class SyncLogger:
         """
         Construct an instance of a SyncLogger
 
-        Takes an Crazyflie or SyncCrazyflie instance and a log configuration
+        Takes an Crazyflie or SyncCrazyflie instance and one log configuration
+        or an array of log configurations
         """
         if isinstance(crazyflie, SyncCrazyflie):
             self._cf = crazyflie.cf
         else:
             self._cf = crazyflie
 
-        self._log_config = log_config
-        self._cf.log.add_config(self._log_config)
+        if isinstance(log_config, list):
+            self._log_config = log_config
+        else:
+            self._log_config = [log_config]
 
         self._queue = Queue()
 
@@ -65,21 +68,23 @@ class SyncLogger:
             raise Exception('Already connected')
 
         self._cf.disconnected.add_callback(self._disconnected)
-        self._log_config.data_received_cb.add_callback(self._log_callback)
-        self._log_config.start()
+        for config in self._log_config:
+            self._cf.log.add_config(config)
+            config.data_received_cb.add_callback(self._log_callback)
+            config.start()
 
         self._is_connected = True
 
     def disconnect(self):
         if self._is_connected:
-            self._log_config.stop()
-            self._log_config.delete()
+            for config in self._log_config:
+                config.stop()
+                config.delete()
 
-            self._log_config.data_received_cb.remove_callback(
-                self._log_callback)
-            self._cf.disconnected.remove_callback(self._disconnected)
+                config.data_received_cb.remove_callback(
+                    self._log_callback)
 
-            self._queue.empty()
+            self._cf.disconnected.remove_callback(self._disconnected)
 
             self._is_connected = False
 
@@ -89,6 +94,9 @@ class SyncLogger:
     def __iter__(self):
         return self
 
+    def next(self):
+        return self.__next__()
+
     def __next__(self):
         if not self._is_connected:
             raise StopIteration
@@ -96,6 +104,7 @@ class SyncLogger:
         data = self._queue.get()
 
         if data == self.DISCONNECT_EVENT:
+            self._queue.empty()
             raise StopIteration
 
         return data
@@ -106,10 +115,11 @@ class SyncLogger:
 
     def __exit__(self, exc_type, exc_val, exc_tb):
         self.disconnect()
+        self._queue.empty()
 
     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._queue.put(self.DISCONNECT_EVENT)

pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/toc.py → dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/toc.py

@@ -41,8 +41,10 @@ logger = logging.getLogger(__name__)
 TOC_CHANNEL = 0
 
 # Commands used when accessing the Table of Contents
-CMD_TOC_ELEMENT = 0
-CMD_TOC_INFO = 1
+CMD_TOC_ELEMENT = 0  # original version: up to 255 entries
+CMD_TOC_INFO = 1    # original version: up to 255 entries
+CMD_TOC_ITEM_V2 = 2  # version 2: up to 16k entries
+CMD_TOC_INFO_V2 = 3  # version 2: up to 16k entries
 
 # Possible states when receiving TOC
 IDLE = 'IDLE'
@@ -121,9 +123,14 @@ class TocFetcher:
         self._toc_cache = toc_cache
         self.finished_callback = finished_callback
         self.element_class = element_class
+        self._useV2 = False
 
     def start(self):
         """Initiate fetching of the TOC."""
+        self._useV2 = self.cf.platform.get_protocol_version() >= 4
+
+        logger.debug('[%d]: Using V2 protocol: %d', self.port, self._useV2)
+
         logger.debug('[%d]: Start fetching...', self.port)
         # Register callback in this class for the port
         self.cf.add_port_callback(self.port, self._new_packet_cb)
@@ -132,8 +139,12 @@ class TocFetcher:
         self.state = GET_TOC_INFO
         pk = CRTPPacket()
         pk.set_header(self.port, TOC_CHANNEL)
-        pk.data = (CMD_TOC_INFO,)
-        self.cf.send_packet(pk, expected_reply=(CMD_TOC_INFO,))
+        if self._useV2:
+            pk.data = (CMD_TOC_INFO_V2,)
+            self.cf.send_packet(pk, expected_reply=(CMD_TOC_INFO_V2,))
+        else:
+            pk.data = (CMD_TOC_INFO,)
+            self.cf.send_packet(pk, expected_reply=(CMD_TOC_INFO,))
 
     def _toc_fetch_finished(self):
         """Callback for when the TOC fetching is finished"""
@@ -149,7 +160,12 @@ class TocFetcher:
         payload = packet.data[1:]
 
         if (self.state == GET_TOC_INFO):
-            [self.nbr_of_items, self._crc] = struct.unpack('<BI', payload[:5])
+            if self._useV2:
+                [self.nbr_of_items, self._crc] = struct.unpack(
+                    '<HI', payload[:6])
+            else:
+                [self.nbr_of_items, self._crc] = struct.unpack(
+                    '<BI', payload[:5])
             logger.debug('[%d]: Got TOC CRC, %d items and crc=0x%08X',
                          self.port, self.nbr_of_items, self._crc)
 
@@ -166,11 +182,18 @@ class TocFetcher:
         elif (self.state == GET_TOC_ELEMENT):
             # Always add new element, but only request new if it's not the
             # last one.
-            if self.requested_index != payload[0]:
+            if self._useV2:
+                ident = struct.unpack('<H', payload[:2])[0]
+            else:
+                ident = payload[0]
+
+            if ident != self.requested_index:
                 return
-            self.toc.add_element(self.element_class(payload))
-            logger.debug('Added element [%s]',
-                         self.element_class(payload).ident)
+            if self._useV2:
+                self.toc.add_element(self.element_class(ident, payload[2:]))
+            else:
+                self.toc.add_element(self.element_class(ident, payload[1:]))
+            logger.debug('Added element [%s]', ident)
             if (self.requested_index < (self.nbr_of_items - 1)):
                 logger.debug('[%d]: More variables, requesting index %d',
                              self.port, self.requested_index + 1)
@@ -184,6 +207,12 @@ class TocFetcher:
         """Request information about a specific item in the TOC"""
         logger.debug('Requesting index %d on port %d', index, self.port)
         pk = CRTPPacket()
-        pk.set_header(self.port, TOC_CHANNEL)
-        pk.data = (CMD_TOC_ELEMENT, index)
-        self.cf.send_packet(pk, expected_reply=(CMD_TOC_ELEMENT, index))
+        if self._useV2:
+            pk.set_header(self.port, TOC_CHANNEL)
+            pk.data = (CMD_TOC_ITEM_V2, index & 0x0ff, (index >> 8) & 0x0ff)
+            self.cf.send_packet(pk, expected_reply=(
+                CMD_TOC_ITEM_V2, index & 0x0ff, (index >> 8) & 0x0ff))
+        else:
+            pk.set_header(self.port, TOC_CHANNEL)
+            pk.data = (CMD_TOC_ELEMENT, index)
+            self.cf.send_packet(pk, expected_reply=(CMD_TOC_ELEMENT, index))

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

@@ -29,6 +29,7 @@ import logging
 
 from .debugdriver import DebugDriver
 from .exceptions import WrongUriType
+from .prrtdriver import PrrtDriver
 from .radiodriver import RadioDriver
 from .serialdriver import SerialDriver
 from .udpdriver import UdpDriver
@@ -40,7 +41,8 @@ __all__ = []
 logger = logging.getLogger(__name__)
 
 
-DRIVERS = [RadioDriver, SerialDriver, UdpDriver, DebugDriver, UsbDriver]
+DRIVERS = [RadioDriver, SerialDriver, UdpDriver,
+           DebugDriver, UsbDriver, PrrtDriver]
 CLASSES = []