Adding support for using only PDO define in EDS file and not configured for adding mapping in node
authorgreg
Mon, 04 May 2009 09:31:16 +0200
changeset 340 651b8fb572e7
parent 339 6dbde4a0c31d
child 341 d3ae1f0e0220
Adding support for using only PDO define in EDS file and not configured for adding mapping in node
plugins/canfestival/config_utils.py
--- a/plugins/canfestival/config_utils.py	Mon May 04 09:29:24 2009 +0200
+++ b/plugins/canfestival/config_utils.py	Mon May 04 09:31:16 2009 +0200
@@ -36,6 +36,8 @@
 
 SlavePDOType = {"I" : TPDO, "Q" : RPDO}
 InvertPDOType = {RPDO : TPDO, TPDO : RPDO}
+PDOTypeBaseIndex = {RPDO : 0x1400, TPDO : 0x1800}
+PDOTypeBaseCobId = {RPDO : 0x200, TPDO : 0x180}
 
 VariableIncrement = 0x100
 VariableStartIndex = {TPDO : 0x2000, RPDO : 0x4000}
@@ -44,6 +46,14 @@
 
 TrashVariables = [(1, 0x01), (8, 0x05), (16, 0x06), (32, 0x07), (64, 0x1B)]
 
+#-------------------------------------------------------------------------------
+#                  Specific exception for PDO mapping errors
+#-------------------------------------------------------------------------------
+
+class PDOmappingException(Exception):
+    pass
+
+
 def LE_to_BE(value, size):
     """
     Convert Little Endian to Big Endian
@@ -109,18 +119,21 @@
     """
     
     # Create entry for RPDO or TPDO parameters and Disable PDO
-    dcfdata = LE_to_BE(idx, 2) + LE_to_BE(0x01, 1) + LE_to_BE(0x04, 4) + LE_to_BE((0x80000000 + cobid), 4)
+    dcfdata = LE_to_BE(idx, 2) + LE_to_BE(0x01, 1) + LE_to_BE(0x04, 4) + LE_to_BE(0x80000000 + cobid, 4)
     # Set Transmit type synchrone
     dcfdata += LE_to_BE(idx, 2) + LE_to_BE(0x02, 1) + LE_to_BE(0x01, 4) + LE_to_BE(transmittype, 1)
     # Re-Enable PDO
     #         ---- INDEX -----   --- SUBINDEX ----   ----- SIZE ------   ------ DATA ------
-    dcfdata += LE_to_BE(idx, 2) + LE_to_BE(0x01, 1) + LE_to_BE(0x04, 4) + LE_to_BE(0x00000000 + cobid, 4)
+    dcfdata += LE_to_BE(idx, 2) + LE_to_BE(0x01, 1) + LE_to_BE(0x04, 4) + LE_to_BE(cobid, 4)
     nbparams = 3
-    # Map Variables
-    for subindex, (name, loc_infos) in enumerate(pdomapping):
-        value = (loc_infos["index"] << 16) + (loc_infos["subindex"] << 8) + loc_infos["size"]
-        dcfdata += LE_to_BE(idx + 0x200, 2) + LE_to_BE(subindex + 1, 1) + LE_to_BE(0x04, 4) + LE_to_BE(value, 4)
+    if len(pdomapping) > 0:
+        dcfdata += LE_to_BE(idx + 0x200, 2) + LE_to_BE(0x00, 1) + LE_to_BE(0x01, 4) + LE_to_BE(len(pdomapping), 1)
         nbparams += 1
+        # Map Variables
+        for subindex, (name, loc_infos) in enumerate(pdomapping):
+            value = (loc_infos["index"] << 16) + (loc_infos["subindex"] << 8) + loc_infos["size"]
+            dcfdata += LE_to_BE(idx + 0x200, 2) + LE_to_BE(subindex + 1, 1) + LE_to_BE(0x04, 4) + LE_to_BE(value, 4)
+            nbparams += 1
     return dcfdata, nbparams
 
 class ConciseDCFGenerator:
@@ -134,7 +147,6 @@
         self.MasterMapping = {}
         # List of COB IDs available
         self.ListCobIDAvailable = range(0x180, 0x580)
-        self.SlavesPdoNumber = {}
         # Dictionary of mapping value where unexpected variables are stored
         self.TrashVariables = {}
         # Dictionary of pointed variables
@@ -206,9 +218,6 @@
             
             RPDOnumber, TPDOnumber = self.RemoveUsedNodeCobId(node)
             
-            # Store the number of TPDO and RPDO for this node
-            self.SlavesPdoNumber[nodeid] = {RPDO : RPDOnumber, TPDO : TPDOnumber}
-            
             # Get Slave's default SDO server parameters
             RSDO_cobid = node.GetEntry(0x1200,0x01)
             if not RSDO_cobid:
@@ -229,45 +238,6 @@
         Return MasterNode.
         """
         return self.MasterNode
-
-    
-    def GetNewCobID(self, nodeid, type):
-        """
-        Select a COB ID from the list of those available
-        @param nodeid: id of the slave (int)
-        @param type: type of PDO (RPDO or TPDO)
-        @return: a tuple of the COD ID and PDO index or None
-        """
-        # Verify that there is still some cobid available
-        if len(self.ListCobIDAvailable) == 0:
-            return None
-        
-        # Get the number of PDO of the type given for the node
-        nbSlavePDO = self.SlavesPdoNumber[nodeid][type]
-        if type == RPDO:
-            if nbSlavePDO < 4:
-                # For the four first RPDO -> cobid = 0x200 + ( numPdo parameters * 0x100) + nodeid
-                newcobid = (0x200 + nbSlavePDO * 0x100 + nodeid)
-                # Return calculated cobid if it's still available
-                if newcobid in self.ListCobIDAvailable:
-                    self.ListCobIDAvailable.remove(newcobid)
-                    return newcobid, 0x1400 + nbSlavePDO
-            # Return the first cobid available if no cobid found
-            return self.ListCobIDAvailable.pop(0), 0x1400 + nbSlavePDO
-    
-        elif type == TPDO:
-            if nbSlavePDO < 4:
-                # For the four first TPDO -> cobid = 0x180 + ( numPdo parameters * 0x100) + nodeid
-                newcobid = (0x180 + nbSlavePDO * 0x100 + nodeid)
-                # Return calculated cobid if it's still available
-                if newcobid in self.ListCobIDAvailable:
-                    self.ListCobIDAvailable.remove(newcobid)
-                    return newcobid, 0x1800 + nbSlavePDO
-            # Return the first cobid available if no cobid found
-            return self.ListCobIDAvailable.pop(0), 0x1800 + nbSlavePDO
-        
-        return None
-    
     
     def AddParamsToDCF(self, nodeid, data, nbparams):
         """
@@ -291,30 +261,57 @@
         # Set new DCF for slave
         self.MasterNode.SetEntry(0x1F22, nodeid, dcf)
     
-    def AddPDOMapping(self, nodeid, pdotype, pdomapping, sync_TPDOs):
+    def GetEmptyPDO(self, nodeid, pdotype, start_index=None):
+        """
+        Search a not configured PDO for a slave
+        @param node: the slave node object
+        @param pdotype: type of PDO to generated (RPDO or TPDO)
+        @param start_index: Index where search must start (default: None)
+        @return tuple of PDO index, COB ID and number of subindex defined
+        """
+        # If no start_index defined, start with PDOtype base index
+        if start_index is None:
+            index = PDOTypeBaseIndex[pdotype]
+        else:
+            index = start_index
+        
+        # Search for all PDO possible index until find a configurable PDO
+        # starting from start_index
+        while index < PDOTypeBaseIndex[pdotype] + 0x200:
+            values = self.NodeList.GetSlaveNodeEntry(nodeid, index + 0x200)
+            if values != None and values[0] > 0:
+                # Check that all subindex upper than 0 equal 0 => configurable PDO
+                if reduce(lambda x, y: x and y, map(lambda x: x == 0, values[1:]), True):
+                    cobid = self.NodeList.GetSlaveNodeEntry(nodeid, index, 1)
+                    # If no COB ID defined in PDO, generate a new one (not used)
+                    if cobid == 0:
+                        if len(self.ListCobIDAvailable) == 0:
+                            return None
+                        # Calculate COB ID from standard values
+                        if index < PDOTypeBaseIndex[pdotype] + 4:
+                            cobid = PDOTypeBaseCobId[pdotype] + 0x100 * (index - PDOTypeBaseIndex[pdotype]) + nodeid
+                        if cobid not in self.ListCobIDAvailable:
+                            cobid = self.ListCobIDAvailable.pop(0)
+                    return index, cobid, values[0]
+            index += 1
+        return None
+    
+    def AddPDOMapping(self, nodeid, pdotype, pdoindex, pdocobid, pdomapping, sync_TPDOs):
         """
         Record a new mapping request for a slave, and add related slave config to the DCF
         @param nodeid: id of the slave (int)
         @param pdotype: type of PDO to generated (RPDO or TPDO)
         @param pdomapping: list od variables to map with PDO
         """
-        # Get a new cob id
-        result = self.GetNewCobID(nodeid, pdotype)
-        if result:
-            new_cobid, new_idx = result
-            
-            # Increment the number of PDO of this type for node
-            self.SlavesPdoNumber[nodeid][pdotype] += 1
-            
-            # Add an entry to MasterMapping
-            self.MasterMapping[new_cobid] = {"type" : InvertPDOType[pdotype], 
-                "mapping" : [None] + [(loc_infos["type"], name) for name, loc_infos in pdomapping]}
-            
-            # Return the data to add to DCF
-            if sync_TPDOs:
-                return GeneratePDOMappingDCF(new_idx, new_cobid, 0x01, pdomapping)
-            else:
-                return GeneratePDOMappingDCF(new_idx, new_cobid, 0xFF, pdomapping)
+        # Add an entry to MasterMapping
+        self.MasterMapping[pdocobid] = {"type" : InvertPDOType[pdotype], 
+            "mapping" : [None] + [(loc_infos["type"], name) for name, loc_infos in pdomapping]}
+        
+        # Return the data to add to DCF
+        if sync_TPDOs:
+            return GeneratePDOMappingDCF(pdoindex, pdocobid, 0x01, pdomapping)
+        else:
+            return GeneratePDOMappingDCF(pdoindex, pdocobid, 0xFF, pdomapping)
         return 0, ""
     
     def GenerateDCF(self, locations, current_location, sync_TPDOs):
@@ -335,13 +332,13 @@
             name = location["NAME"]
             if name in self.IECLocations:
                 if self.IECLocations[name]["type"] != COlocationtype:
-                    raise ValueError, "Conflict type for location \"%s\"" % name 
+                    raise PDOmappingException, "Conflict type for location \"%s\"" % name 
             else:
                 # Get only the part of the location that concern this node
                 loc = location["LOC"][len(current_location):]
                 # loc correspond to (ID, INDEX, SUBINDEX [,BIT])
                 if len(loc) not in (2, 3, 4):
-                    raise ValueError, "Bad location size : %s"%str(loc)
+                    raise PDOmappingException, "Bad location size : %s"%str(loc)
                 elif len(loc) == 2:
                     continue
                 
@@ -354,14 +351,14 @@
                 
                 # Check Id is in slave node list
                 if nodeid not in self.NodeList.SlaveNodes.keys():
-                    raise ValueError, "Non existing node ID : %d (variable %s)" % (nodeid,name)
+                    raise PDOmappingException, "Non existing node ID : %d (variable %s)" % (nodeid,name)
                 
                 # Get the model for this node (made from EDS)
                 node = self.NodeList.SlaveNodes[nodeid]["Node"]
                 
                 # Extract and check index and subindex
                 if not node.IsEntry(index, subindex):
-                    raise ValueError, "No such index/subindex (%x,%x) in ID : %d (variable %s)" % (index,subindex,nodeid,name)
+                    raise PDOmappingException, "No such index/subindex (%x,%x) in ID : %d (variable %s)" % (index,subindex,nodeid,name)
                 
                 # Get the entry info
                 subentry_infos = node.GetSubentryInfos(index, subindex)
@@ -371,19 +368,19 @@
                     if sizelocation == "X" and len(loc) > 3:
                         numbit = loc[3]
                     elif sizelocation != "X" and len(loc) > 3:
-                        raise ValueError, "Cannot set bit offset for non bool '%s' variable (ID:%d,Idx:%x,sIdx:%x))" % (name,nodeid,index,subindex)
+                        raise PDOmappingException, "Cannot set bit offset for non bool '%s' variable (ID:%d,Idx:%x,sIdx:%x))" % (name,nodeid,index,subindex)
                     else:
                         numbit = None
                     
                     if location["IEC_TYPE"] != "BOOL" and subentry_infos["type"] != COlocationtype:
-                        raise ValueError, "Invalid type \"%s\"-> %d != %d  for location\"%s\"" % (location["IEC_TYPE"], COlocationtype, subentry_infos["type"] , name)
+                        raise PDOmappingException, "Invalid type \"%s\"-> %d != %d  for location\"%s\"" % (location["IEC_TYPE"], COlocationtype, subentry_infos["type"] , name)
                     
                     typeinfos = node.GetEntryInfos(COlocationtype)
                     self.IECLocations[name] = {"type":COlocationtype, "pdotype":SlavePDOType[direction],
                                                 "nodeid": nodeid, "index": index,"subindex": subindex,
                                                 "bit": numbit, "size": typeinfos["size"], "sizelocation": sizelocation}
                 else:
-                    raise ValueError, "Not PDO mappable variable : '%s' (ID:%d,Idx:%x,sIdx:%x))" % (name,nodeid,index,subindex)
+                    raise PDOmappingException, "Not PDO mappable variable : '%s' (ID:%d,Idx:%x,sIdx:%x))" % (name,nodeid,index,subindex)
         
         #-------------------------------------------------------------------------------
         #                         Search for locations already mapped
@@ -450,27 +447,36 @@
             
             # Generate the best PDO mapping for each type of PDO
             for pdotype in (TPDO, RPDO):
-                pdosize = 0
-                pdomapping = []
-                for name, loc_infos in locations[pdotype]:
-                    pdosize += loc_infos["size"]
-                    # If pdo's size > 64 bits
-                    if pdosize > 64:
+                if len(locations[pdotype]) > 0:
+                    pdosize = 0
+                    pdomapping = []
+                    result = self.GetEmptyPDO(nodeid, pdotype)
+                    if result is None:
+                        raise PDOmappingException, "Impossible to define PDO mapping for node %02x"%nodeid
+                    pdoindex, pdocobid, pdonbparams = result
+                    for name, loc_infos in locations[pdotype]:
+                        pdosize += loc_infos["size"]
+                        # If pdo's size > 64 bits
+                        if pdosize > 64 or len(pdomapping) >= pdonbparams:
+                            # Generate a new PDO Mapping
+                            data, nbaddedparams = self.AddPDOMapping(nodeid, pdotype, pdoindex, pdocobid, pdomapping, sync_TPDOs)
+                            dataparams += data
+                            nbparams += nbaddedparams
+                            pdosize = loc_infos["size"]
+                            pdomapping = [(name, loc_infos)]
+                            result = self.GetEmptyPDO(nodeid, pdotype, pdoindex + 1)
+                            if result is None:
+                                raise PDOmappingException, "Impossible to define PDO mapping for node %02x"%nodeid
+                            pdoindex, pdocobid, pdonbparams = result
+                        else:
+                            pdomapping.append((name, loc_infos))
+                    # If there isn't locations yet but there is still a PDO to generate
+                    if len(pdomapping) > 0:
                         # Generate a new PDO Mapping
-                        data, nbaddedparams = self.AddPDOMapping(nodeid, pdotype, pdomapping, sync_TPDOs)
+                        data, nbaddedparams = self.AddPDOMapping(nodeid, pdotype, pdoindex, pdocobid, pdomapping, sync_TPDOs)
                         dataparams += data
                         nbparams += nbaddedparams
-                        pdosize = loc_infos["size"]
-                        pdomapping = [(name, loc_infos)]
-                    else:
-                        pdomapping.append((name, loc_infos))
-                # If there isn't locations yet but there is still a PDO to generate
-                if len(pdomapping) > 0:
-                    # Generate a new PDO Mapping
-                    data, nbaddedparams = self.AddPDOMapping(nodeid, pdotype, pdomapping, sync_TPDOs)
-                    dataparams += data
-                    nbparams += nbaddedparams
-            
+                
             # Add number of params and data to node DCF
             self.AddParamsToDCF(nodeid, dataparams, nbparams)
         
@@ -608,13 +614,13 @@
         name = location["NAME"]
         if name in IECLocations:
             if IECLocations[name] != COlocationtype:
-                raise ValueError, "Conflict type for location \"%s\"" % name 
+                raise PDOmappingException, "Conflict type for location \"%s\"" % name 
         else:
             # Get only the part of the location that concern this node
             loc = location["LOC"][len(current_location):]
             # loc correspond to (ID, INDEX, SUBINDEX [,BIT])
             if len(loc) not in (2, 3, 4):
-                raise ValueError, "Bad location size : %s"%str(loc)
+                raise PDOmappingException, "Bad location size : %s"%str(loc)
             elif len(loc) != 2:
                 continue
             
@@ -623,12 +629,12 @@
             
             # Extract and check index and subindex
             if not slave.IsEntry(index, subindex):
-                raise ValueError, "No such index/subindex (%x,%x) (variable %s)" % (index, subindex, name)
+                raise PDOmappingException, "No such index/subindex (%x,%x) (variable %s)" % (index, subindex, name)
             
             # Get the entry info
             subentry_infos = slave.GetSubentryInfos(index, subindex)    
             if subentry_infos["type"] != COlocationtype:
-                raise ValueError, "Invalid type \"%s\"-> %d != %d  for location\"%s\"" % (location["IEC_TYPE"], COlocationtype, subentry_infos["type"] , name)
+                raise PDOmappingException, "Invalid type \"%s\"-> %d != %d  for location\"%s\"" % (location["IEC_TYPE"], COlocationtype, subentry_infos["type"] , name)
             
             IECLocations[name] = COlocationtype
             pointers[(index, subindex)] = name