Scan for nodes and transfer images

canopen/lss.py

@@ -64,6 +64,11 @@
     CS_INQUIRE_NODE_ID,
 ]
 
+ListMessageOptionalResponse = [
+    CS_IDENTIFY_REMOTE_SLAVE_SERIAL_NUMBER_HIGH,
+]
+
+
 
 class LssMaster:
     """The Master of Layer Setting Services"""
@@ -79,7 +84,7 @@ class LssMaster:
     CONFIGURATION_MODE = 0x01
 
     #: Max time in seconds to wait for response from server
-    RESPONSE_TIMEOUT = 0.5
+    RESPONSE_TIMEOUT = 0.25
 
     def __init__(self):
         self.network = None
@@ -233,7 +238,8 @@ def send_identify_remote_slave(self,
         self.__send_lss_address(CS_IDENTIFY_REMOTE_SLAVE_REVISION_NUMBER_LOW, revisionNumberLow)
         self.__send_lss_address(CS_IDENTIFY_REMOTE_SLAVE_REVISION_NUMBER_HIGH, revisionNumberHigh)
         self.__send_lss_address(CS_IDENTIFY_REMOTE_SLAVE_SERIAL_NUMBER_LOW, serialNumberLow)
-        self.__send_lss_address(CS_IDENTIFY_REMOTE_SLAVE_SERIAL_NUMBER_HIGH, serialNumberHigh)
+        r = self.__send_lss_address(CS_IDENTIFY_REMOTE_SLAVE_SERIAL_NUMBER_HIGH, serialNumberHigh)
+        return r is not None
 
     def send_identify_non_configured_remote_slave(self):
         # TODO it should handle the multiple respones from slaves
@@ -242,12 +248,12 @@ def send_identify_non_configured_remote_slave(self):
         self.__send_command(message)
 
     def fast_scan(self):
-        """This command sends a series of fastscan message 
+        """This command sends a series of fastscan message
         to find unconfigured slave with lowest number of LSS idenities
 
         :return:
             True if a slave is found.
-            False if there is no candidate. 
+            False if there is no candidate.
             list is the LSS identities [vendor_id, product_code, revision_number, serial_number]
         :rtype: bool, list
         """
@@ -304,7 +310,7 @@ def __send_lss_address(self, req_cs, number):
         response = self.__send_command(message)
         # some device needs these delays between messages
         # because it can't handle messages arriving with no delay
-        time.sleep(0.2)
+        #time.sleep(0.2)
 
         return response
 
@@ -382,16 +388,26 @@ def __send_command(self, message):
 
         self.network.send_message(self.LSS_TX_COBID, message)
 
-        if not bool(message[0] in ListMessageNeedResponse):
-            return response
-
-        # Wait for the slave to respond
-        # TODO check if the response is LSS response message
-        try:
-            response = self.responses.get(
-                block=True, timeout=self.RESPONSE_TIMEOUT)
-        except queue.Empty:
-            raise LssError("No LSS response received")
+        if bool(message[0] in ListMessageNeedResponse):
+            # Wait for the slave to respond
+            # TODO check if the response is LSS response message
+            try:
+                response = self.responses.get(
+                    block=True, timeout=self.RESPONSE_TIMEOUT)
+            except queue.Empty:
+                raise LssError("No LSS response received")
+
+        elif bool(message[0] in ListMessageOptionalResponse):
+            # Wait for the slave to respond
+            # TODO check if the response is LSS response message
+            try:
+                response = self.responses.get(
+                    block=True, timeout=self.RESPONSE_TIMEOUT)
+            except queue.Empty:
+                pass
+
+        else:
+            pass
 
         return response
 

canopen/sdo/client.py

@@ -85,7 +85,7 @@ def request_response(self, sdo_request):
             except SdoCommunicationError as e:
                 retries_left -= 1
                 if not retries_left:
-                    self.abort(0x5040000) 
+                    self.abort(0x5040000)
                     raise
                 logger.warning(str(e))
 
@@ -156,6 +156,7 @@ def download(
             When node responds with an error.
         """
         with self.open(index, subindex, "wb", buffering=7, size=len(data),
+                       block_transfer=True,
                        force_segment=force_segment) as fp:
             fp.write(data)
 
@@ -462,7 +463,7 @@ def __init__(self, sdo_client, index, subindex=0, request_crc_support=True):
         :param int subindex:
             Object dictionary sub-index to read from.
         :param bool request_crc_support:
-            If crc calculation should be requested when using block transfer            
+            If crc calculation should be requested when using block transfer
         """
         self._done = False
         self.sdo_client = sdo_client
@@ -618,7 +619,7 @@ def __init__(self, sdo_client, index, subindex=0, size=None, request_crc_support
         :param int size:
             Size of data in number of bytes if known in advance.
         :param bool request_crc_support:
-            If crc calculation should be requested when using block transfer            
+            If crc calculation should be requested when using block transfer
         """
         self.sdo_client = sdo_client
         self.size = size
@@ -745,7 +746,7 @@ def _block_ack(self):
         logger.debug("Server requested a block size of %d", blksize)
         self._blksize = blksize
         self._seqno = 0
-        
+
     def _retransmit(self, ackseq, blksize):
         """Retransmit the failed block"""
         logger.info(("%d of %d sequences were received. "

examples/lss_scanner.py

@@ -0,0 +1,131 @@
+"""
+LSS scanner
+
+Performs a binary search using send_identify_remote_slave to all identify slaves
+with a predefined vendor and product id.
+
+"""
+
+import queue
+import canopen
+import traceback
+import time
+
+
+class Range(object):
+    '''
+    Scan Range
+    '''
+
+    def __init__(self):
+        '''
+        construct a Range object
+        '''
+        self.q = queue.Queue()
+        self.init()
+
+    def init(self):
+        self.q.queue.clear()
+        self.q.put((0, 0xffffffff))
+        self.current = None
+        self.cnt = 0
+
+    def next(self):
+        self.current = None if self.q.empty() else self.q.get()
+        return self.current
+
+    def progress(self):
+        pc = self.cnt
+        pc *= 100.0
+        pc /= 0x100000000
+        return int(pc)
+
+    def found(self, f):
+        '''
+        current range fouund
+        '''
+        assert self.current is not None
+        if self.current[0] == self.current[1] or not f:
+            self.cnt += self.current[1] - self.current[0] + 1
+        else:
+            d = self.current[1] - self.current[0]
+            if d == 1:
+                self.q.put((self.current[0], self.current[0]))
+                self.q.put((self.current[1], self.current[1]))
+            else:
+                d >>= 1
+                ne1 = self.current[0] + d, self.current[1]
+                self.q.put(ne1)
+                ne2 = self.current[0], ne1[0] - 1
+                self.q.put(ne2)
+        self.current = None
+
+
+
+def identify_all(network, vendorId, productCode, interval = 0.1):
+
+    network.nmt.state = 'STOPPED'
+    time.sleep(interval)
+    lss = network.lss
+
+    ident = []
+    revisions = []
+    rev_iter = Range()
+
+    while rev_iter.next() is not None:
+        ser_iter = Range()
+        ser_iter.next()
+        f = lss.send_identify_remote_slave(vendorId, productCode,
+                                       rev_iter.current[0], rev_iter.current[1],
+                                       ser_iter.current[0], ser_iter.current[1])
+        if f:
+            print(rev_iter.current)
+        time.sleep(interval)
+        if f and rev_iter.current[0] == rev_iter.current[1]:
+            revisions.append(rev_iter.current[0])
+        rev_iter.found(f)
+
+    for rev in revisions:
+        ser_iter = Range()
+
+        while ser_iter.next() is not None:
+            f = lss.send_identify_remote_slave(vendorId, productCode,
+                                        rev, rev,
+                                        ser_iter.current[0], ser_iter.current[1])
+            if f:
+                print(ser_iter.current)
+            time.sleep(interval)
+            if f and ser_iter.current[0] == ser_iter.current[1]:
+                lss.send_switch_state_global(lss.WAITING_STATE)
+                time.sleep(interval)
+                lss.send_switch_state_selective(vendorId, productCode, rev, ser_iter.current[0]);
+                time.sleep(interval)
+                node = lss.inquire_node_id()
+                ident.append((vendorId, productCode, rev, ser_iter.current[0], node))
+            ser_iter.found(f)
+
+    network.nmt.state = 'PRE-OPERATIONAL'
+    return ident
+
+
+
+
+if __name__ == '__main__':
+
+    VENDOR_ID = 0xFF
+    PRODUCT_CODE = 1
+
+    # Start with creating a network representing one CAN bus
+    network = canopen.Network()
+    # Connect to the CAN bus
+    network.connect(channel='vcan0', bustype='socketcan')
+    network.check()
+
+    for _ in identify_all(network, VENDOR_ID, PRODUCT_CODE):
+        print(_)
+
+    # Disconnect from CAN bus
+    if network:
+        network.sync.stop()
+        network.disconnect()
+