Merge pull request #394 from bitcraze/tobba/multirange_paa3905_memory

Added multiranger and paa3905 memory types.

cflib/crazyflie/mem/__init__.py

@@ -43,7 +43,9 @@
 from .loco_memory_2 import LocoMemory2
 from .memory_element import MemoryElement
 from .memory_tester import MemoryTester
+from .multiranger_memory import MultirangerMemory
 from .ow_element import OWElement
+from .paa3905_memory import PAA3905Memory
 from .trajectory_memory import CompressedSegment
 from .trajectory_memory import CompressedStart
 from .trajectory_memory import Poly4D
@@ -521,6 +523,16 @@ def _handle_cmd_info_details(self, payload):
                 self.mem_read_failed_cb.add_callback(mem._new_data_failed)
                 self.mem_write_cb.add_callback(mem._write_done)
                 self.mem_write_failed_cb.add_callback(mem._write_failed)
+            elif mem_type == MemoryElement.TYPE_DECK_MULTIRANGER:
+                mem = MultirangerMemory(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_read_failed_cb.add_callback(mem.read_failed)
+            elif mem_type == MemoryElement.TYPE_DECK_PAA3905:
+                mem = PAA3905Memory(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_read_failed_cb.add_callback(mem.read_failed)
             else:
                 mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self)
                 logger.debug(mem)

cflib/crazyflie/mem/memory_element.py

@@ -38,6 +38,8 @@ class MemoryElement(object):
     TYPE_DRIVER_LEDTIMING = 0x17
     TYPE_APP = 0x18
     TYPE_DECK_MEMORY = 0x19
+    TYPE_DECK_MULTIRANGER = 0x1A
+    TYPE_DECK_PAA3905 = 0x1B
 
     def __init__(self, id, type, size, mem_handler):
         """Initialize the element with default values"""

cflib/crazyflie/mem/multiranger_memory.py

@@ -0,0 +1,69 @@
+# -*- coding: utf-8 -*-
+#
+# ,---------,       ____  _ __
+# |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+# | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+# | / ,--'  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+# Copyright (C) 2019 - 2020 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, in version 3.
+#
+# 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, see <http://www.gnu.org/licenses/>.
+import logging
+import struct
+
+from .memory_element import MemoryElement
+from cflib.utils.callbacks import Syncer
+
+logger = logging.getLogger(__name__)
+
+
+class MultirangerMemory(MemoryElement):
+    """Memory interface for reading the multiranger values"""
+
+    def __init__(self, id, type, size, mem_handler):
+        super(MultirangerMemory, self).__init__(id=id, type=type, size=size,
+                                                mem_handler=mem_handler)
+        self._read_finished_cb = None
+
+    def new_data(self, mem, addr, data):
+        """Callback for when new memory data has been fetched"""
+        if mem.id == self.id and self._read_finished_cb:
+            unpacked_data = struct.unpack('<'+'H'*int(len(data) / 2), data)
+            zone_matrix = []
+            for i in range(8):
+                zone_matrix.append(unpacked_data[i*8:i*8+8])
+
+            self._read_finished_cb(addr, zone_matrix)
+
+    def read_data(self, read_finished_cb):
+        """Write the saved LED-ring data to the Crazyflie"""
+        self._read_finished_cb = read_finished_cb
+        self.mem_handler.read(self, 0, 128)
+
+    def read_data_sync(self):
+        """Write the saved LED-ring data to the Crazyflie"""
+        syncer = Syncer()
+        self.read_data(syncer.success_cb)
+        syncer.wait()
+        if syncer.is_success:
+            return syncer.success_args[1]
+        else:
+            return None
+
+    def read_failed(self, mem, addr):
+        if mem.id == self.id:
+            logger.debug('Read failed')
+
+    def disconnect(self):
+        self._read_finished_cb = None

cflib/crazyflie/mem/paa3905_memory.py

@@ -0,0 +1,68 @@
+# -*- coding: utf-8 -*-
+#
+# ,---------,       ____  _ __
+# |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+# | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+# | / ,--'  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+# Copyright (C) 2019 - 2020 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, in version 3.
+#
+# 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, see <http://www.gnu.org/licenses/>.
+import logging
+
+from .memory_element import MemoryElement
+from cflib.utils.callbacks import Syncer
+
+logger = logging.getLogger(__name__)
+
+
+class PAA3905Memory(MemoryElement):
+    """Memory interface for reading the multiranger values"""
+
+    def __init__(self, id, type, size, mem_handler):
+        super(PAA3905Memory, self).__init__(id=id, type=type, size=size,
+                                            mem_handler=mem_handler)
+        self._read_finished_cb = None
+
+    def new_data(self, mem, addr, data):
+        """Callback for when new memory data has been fetched"""
+        if mem.id == self.id and self._read_finished_cb:
+            print(len(data))
+            image_matrix = []
+            for i in range(35):
+                image_matrix.append(data[i*35:i*35+35])
+
+            self._read_finished_cb(addr, image_matrix)
+
+    def read_data(self, read_finished_cb):
+        """Read image data from PAA3905"""
+        self._read_finished_cb = read_finished_cb
+        self.mem_handler.read(self, 0, 1225)
+
+    def read_data_sync(self):
+        """Write the saved LED-ring data to the Crazyflie"""
+        syncer = Syncer()
+        self.read_data(syncer.success_cb)
+        syncer.wait()
+        if syncer.is_success:
+            return syncer.success_args[1]
+        else:
+            return None
+
+    def read_failed(self, mem, addr):
+        if mem.id == self.id:
+            logger.debug('Read failed')
+
+    def disconnect(self):
+        self._read_finished_cb = None

examples/memory/read-l5.py

@@ -0,0 +1,75 @@
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2019 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, see <https://www.gnu.org/licenses/>.
+"""
+Example of how to read the memory from the multiranger
+"""
+import logging
+import time
+from threading import Event
+
+import matplotlib.pyplot as plt
+
+import cflib.crtp  # noqa
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.mem import MemoryElement
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from cflib.utils import uri_helper
+
+# Only output errors from the logging framework
+logging.basicConfig(level=logging.ERROR)
+
+
+class ReadMem:
+    def __init__(self, uri):
+        self._event = Event()
+        self._cf = Crazyflie(rw_cache='./cache')
+
+        with SyncCrazyflie(uri, cf=self._cf) as scf:
+            mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MULTIRANGER)
+
+            count = len(mems)
+            if count != 1:
+                raise Exception('Unexpected nr of memories found:', count)
+
+            mem = mems[0]
+
+            data = [[0 for x in range(8)] for y in range(8)]
+            im = plt.imshow(data, cmap='gray', vmin=0, vmax=400)
+
+            start_time = time.time()
+            for frames in range(100):
+                data = mem.read_data_sync()
+                im.set_data(data)
+                plt.pause(0.01)
+
+            end_time = time.time()
+            print('FPS: {}'.format(100/(end_time - start_time)))
+
+
+if __name__ == '__main__':
+    # URI to the Crazyflie to connect to
+    uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
+
+    # Initialize the low-level drivers
+    cflib.crtp.init_drivers()
+
+    rm = ReadMem(uri)

examples/memory/read-paa3905.py

@@ -0,0 +1,77 @@
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2019 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, see <https://www.gnu.org/licenses/>.
+"""
+Example of how to read the memory from the multiranger
+"""
+import logging
+import time
+from threading import Event
+
+import matplotlib.pyplot as plt
+
+import cflib.crtp  # noqa
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.mem import MemoryElement
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from cflib.utils import uri_helper
+
+# Only output errors from the logging framework
+logging.basicConfig(level=logging.ERROR)
+
+
+class ReadMem:
+    def __init__(self, uri):
+        self._event = Event()
+        self._cf = Crazyflie(rw_cache='./cache')
+
+        with SyncCrazyflie(uri, cf=self._cf) as scf:
+            mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_PAA3905)
+
+            count = len(mems)
+            if count != 1:
+                raise Exception('Unexpected nr of memories found:', count)
+
+            mem = mems[0]
+
+            data = [[0 for x in range(35)] for y in range(35)]
+            im = plt.imshow(data, cmap='gray', vmin=0, vmax=255, origin='upper')
+
+            start_time = time.time()
+            for frames in range(100):
+                data = mem.read_data_sync()
+                im.set_data(data)
+                plt.pause(0.01)
+
+            end_time = time.time()
+            print('FPS: {}'.format(100/(end_time - start_time)))
+            time.sleep(5)
+
+
+if __name__ == '__main__':
+    # URI to the Crazyflie to connect to
+    #    uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
+    uri = uri_helper.uri_from_env(default='usb://0')
+
+    # Initialize the low-level drivers
+    cflib.crtp.init_drivers()
+
+    rm = ReadMem(uri)