# HG changeset patch # User greg # Date 1189516256 -7200 # Node ID 1db681d345790077da23c991b8515f7fb52c6d5c # Parent 7bc11b005c8bc9d3cf58c89650dba05ee68c1b03 CONFIG_UTILS (sync) diff -r 7bc11b005c8b -r 1db681d34579 plugins/canfestival/config_utils.py --- a/plugins/canfestival/config_utils.py Tue Sep 11 08:00:28 2007 +0200 +++ b/plugins/canfestival/config_utils.py Tue Sep 11 15:10:56 2007 +0200 @@ -24,16 +24,13 @@ from types import * +# Translation between IEC types and Can Open types DicoTypes = {"BOOL":0x01, "SINT":0x02, "INT":0x03,"DINT":0x04,"LINT":0x10, "USINT":0x05,"UINT":0x06,"UDINT":0x07,"ULINT":0x1B,"REAL":0x08, "LREAL":0x11,"STRING":0x09,"BYTE":0x05,"WORD":0x06,"DWORD":0x07, "LWORD":0x1B,"WSTRING":0x0B} -DictLocations = {} -DictCobID = {} -DictLocationsNotMapped = {} -ListCobIDAvailable = [] -SlavesPdoNumber = {} +DictNameVariable = { "" : 1, "X": 2, "B": 3, "W": 4, "D": 5, "L": 6, "increment": 0x100, 1:("__I", 0x2000), 2:("__Q", 0x4000)} # Constants for PDO types RPDO = 1 @@ -41,104 +38,446 @@ SlavePDOType = {"I" : TPDO, "Q" : RPDO} InvertPDOType = {RPDO : TPDO, TPDO : RPDO} -DefaultTransmitTypeMaster = 0x01 - GenerateMasterMapping = lambda x:[None] + [(loc_infos["type"], name) for name, loc_infos in x] TrashVariableSizes = {1 : 0x01, 8 : 0x05, 16 : 0x06, 32 : 0x07, 64 : 0x1B} - -def GetSlavePDOIndexes(slave, type, parameters = False): +def LE_to_BE(value, size): + """ + Convert Little Endian to Big Endian + @param value: value expressed in integer + @param size: number of bytes generated + @return: a string containing the value converted + """ + + data = ("%" + str(size * 2) + "." + str(size * 2) + "X") % value + list_car = [data[i:i+2] for i in xrange(0, len(data), 2)] + list_car.reverse() + return "".join([chr(int(car, 16)) for car in list_car]) + +def GetNodePDOIndexes(node, type, parameters = False): + """ + Find the PDO indexes of a node + @param node: node + @param type: type of PDO searched (RPDO or TPDO or both) + @param parameters: indicate which indexes are expected (PDO paramaters : True or PDO mappings : False) + @return: a list of indexes found + """ + indexes = [] if type & RPDO: - indexes.extend([idx for idx in slave.GetIndexes() if 0x1400 <= idx <= 0x15FF]) + indexes.extend([idx for idx in node.GetIndexes() if 0x1400 <= idx <= 0x15FF]) if type & TPDO: - indexes.extend([idx for idx in slave.GetIndexes() if 0x1800 <= idx <= 0x19FF]) + indexes.extend([idx for idx in node.GetIndexes() if 0x1800 <= idx <= 0x19FF]) if not parameters: return [idx + 0x200 for idx in indexes] else: return indexes - -def LE_to_BE(value, size): # Convert Little Endian to Big Endian - data = ("%" + str(size * 2) + "." + str(size * 2) + "X") % value - list_car = [data[i:i+2] for i in xrange(0, len(data), 2)] - list_car.reverse() - return "".join([chr(int(car, 16)) for car in list_car]) - - - -def SearchSlavePDOMapping(loc_infos, slave): # Search the TPDO or RPDO mapping where location is defined on the slave - typeinfos = slave.GetEntryInfos(loc_infos["type"]) +def SearchNodePDOMapping(loc_infos, node): + """ + Find the PDO indexes of a node + @param node: node + @param type: type of PDO searched (RPDO or TPDO or both) + @param parameters: indicate which indexes are expected (PDO paramaters : True or PDO mappings : False) + @return: a list of indexes found + """ + + typeinfos = node.GetEntryInfos(loc_infos["type"]) model = (loc_infos["index"] << 16) + (loc_infos["subindex"] << 8) + typeinfos["size"] - slavePDOidxlist = GetSlavePDOIndexes(slave, loc_infos["pdotype"]) - - for PDOidx in slavePDOidxlist: - values = slave.GetEntry(PDOidx) + + for PDOidx in GetNodePDOIndexes(node, loc_infos["pdotype"]): + values = node.GetEntry(PDOidx) if values != None: for subindex, mapping in enumerate(values): if subindex != 0 and mapping == model: return PDOidx, subindex return None -def GenerateMappingDCF(cobid, idx, pdomapping, mapped): # Build concise 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) - # Set Transmit type synchrone - dcfdata += LE_to_BE(idx, 2) + LE_to_BE(0x02, 1) + LE_to_BE(0x01, 4) + LE_to_BE(DefaultTransmitTypeSlave, 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) - nbparams = 3 - if mapped == False and pdomapping != None: - # 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 - -def GetNewCobID(nodeid, type): # Return a cobid not used - global ListCobIDAvailable, SlavesPdoNumber - - if len(ListCobIDAvailable) == 0: +class ConciseDCFGenerator: + + def __init__(self, nodelist): + # Dictionary of location informations classed by name + self.DictLocations = {} + # Dictionary of location informations classed by name + self.DictCobID = {} + # Dictionary of location that have not been mapped yet + self.DictLocationsNotMapped = {} + # List of COB IDs available + self.ListCobIDAvailable = range(0x180, 0x580) + self.SlavesPdoNumber = {} + # Dictionary of mapping value where unexpected variables are stored + TrashVariableValue = {} + + self.NodeList = nodelist + self.Manager = self.NodeList.Manager + self.MasterNode = self.Manager.GetCurrentNodeCopy() + self.PrepareMasterNode() + + def RemoveUsedNodeCobId(self, node): + """ + Remove all PDO COB ID used by the given node from the list of available COB ID + @param node: node + @return: a tuple of number of RPDO and TPDO for the node + """ + + # Get list of all node TPDO and RPDO indexes + nodeRpdoIndexes = GetNodePDOIndexes(node, RPDO, True) + nodeTpdoIndexes = GetNodePDOIndexes(node, TPDO, True) + + # Mark all the COB ID of the node already mapped PDO as not available + for PdoIdx in nodeRpdoIndexes + nodeTpdoIndexes: + pdo_cobid = node.GetEntry(PdoIdx, 0x01) + # Extract COB ID, if PDO isn't active + if pdo_cobid > 0x600 : + pdo_cobid -= 0x80000000 + # Remove COB ID from the list of available COB ID + if pdo_cobid in self.ListCobIDAvailable: + self.ListCobIDAvailable.remove(pdo_cobid) + + return len(nodeRpdoIndexes), len(nodeTpdoIndexes) + + def PrepareMasterNode(self): + """ + Add mandatory entries for DCF generation into MasterNode. + """ + + # Adding DCF entry into Master node + if not self.MasterNode.IsEntry(0x1F22): + self.MasterNode.AddEntry(0x1F22, 1, "") + self.Manager.AddSubentriesToCurrent(0x1F22, 127, masternode) + + # Adding trash mappable variables for unused mapped datas + idxTrashVariables = 0x2000 + masternode.GetNodeID() + # Add an entry for storing unexpected all variable + self.Manager.AddMapVariableToCurrent(idxTrashVariables, "trashvariables", 3, len(TrashVariableSizes), self.MasterNode) + for subidx, (size, typeidx) in enumerate(TrashVariableSizes.items()): + # Add a subentry for storing unexpected variable of this size + self.Manager.SetCurrentEntry(idxTrashVariables, subidx + 1, "TRASH%d" % size, "name", None, self.MasterNode) + self.Manager.SetCurrentEntry(idxTrashVariables, subidx + 1, typeidx, "type", None, self.MasterNode) + # Store the mapping value for this entry + self.TrashVariableValue[size] = (idxTrashVariables << 16) + ((subidx + 1) << 8) + size + + RPDOnumber, TPDOnumber = self.RemoveUsedNodeCobId(self.MasterNode) + + # Store the indexes of the first RPDO and TPDO available for MasterNode + self.CurrentPDOParamsIdx = {RPDO : 0x1400 + RPDOnumber, TPDO : 0x1800 + TPDOnumber} + + # Prepare MasterNode with all nodelist slaves + for nodeid, nodeinfos in self.NodeList.SlaveNodes.items(): + 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) + if not TSDO_cobid: + TSDO_cobid = 0x580 + nodeid + + # Configure Master's SDO parameters entries + self.Manager.ManageEntriesOfCurrent([0x1280 + nodeid], [], self.MasterNode) + self.MasterNode.SetEntry(0x1280 + nodeid, 0x01, RSDO_cobid) + self.MasterNode.SetEntry(0x1280 + nodeid, 0x02, TSDO_cobid) + self.MasterNode.SetEntry(0x1280 + nodeid, 0x03, nodeid) + + def GetMasterNode(self): + """ + Return MasterNode. + """ + return self.MasterNode + + # Build concise DCF + def GenerateMappingDCF(cobid, idx, pdomapping, mapped): + + # 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) + # Set Transmit type synchrone + dcfdata += LE_to_BE(idx, 2) + LE_to_BE(0x02, 1) + LE_to_BE(0x01, 4) + LE_to_BE(DefaultTransmitTypeSlave, 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) + nbparams = 3 + if mapped == False and pdomapping != None: + # 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 + + # Return a cobid not used + def GetNewCobID(self, nodeid, type): + """ + Select a COB ID from the list of those available + @param nodeid: id of the slave node + @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 - - nbSlavePDO = SlavesPdoNumber[nodeid][type] - if type == RPDO: - if nbSlavePDO < 4: - # For the fourth PDO -> cobid = 0x200 + ( numPdo parameters * 0x100) + nodeid - newcobid = (0x200 + nbSlavePDO * 0x100 + nodeid) - if newcobid in ListCobIDAvailable: - ListCobIDAvailable.remove(newcobid) - return newcobid, 0x1400 + nbSlavePDO - return ListCobIDAvailable.pop(0), 0x1400 + nbSlavePDO - - elif type == TPDO: - if nbSlavePDO < 4: - # For the fourth PDO -> cobid = 0x180 + (numPdo parameters * 0x100) + nodeid - newcobid = (0x180 + nbSlavePDO * 0x100 + nodeid) - if newcobid in ListCobIDAvailable: - ListCobIDAvailable.remove(newcobid) - return newcobid, 0x1800 + nbSlavePDO - return ListCobIDAvailable.pop(0), 0x1800 + nbSlavePDO - - for number in xrange(4): - if type == RPDO: - # For the fourth PDO -> cobid = 0x200 + ( numPdo * 0x100) + nodeid - newcobid = (0x200 + number * 0x100 + nodeid) - elif type == TPDO: - # For the fourth PDO -> cobid = 0x180 + (numPdo * 0x100) + nodeid - newcobid = (0x180 + number * 0x100 + nodeid) - else: - return None - if newcobid in ListCobIDAvailable: - ListCobIDAvailable.remove(newcobid) - return newcobid - return ListCobIDAvailable.pop(0) - - + + def GenerateDCF(self, locations, current_location, sync_TPDOs): + """ + Generate Concise DCF of MasterNode for the locations list given + @param locations: list of locations to be mapped + @param current_location: tuple of the located prefixes not to be considered + @param sync_TPDOs: indicate if TPDO must be synchronous + """ + + # Get list of locations check if exists and mappables -> put them in DictLocations + for location in locations: + COlocationtype = DicoTypes[location["IEC_TYPE"]] + name = location["NAME"] + if name in DictLocations: + if DictLocations[name]["type"] != COlocationtype: + raise ValueError, "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 (3, 4): + raise ValueError, "Bad location size : %s"%str(loc) + + direction = location["DIR"] + + sizelocation = location["SIZE"] + + # Extract and check nodeid + nodeid, index, subindex = loc[:3] + + # Check Id is in slave node list + if nodeid not in nodelist.SlaveNodes.keys(): + raise ValueError, "Non existing node ID : %d (variable %s)" % (nodeid,name) + + # Get the model for this node (made from EDS) + node = 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) + + #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[4] + 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) + else: + numbit = None + + entryinfos = node.GetSubentryInfos(index, subindex) + if entryinfos["type"] != COlocationtype: + raise ValueError, "Invalid type \"%s\"-> %d != %d for location\"%s\"" % (location["IEC_TYPE"], COlocationtype, entryinfos["type"] , name) + + typeinfos = node.GetEntryInfos(COlocationtype) + DictLocations[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) + + # Create DictCobID with variables already mapped and add them in DictValidLocations + for name, locationinfos in DictLocations.items(): + node = self.NodeList.SlaveNodes[locationinfos["nodeid"]]["Node"] + result = SearchNodePDOMapping(locationinfos, node) + if result != None: + index, subindex = result + cobid = nodelist.GetSlaveNodeEntry(locationinfos["nodeid"], index - 0x200, 1) + + transmittype = nodelist.GetSlaveNodeEntry(locationinfos["nodeid"], index - 0x200, 2) + + if transmittype != sync_TPDOs: + if sync_TPDOs : # Change TransmitType to ASYCHRONE + node = nodelist.SlaveNodes[nodeid]["Node"] + nodeDCF = masternode.GetEntry(0x1F22, nodeid) + + if nodeDCF != None and nodeDCF != '': + tmpnbparams = [i for i in nodeDCF[:4]] + tmpnbparams.reverse() + nbparams = int(''.join(["%2.2x"%ord(i) for i in tmpnbparams]), 16) + dataparams = nodeDCF[4:] + else: + nbparams = 0 + dataparams = "" + + else: # Change TransmitType to SYNCHRONE + pass + + + if cobid not in DictCobID.keys(): + mapping = [None] + values = node.GetEntry(index) + for value in values[1:]: + mapping.append(value % 0x100) + DictCobID[cobid] = {"type" : InvertPDOType[locationinfos["pdotype"]], "mapping" : mapping} + + DictCobID[cobid]["mapping"][subindex] = (locationinfos["type"], name) + + else: + if locationinfos["nodeid"] not in DictLocationsNotMapped.keys(): + DictLocationsNotMapped[locationinfos["nodeid"]] = {TPDO : [], RPDO : []} + DictLocationsNotMapped[locationinfos["nodeid"]][locationinfos["pdotype"]].append((name, locationinfos)) + + #------------------------------------------------------------------------------- + # Build concise DCF for the others locations + #------------------------------------------------------------------------------- + + for nodeid, locations in DictLocationsNotMapped.items(): + # Get current concise DCF + node = nodelist.SlaveNodes[nodeid]["Node"] + nodeDCF = masternode.GetEntry(0x1F22, nodeid) + + if nodeDCF != None and nodeDCF != '': + tmpnbparams = [i for i in nodeDCF[:4]] + tmpnbparams.reverse() + nbparams = int(''.join(["%2.2x"%ord(i) for i in tmpnbparams]), 16) + dataparams = nodeDCF[4:] + else: + nbparams = 0 + dataparams = "" + + 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: + result = GetNewCobID(nodeid, pdotype) + if result: + SlavesPdoNumber[nodeid][pdotype] += 1 + new_cobid, new_idx = result + data, nbaddedparams = GenerateMappingDCF(new_cobid, new_idx, pdomapping, False) + dataparams += data + nbparams += nbaddedparams + DictCobID[new_cobid] = {"type" : InvertPDOType[pdotype], "mapping" : GenerateMasterMapping(pdomapping)} + pdosize = loc_infos["size"] + pdomapping = [(name, loc_infos)] + else: + pdomapping.append((name, loc_infos)) + if len(pdomapping) > 0: + result = GetNewCobID(nodeid, pdotype) + if result: + SlavesPdoNumber[nodeid][pdotype] += 1 + new_cobid, new_idx = result + data, nbaddedparams = GenerateMappingDCF(new_cobid, new_idx, pdomapping, False) + dataparams += data + nbparams += nbaddedparams + DictCobID[new_cobid] = {"type" : InvertPDOType[pdotype], "mapping" : GenerateMasterMapping(pdomapping)} + + dcf = LE_to_BE(nbparams, 0x04) + dataparams + masternode.SetEntry(0x1F22, nodeid, dcf) + + + #------------------------------------------------------------------------------- + # Master Node Configuration + #------------------------------------------------------------------------------- + + + + # Configure Master's PDO parameters entries and set cobid, transmit type + for cobid, pdo_infos in DictCobID.items(): + current_idx = CurrentPDOParamsIdx[pdo_infos["type"]] + addinglist = [current_idx, current_idx + 0x200] + manager.ManageEntriesOfCurrent(addinglist, [], masternode) + masternode.SetEntry(current_idx, 0x01, cobid) + masternode.SetEntry(current_idx, 0x02, DefaultTransmitTypeMaster) + if len(pdo_infos["mapping"]) > 2: + manager.AddSubentriesToCurrent(current_idx + 0x200, len(pdo_infos["mapping"]) - 2, masternode) + + # Create Master's PDO mapping + for subindex, variable in enumerate(pdo_infos["mapping"]): + if subindex == 0: + continue + new_index = False + + if type(variable) != IntType: + + typeidx, varname = variable + indexname = \ + DictNameVariable[DictLocations[variable[1]]["pdotype"]][0] + \ + DictLocations[variable[1]]["sizelocation"] + \ + '_'.join(map(str,current_location)) + \ + "_" + \ + str(DictLocations[variable[1]]["nodeid"]) + mapvariableidx = DictNameVariable[DictLocations[variable[1]]["pdotype"]][1] + DictNameVariable[DictLocations[variable[1]]["sizelocation"]] * DictNameVariable["increment"] + + #indexname = DictNameVariable[DictLocations[variable[1]]["pdotype"]][0] + DictLocations[variable[1]]["sizelocation"] + str(DictLocations[variable[1]]["prefix"]) + "_" + str(DictLocations[variable[1]]["nodeid"]) + #mapvariableidx = DictNameVariable[DictLocations[variable[1]]["pdotype"]][1] + DictNameVariable[DictLocations[variable[1]]["sizelocation"]] * DictNameVariable["increment"] + + if not masternode.IsEntry(mapvariableidx): + manager.AddMapVariableToCurrent(mapvariableidx, indexname, 3, 1, masternode) + new_index = True + nbsubentries = masternode.GetEntry(mapvariableidx, 0x00) + else: + nbsubentries = masternode.GetEntry(mapvariableidx, 0x00) + mapvariableidxbase = mapvariableidx + while mapvariableidx < (mapvariableidxbase + 0x1FF) and nbsubentries == 0xFF: + mapvariableidx += 0x800 + if not manager.IsCurrentEntry(mapvariableidx): + manager.AddMapVariableToCurrent(mapvariableidx, indexname, 3, 1, masternode) + new_index = True + nbsubentries = masternode.GetEntry(mapvariableidx, 0x00) + + if mapvariableidx < 0x6000: + if DictLocations[variable[1]]["bit"] != None: + subindexname = str(DictLocations[variable[1]]["index"]) + "_" + str(DictLocations[variable[1]]["subindex"]) + "_" + str(DictLocations[variable[1]]["bit"]) + else: + subindexname = str(DictLocations[variable[1]]["index"]) + "_" + str(DictLocations[variable[1]]["subindex"]) + if not new_index: + manager.AddSubentriesToCurrent(mapvariableidx, 1, masternode) + nbsubentries += 1 + masternode.SetMappingEntry(mapvariableidx, nbsubentries, values = {"name" : subindexname}) + masternode.SetMappingEntry(mapvariableidx, nbsubentries, values = {"type" : typeidx}) + + # Map Variable + typeinfos = manager.GetEntryInfos(typeidx) + if typeinfos != None: + value = (mapvariableidx << 16) + ((nbsubentries) << 8) + typeinfos["size"] + masternode.SetEntry(current_idx + 0x200, subindex, value) + else: + masternode.SetEntry(current_idx + 0x200, subindex, TrashVariableValue[variable]) + + CurrentPDOParamsIdx[pdo_infos["type"]] += 1 + + def GenerateConciseDCF(locations, current_location, nodelist, sync_TPDOs): """ Fills a CanFestival network editor model, with DCF with requested PDO mappings. @@ -153,277 +492,11 @@ @return: a modified copy of the given CanFestival network editor model """ - global DictLocations, DictCobID, DictLocationsNotMapped, ListCobIDAvailable, SlavesPdoNumber, DefaultTransmitTypeSlave - - DictLocations = {} - DictCobID = {} - DictLocationsNotMapped = {} - DictSDOparams = {} - ListCobIDAvailable = range(0x180, 0x580) - SlavesPdoNumber = {} - DictNameVariable = { "" : 1, "X": 2, "B": 3, "W": 4, "D": 5, "L": 6, "increment": 0x100, 1:("__I", 0x2000), 2:("__Q", 0x4000)} - DefaultTransmitTypeSlave = 0xFF - # Master Node initialisation - - manager = nodelist.Manager - masternode = manager.GetCurrentNodeCopy() - if not masternode.IsEntry(0x1F22): - masternode.AddEntry(0x1F22, 1, "") - manager.AddSubentriesToCurrent(0x1F22, 127, masternode) - # Adding trash mappable variables for unused mapped datas - idxTrashVariables = 0x2000 + masternode.GetNodeID() - TrashVariableValue = {} - manager.AddMapVariableToCurrent(idxTrashVariables, "trashvariables", 3, len(TrashVariableSizes), masternode) - for subidx, (size, typeidx) in enumerate(TrashVariableSizes.items()): - manager.SetCurrentEntry(idxTrashVariables, subidx + 1, "TRASH%d" % size, "name", None, masternode) - manager.SetCurrentEntry(idxTrashVariables, subidx + 1, typeidx, "type", None, masternode) - TrashVariableValue[size] = (idxTrashVariables << 16) + ((subidx + 1) << 8) + size - - - # Extract Master Node current empty mapping index - CurrentPDOParamsIdx = {RPDO : 0x1400 + len(GetSlavePDOIndexes(masternode, RPDO)), - TPDO : 0x1800 + len(GetSlavePDOIndexes(masternode, TPDO))} - - # Get list of all Slave's CobID and Slave's default SDO server parameters - for nodeid, nodeinfos in nodelist.SlaveNodes.items(): - node = nodeinfos["Node"] - node.SetNodeID(nodeid) - DictSDOparams[nodeid] = {"RSDO" : node.GetEntry(0x1200,0x01), "TSDO" : node.GetEntry(0x1200,0x02)} - slaveRpdoIndexes = GetSlavePDOIndexes(node, RPDO, True) - slaveTpdoIndexes = GetSlavePDOIndexes(node, TPDO, True) - SlavesPdoNumber[nodeid] = {RPDO : len(slaveRpdoIndexes), TPDO : len(slaveTpdoIndexes)} - for PdoIdx in slaveRpdoIndexes + slaveTpdoIndexes: - pdo_cobid = node.GetEntry(PdoIdx, 0x01) - if pdo_cobid > 0x600 : - pdo_cobid -= 0x80000000 - if pdo_cobid in ListCobIDAvailable: - ListCobIDAvailable.remove(pdo_cobid) - - # Get list of locations check if exists and mappables -> put them in DictLocations - for location in locations: - locationtype = location["IEC_TYPE"] - name = location["NAME"] - if name in DictLocations: - if DictLocations[name]["type"] != DicoTypes[locationtype]: - raise ValueError, "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 (3, 4): - raise ValueError, "Bad location size : %s"%str(loc) - - direction = location["DIR"] - - sizelocation = location["SIZE"] - - # Extract and check nodeid - nodeid, index, subindex = loc[:3] - - # Check Id is in slave node list - if nodeid not in nodelist.SlaveNodes.keys(): - raise ValueError, "Non existing node ID : %d (variable %s)" % (nodeid,name) - - # Get the model for this node (made from EDS) - node = 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) - - #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[4] - 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) - else: - numbit = None - - COlocationtype = DicoTypes[locationtype] - entryinfos = node.GetSubentryInfos(index, subindex) - if entryinfos["type"] != COlocationtype: - raise ValueError, "Invalid type \"%s\"-> %d != %d for location\"%s\"" % (locationtype,COlocationtype, entryinfos["type"] , name) - - typeinfos = node.GetEntryInfos(COlocationtype) - DictLocations[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) - - # Create DictCobID with variables already mapped and add them in DictValidLocations - for name, locationinfos in DictLocations.items(): - node = nodelist.SlaveNodes[locationinfos["nodeid"]]["Node"] - result = SearchSlavePDOMapping(locationinfos, node) - if result != None: - index, subindex = result - cobid = nodelist.GetSlaveNodeEntry(locationinfos["nodeid"], index - 0x200, 1) - if cobid not in DictCobID.keys(): - mapping = [None] - values = node.GetEntry(index) - for value in values[1:]: - mapping.append(value % 0x100) - DictCobID[cobid] = {"type" : InvertPDOType[locationinfos["pdotype"]], "mapping" : mapping} - - DictCobID[cobid]["mapping"][subindex] = (locationinfos["type"], name) - - else: - if locationinfos["nodeid"] not in DictLocationsNotMapped.keys(): - DictLocationsNotMapped[locationinfos["nodeid"]] = {TPDO : [], RPDO : []} - DictLocationsNotMapped[locationinfos["nodeid"]][locationinfos["pdotype"]].append((name, locationinfos)) - - # Check Master Pdo parameters for cobid already used and remove it in ListCobIDAvailable - ListPdoParams = [idx for idx in masternode.GetIndexes() if 0x1400 <= idx <= 0x15FF or 0x1800 <= idx <= 0x19FF] - for idx in ListPdoParams: - cobid = masternode.GetEntry(idx, 0x01) - if cobid not in DictCobID.keys(): - ListCobIDAvailable.pop(cobid) - - #------------------------------------------------------------------------------- - # Build concise DCF for the others locations - #------------------------------------------------------------------------------- - - for nodeid, locations in DictLocationsNotMapped.items(): - # Get current concise DCF - node = nodelist.SlaveNodes[nodeid]["Node"] - nodeDCF = masternode.GetEntry(0x1F22, nodeid) - - if nodeDCF != None and nodeDCF != '': - tmpnbparams = [i for i in nodeDCF[:4]] - tmpnbparams.reverse() - nbparams = int(''.join(["%2.2x"%ord(i) for i in tmpnbparams]), 16) - dataparams = nodeDCF[4:] - else: - nbparams = 0 - dataparams = "" - - 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: - result = GetNewCobID(nodeid, pdotype) - if result: - SlavesPdoNumber[nodeid][pdotype] += 1 - new_cobid, new_idx = result - data, nbaddedparams = GenerateMappingDCF(new_cobid, new_idx, pdomapping, False) - dataparams += data - nbparams += nbaddedparams - DictCobID[new_cobid] = {"type" : InvertPDOType[pdotype], "mapping" : GenerateMasterMapping(pdomapping)} - pdosize = loc_infos["size"] - pdomapping = [(name, loc_infos)] - else: - pdomapping.append((name, loc_infos)) - if len(pdomapping) > 0: - result = GetNewCobID(nodeid, pdotype) - if result: - SlavesPdoNumber[nodeid][pdotype] += 1 - new_cobid, new_idx = result - data, nbaddedparams = GenerateMappingDCF(new_cobid, new_idx, pdomapping, False) - dataparams += data - nbparams += nbaddedparams - DictCobID[new_cobid] = {"type" : InvertPDOType[pdotype], "mapping" : GenerateMasterMapping(pdomapping)} - - dcf = LE_to_BE(nbparams, 0x04) + dataparams - masternode.SetEntry(0x1F22, nodeid, dcf) - - - #------------------------------------------------------------------------------- - # Master Node Configuration - #------------------------------------------------------------------------------- - - # Configure Master's SDO parameters entries - for nodeid, SDOparams in DictSDOparams.items(): - SdoClient_index = [0x1280 + nodeid] - manager.ManageEntriesOfCurrent(SdoClient_index,[], masternode) - if SDOparams["RSDO"] != None: - RSDO_cobid = SDOparams["RSDO"] - else: - RSDO_cobid = 0x600 + nodeid - - if SDOparams["TSDO"] != None: - TSDO_cobid = SDOparams["TSDO"] - else: - TSDO_cobid = 0x580 + nodeid - - masternode.SetEntry(SdoClient_index[0], 0x01, RSDO_cobid) - masternode.SetEntry(SdoClient_index[0], 0x02, TSDO_cobid) - masternode.SetEntry(SdoClient_index[0], 0x03, nodeid) - - # Configure Master's PDO parameters entries and set cobid, transmit type - for cobid, pdo_infos in DictCobID.items(): - current_idx = CurrentPDOParamsIdx[pdo_infos["type"]] - addinglist = [current_idx, current_idx + 0x200] - manager.ManageEntriesOfCurrent(addinglist, [], masternode) - masternode.SetEntry(current_idx, 0x01, cobid) - masternode.SetEntry(current_idx, 0x02, DefaultTransmitTypeMaster) - if len(pdo_infos["mapping"]) > 2: - manager.AddSubentriesToCurrent(current_idx + 0x200, len(pdo_infos["mapping"]) - 2, masternode) - - # Create Master's PDO mapping - for subindex, variable in enumerate(pdo_infos["mapping"]): - if subindex == 0: - continue - new_index = False - - if type(variable) != IntType: - - typeidx, varname = variable - indexname = \ - DictNameVariable[DictLocations[variable[1]]["pdotype"]][0] + \ - DictLocations[variable[1]]["sizelocation"] + \ - '_'.join(map(str,current_location)) + \ - "_" + \ - str(DictLocations[variable[1]]["nodeid"]) - mapvariableidx = DictNameVariable[DictLocations[variable[1]]["pdotype"]][1] + DictNameVariable[DictLocations[variable[1]]["sizelocation"]] * DictNameVariable["increment"] - - #indexname = DictNameVariable[DictLocations[variable[1]]["pdotype"]][0] + DictLocations[variable[1]]["sizelocation"] + str(DictLocations[variable[1]]["prefix"]) + "_" + str(DictLocations[variable[1]]["nodeid"]) - #mapvariableidx = DictNameVariable[DictLocations[variable[1]]["pdotype"]][1] + DictNameVariable[DictLocations[variable[1]]["sizelocation"]] * DictNameVariable["increment"] - - if not masternode.IsEntry(mapvariableidx): - manager.AddMapVariableToCurrent(mapvariableidx, indexname, 3, 1, masternode) - new_index = True - nbsubentries = masternode.GetEntry(mapvariableidx, 0x00) - else: - nbsubentries = masternode.GetEntry(mapvariableidx, 0x00) - mapvariableidxbase = mapvariableidx - while mapvariableidx < (mapvariableidxbase + 0x1FF) and nbsubentries == 0xFF: - mapvariableidx += 0x800 - if not manager.IsCurrentEntry(mapvariableidx): - manager.AddMapVariableToCurrent(mapvariableidx, indexname, 3, 1, masternode) - new_index = True - nbsubentries = masternode.GetEntry(mapvariableidx, 0x00) - - if mapvariableidx < 0x6000: - if DictLocations[variable[1]]["bit"] != None: - subindexname = str(DictLocations[variable[1]]["index"]) + "_" + str(DictLocations[variable[1]]["subindex"]) + "_" + str(DictLocations[variable[1]]["bit"]) - else: - subindexname = str(DictLocations[variable[1]]["index"]) + "_" + str(DictLocations[variable[1]]["subindex"]) - if not new_index: - manager.AddSubentriesToCurrent(mapvariableidx, 1, masternode) - nbsubentries += 1 - masternode.SetMappingEntry(mapvariableidx, nbsubentries, values = {"name" : subindexname}) - masternode.SetMappingEntry(mapvariableidx, nbsubentries, values = {"type" : typeidx}) - - # Map Variable - typeinfos = manager.GetEntryInfos(typeidx) - if typeinfos != None: - value = (mapvariableidx << 16) + ((nbsubentries) << 8) + typeinfos["size"] - masternode.SetEntry(current_idx + 0x200, subindex, value) - else: - masternode.SetEntry(current_idx + 0x200, subindex, TrashVariableValue[variable]) - - CurrentPDOParamsIdx[pdo_infos["type"]] += 1 - #masternode.Print() - return masternode - -if __name__ == "__main__": + dcfgenerator = ConciseDCFGenerator(nodelist) + dcfgenerator.GenerateDCF(locations, current_location, sync_TPDOs) + return dcfgenerator.GetMasterNode() + +if __name__ == "__main__": from nodemanager import * from nodelist import * import sys