Bug fixes, send/receive debug logs and Unsigned 24 support

.gitignore

@@ -4,6 +4,7 @@
 __pycache__/
 *.py[cod]
 *$py.class
+.idea/
 
 # C extensions
 *.so

canopen/network.py

@@ -79,7 +79,8 @@ def unsubscribe(self, can_id, callback=None):
         if callback is None:
             del self.subscribers[can_id]
         else:
-            self.subscribers[can_id].remove(callback)
+            if callback in self.subscribers[can_id]:
+                self.subscribers[can_id].remove(callback)
 
     def connect(self, *args, **kwargs):
         """Connect to CAN bus using python-can.

canopen/objectdictionary/__init__.py

@@ -219,6 +219,17 @@ def add_member(self, variable):
         self.subindices[variable.subindex] = variable
         self.names[variable.name] = variable
 
+class Unsigned24(struct.Struct):
+    def __init__(self, *args, **kwargs):
+        super(Unsigned24, self).__init__("<I", *args, **kwargs)
+
+    def unpack(self, data, *args, **kwargs):
+        if isinstance(data, bytearray):
+            while len(data) < 4:
+                data += b'\x00'
+        else:
+            logger.error(f"Unsigned24.unpack received wrong type - {type(data)}")
+        return super(Unsigned24, self).unpack(data, *args, **kwargs)
 
 class Variable(object):
     """Simple variable."""
@@ -232,6 +243,7 @@ class Variable(object):
         UNSIGNED8: struct.Struct("B"),
         UNSIGNED16: struct.Struct("<H"),
         UNSIGNED32: struct.Struct("<L"),
+        UNSIGNED24: Unsigned24(),
         UNSIGNED64: struct.Struct("<Q"),
         REAL32: struct.Struct("<f"),
         REAL64: struct.Struct("<d")

canopen/objectdictionary/datatypes.py

@@ -13,10 +13,11 @@
 DOMAIN = 0xF
 REAL64 = 0x11
 INTEGER64 = 0x15
+UNSIGNED24 = 0x16
 UNSIGNED64 = 0x1B
 
 SIGNED_TYPES = (INTEGER8, INTEGER16, INTEGER32, INTEGER64)
-UNSIGNED_TYPES = (UNSIGNED8, UNSIGNED16, UNSIGNED32, UNSIGNED64)
+UNSIGNED_TYPES = (UNSIGNED8, UNSIGNED16, UNSIGNED24, UNSIGNED32, UNSIGNED64)
 INTEGER_TYPES = SIGNED_TYPES + UNSIGNED_TYPES
 FLOAT_TYPES = (REAL32, REAL64)
 NUMBER_TYPES = INTEGER_TYPES + FLOAT_TYPES

canopen/pdo/base.py

@@ -562,7 +562,7 @@ def get_data(self):
                 data = data | (~((1 << self.length) - 1))
             data = od_struct.pack(data)
         else:
-            data = self.pdo_parent.data[byte_offset:byte_offset + len(self.od) // 8]
+            data = self.pdo_parent.data[byte_offset:byte_offset + self.length // 8]
 
         return data
 

canopen/sdo/client.py

@@ -29,6 +29,9 @@ class SdoClient(SdoBase):
     #: Seconds to wait before sending a request, for rate limiting
     PAUSE_BEFORE_SEND = 0.0
 
+    # Seconds to wait before next read attempt for response in queue. For delayed responses.
+    PAUSE_BEFORE_READ = 0.2
+
     def __init__(self, rx_cobid, tx_cobid, od):
         """
         :param int rx_cobid:
@@ -42,6 +45,7 @@ def __init__(self, rx_cobid, tx_cobid, od):
         self.responses = queue.Queue()
 
     def on_response(self, can_id, data, timestamp):
+        logger.debug(f"received response in {can_id} data {data}.")
         self.responses.put(bytes(data))
 
     def send_request(self, request):
@@ -50,6 +54,7 @@ def send_request(self, request):
             try:
                 if self.PAUSE_BEFORE_SEND:
                     time.sleep(self.PAUSE_BEFORE_SEND)
+                logger.debug(f"sending to {self.rx_cobid} data {request} ")
                 self.network.send_message(self.rx_cobid, request)
             except CanError as e:
                 # Could be a buffer overflow. Wait some time before trying again
@@ -88,6 +93,7 @@ def request_response(self, sdo_request):
                 if not retries_left:
                     self.abort(0x5040000) 
                     raise
+                time.sleep(self.PAUSE_BEFORE_READ)
                 logger.warning(str(e))
 
     def abort(self, abort_code=0x08000000):

canopen/sdo/server.py

@@ -172,6 +172,7 @@ def segmented_download(self, command, request):
         self.send_response(response)
 
     def send_response(self, response):
+        logger.debug(f"Sending to {self.tx_cobid} data {response}.")
         self.network.send_message(self.tx_cobid, response)
 
     def abort(self, abort_code=0x08000000):