beremiz: comparison plugins/canfestival/config

equal deleted inserted replaced

-:6dbde4a0c31d
+:651b8fb572e7
 RPDO = 1
 TPDO = 2
 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}
 VariableDirText = {TPDO : "__I", RPDO : "__Q"}
 VariableTypeOffset = dict(zip(["","X","B","W","D","L"], range(6)))
 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
 @param value: value expressed in integer
 @param pdomapping: list of PDO mappings
 @return: a tuple of value and number of parameters to add to DCF
 """
 # 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
+if len(pdomapping) > 0:
-for subindex, (name, loc_infos) in enumerate(pdomapping):
+dcfdata += LE_to_BE(idx + 0x200, 2) + LE_to_BE(0x00, 1) + LE_to_BE(0x01, 4) + LE_to_BE(len(pdomapping), 1)
-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
+# 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:
 def __init__(self, nodelist, nodename):
 self.LocationsNotMapped = {}
 # Dictionary of location informations classed by name
 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
 self.PointedVariables = {}
 node = nodeinfos["Node"]
 node.SetNodeID(nodeid)
 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:
 RSDO_cobid = 0x600 + nodeid
 TSDO_cobid = node.GetEntry(0x1200,0x02)
 def GetMasterNode(self):
 """
 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):
 """
 Add entry to DCF, for the requested nodeID
 @param nodeid: id of the slave (int)
 # Build new DCF
 dcf = LE_to_BE(nbparams, 0x04) + data
 # 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
+# Add an entry to MasterMapping
-result = self.GetNewCobID(nodeid, pdotype)
+self.MasterMapping[pdocobid] = {"type" : InvertPDOType[pdotype],
-if result:
+"mapping" : [None] + [(loc_infos["type"], name) for name, loc_infos in pdomapping]}
-new_cobid, new_idx = result
+# Return the data to add to DCF
-# Increment the number of PDO of this type for node
+if sync_TPDOs:
-self.SlavesPdoNumber[nodeid][pdotype] += 1
+return GeneratePDOMappingDCF(pdoindex, pdocobid, 0x01, pdomapping)
+else:
-# Add an entry to MasterMapping
+return GeneratePDOMappingDCF(pdoindex, pdocobid, 0xFF, pdomapping)
-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)
 return 0, ""
 def GenerateDCF(self, locations, current_location, sync_TPDOs):
 """
 Generate Concise DCF of MasterNode for the locations list given
 for location in locations:
 COlocationtype = IECToCOType[location["IEC_TYPE"]]
 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
 direction = location["DIR"]
 # Extract and check nodeid
 nodeid, index, subindex = loc[:3]
 # 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)
 # If a PDO mappable
 if subentry_infos and subentry_infos["pdo"]:
 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
 #-------------------------------------------------------------------------------
 nbparams = 0
 dataparams = ""
 # Generate the best PDO mapping for each type of PDO
 for pdotype in (TPDO, RPDO):
-pdosize = 0
+if len(locations[pdotype]) > 0:
-pdomapping = []
+pdosize = 0
-for name, loc_infos in locations[pdotype]:
+pdomapping = []
-pdosize += loc_infos["size"]
+result = self.GetEmptyPDO(nodeid, pdotype)
-# If pdo's size > 64 bits
+if result is None:
-if pdosize > 64:
+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)
 #-------------------------------------------------------------------------------
 #                         Master Node Configuration
 for location in locations:
 COlocationtype = IECToCOType[location["IEC_TYPE"]]
 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
 # Extract and check nodeid
 index, subindex = loc[:2]
 # 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
 return pointers

changeset 340	651b8fb572e7
parent 307	b80d3a84b8bf
child 361	331d698e1118