objdictgen/config_utils.py

   1 #!/usr/bin/env python
   2 # -*- coding: utf-8 -*-
   3
   4 #This file is part of CanFestival, a library implementing CanOpen Stack.
   5 #
   6 #Copyright (C): Edouard TISSERANT, Francis DUPIN and Laurent BESSARD
   7 #
   8 #See COPYING file for copyrights details.
   9 #
  10 #This library is free software; you can redistribute it and/or
  11 #modify it under the terms of the GNU Lesser General Public
  12 #License as published by the Free Software Foundation; either
  13 #version 2.1 of the License, or (at your option) any later version.
  14 #
  15 #This library is distributed in the hope that it will be useful,
  16 #but WITHOUT ANY WARRANTY; without even the implied warranty of
  17 #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  18 #Lesser General Public License for more details.
  19 #
  20 #You should have received a copy of the GNU Lesser General Public
  21 #License along with this library; if not, write to the Free Software
  22 #Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  23
  24 from types import *
  25
  26 DicoTypes = {"BOOL":0x01, "SINT":0x02, "INT":0x03,"DINT":0x04,"LINT":0x10,
  27              "USINT":0x05,"UINT":0x06,"UDINT":0x07,"ULINT":0x1B,"REAL":0x08,
  28              "LREAL":0x11,"STRING":0x09,"BYTE":0x02,"WORD":0x03,"DWORD":0x04,
  29              "LWORD":0x1B,"WSTRING":0x0B}
  30
  31 DictLocations = {}
  32 DictCobID = {}
  33 DictLocationsNotMapped = {}
  34 ListCobIDAvailable = []
  35 SlavesPdoNumber = {}
  36
  37 # Constants for PDO types
  38 RPDO = 1
  39 TPDO = 2
  40 SlavePDOType = {"I" : TPDO, "Q" : RPDO}
  41 InvertPDOType = {RPDO : TPDO, TPDO : RPDO}
  42
  43 DefaultTransmitType = 0x01
  44
  45 GenerateMasterMapping = lambda x:[None] + [(loc_infos["type"], name) for name, loc_infos in x]
  46
  47 TrashVariableSizes = {1 : 0x01, 8 : 0x05, 16 : 0x06, 32 : 0x07, 64 : 0x1B}
  48
  49
  50 def GetSlavePDOIndexes(slave, type, parameters = False):
  51     indexes = []
  52     if type & RPDO:
  53         indexes.extend([idx for idx in slave.GetIndexes() if 0x1400 <= idx <= 0x15FF])
  54     if type & TPDO:
  55         indexes.extend([idx for idx in slave.GetIndexes() if 0x1800 <= idx <= 0x19FF])
  56     if not parameters:
  57         return [idx + 0x200 for idx in indexes]
  58     else:
  59         return indexes
  60
  61
  62 def LE_to_BE(value, size): # Convert Little Endian to Big Endian
  63     data = ("%" + str(size * 2) + "." + str(size * 2) + "X") % value
  64     list_car = [data[i:i+2] for i in xrange(0, len(data), 2)]
  65     list_car.reverse()
  66     return "".join([chr(int(car, 16)) for car in list_car])
  67
  68
  69
  70 def SearchSlavePDOMapping(loc_infos, slave): # Search the TPDO or RPDO mapping where location is defined on the slave
  71     typeinfos = slave.GetEntryInfos(loc_infos["type"])
  72     model = (loc_infos["index"] << 16) + (loc_infos["subindex"] << 8) + typeinfos["size"]
  73     slavePDOidxlist = GetSlavePDOIndexes(slave, loc_infos["pdotype"])
  74
  75     for PDOidx in slavePDOidxlist:
  76         values = slave.GetEntry(PDOidx)
  77
  78         for subindex, mapping in enumerate(values):
  79             if subindex != 0 and mapping == model:
  80                 return PDOidx, subindex
  81     return None
  82
  83 def GenerateMappingDCF(cobid, idx, pdomapping, mapped): # Build concise DCF
  84
  85     # Create entry for RPDO or TPDO parameters and Disable PDO
  86     dcfdata = LE_to_BE(idx, 2) + LE_to_BE(0x01, 1) + LE_to_BE(0x04, 4) + LE_to_BE((0x80000000 + cobid), 4)
  87
  88     # Set Transmit type synchrone
  89     dcfdata += LE_to_BE(idx, 2) + LE_to_BE(0x02, 1) + LE_to_BE(0x01, 4) + LE_to_BE(DefaultTransmitType, 1)
  90
  91     # Re-Enable PDO
  92     #         ---- INDEX -----   --- SUBINDEX ----   ----- SIZE ------   ------ DATA ------
  93     dcfdata += LE_to_BE(idx, 2) + LE_to_BE(0x01, 1) + LE_to_BE(0x04, 4) + LE_to_BE(0x00000000 + cobid, 4)
  94
  95     nbparams = 3
  96     if mapped == False and pdomapping != None:
  97     # Map Variables
  98         for subindex, (name, loc_infos) in enumerate(pdomapping):
  99             value = (loc_infos["index"] << 16) + (loc_infos["subindex"] << 8) + loc_infos["size"]
 100             dcfdata += LE_to_BE(idx + 0x200, 2) + LE_to_BE(subindex + 1, 1) + LE_to_BE(loc_infos["size"] >> 3, 4) + LE_to_BE(value, loc_infos["size"] >> 3)
 101             nbparams += 1
 102     return dcfdata, nbparams
 103
 104
 105
 106 def GetNewCobID(nodeid, type): # Return a cobid not used
 107     global ListCobIDAvailable, SlavesPdoNumber
 108
 109     if len(ListCobIDAvailable) == 0:
 110         return None
 111
 112     nbSlavePDO = SlavesPdoNumber[nodeid][type]
 113     if type == RPDO:
 114         if nbSlavePDO < 4:
 115             # For the fourth PDO -> cobid = 0x200 + ( numPdo parameters * 0x100) + nodeid
 116             newcobid = (0x200 + nbSlavePDO * 0x100 + nodeid)
 117             if newcobid in ListCobIDAvailable:
 118                 ListCobIDAvailable.remove(newcobid)
 119                 return newcobid, 0x1400 + nbSlavePDO
 120         return ListCobIDAvailable.pop(0), 0x1400 + nbSlavePDO
 121
 122     elif type == TPDO:
 123         if nbSlavePDO < 4:
 124             # For the fourth PDO -> cobid = 0x180 + (numPdo parameters * 0x100) + nodeid
 125             newcobid = (0x180 + nbSlavePDO * 0x100 + nodeid)
 126             if newcobid in ListCobIDAvailable:
 127                 ListCobIDAvailable.remove(newcobid)
 128                 return newcobid, 0x1800 + nbSlavePDO
 129         return ListCobIDAvailable.pop(0), 0x1800 + nbSlavePDO
 130
 131     for number in xrange(4):
 132         if type == RPDO:
 133             # For the fourth PDO -> cobid = 0x200 + ( numPdo * 0x100) + nodeid
 134             newcobid = (0x200 + number * 0x100 + nodeid)
 135         elif type == TPDO:
 136             # For the fourth PDO -> cobid = 0x180 + (numPdo * 0x100) + nodeid
 137             newcobid = (0x180 + number * 0x100 + nodeid)
 138         else:
 139             return None
 140         if newcobid in ListCobIDAvailable:
 141             ListCobIDAvailable.remove(newcobid)
 142             return newcobid
 143     return ListCobIDAvailable.pop(0)
 144
 145
 146 def GenerateConciseDCF(locations, busname, nodelist):
 147     global DictLocations, DictCobID, DictLocationsNotMapped, ListCobIDAvailable, SlavesPdoNumber
 148
 149     DictLocations = {}
 150     DictCobID = {}
 151     DictLocationsNotMapped = {}
 152     DictSDOparams = {}
 153     ListCobIDAvailable = range(0x180, 0x580)
 154     SlavesPdoNumber = {}
 155     DictNameVariable = { "" : 1, "X": 2, "B": 3, "W": 4, "D": 5, "L": 6, "increment": 0x100, 1:("__I", 0x2000), 2:("__Q", 0x4000)}
 156
 157     # Master Node initialisation
 158     manager = nodelist.Manager
 159     manager.AddSubentriesToCurrent(0x1F22, 127)
 160
 161     # Adding trash mappable variables for unused mapped datas
 162     idxTrashVariables = 0x2000 + manager.GetCurrentNodeID()
 163     TrashVariableValue = {}
 164     manager.AddMapVariableToCurrent(idxTrashVariables, "trashvariables", 3, len(TrashVariableSizes))
 165     for subidx, (size, typeidx) in enumerate(TrashVariableSizes.items()):
 166         manager.SetCurrentEntry(idxTrashVariables, subidx + 1, "TRASH%d" % size, "name", None)
 167         manager.SetCurrentEntry(idxTrashVariables, subidx + 1, typeidx, "type", None)
 168         TrashVariableValue[size] = (idxTrashVariables << 16) + ((subidx + 1) << 8) + size
 169
 170     # Extract Master Node current empty mapping index
 171     masternode = manager.GetCurrentNode()
 172     CurrentPDOParamsIdx = {RPDO : 0x1400 + len(GetSlavePDOIndexes(masternode, RPDO)),
 173                            TPDO : 0x1800 + len(GetSlavePDOIndexes(masternode, TPDO))}
 174
 175
 176     # Get list of all Slave's CobID and Slave's default SDO server parameters
 177     for nodeid, nodeinfos in nodelist.SlaveNodes.items():
 178         node = nodeinfos["Node"]
 179         node.SetNodeID(nodeid)
 180         DictSDOparams[nodeid] = {"RSDO" : node.GetEntry(0x1200,0x01), "TSDO" : node.GetEntry(0x1200,0x02)}
 181         slaveRpdoIndexes = GetSlavePDOIndexes(node, RPDO, True)
 182         slaveTpdoIndexes = GetSlavePDOIndexes(node, TPDO, True)
 183         SlavesPdoNumber[nodeid] = {RPDO : len(slaveRpdoIndexes), TPDO : len(slaveTpdoIndexes)}
 184         for PdoIdx in slaveRpdoIndexes + slaveTpdoIndexes:
 185             pdo_cobid = node.GetEntry(PdoIdx, 0x01)
 186             if pdo_cobid > 0x600 :
 187                 pdo_cobid -= 0x80000000
 188             if pdo_cobid in ListCobIDAvailable:
 189                 ListCobIDAvailable.remove(pdo_cobid)
 190
 191     # Get list of locations check if exists and mappables -> put them in DictLocations
 192     for locationtype, name in locations:
 193         if name in DictLocations.keys():
 194             if DictLocations[name]["type"] != DicoTypes[locationtype]:
 195                 raise ValueError, "Conflict type for location \"%s\"" % name
 196         else:
 197             loc = [i for i in name.split("_") if len(i) > 0]
 198             if len(loc) not in (4, 5):
 199                 continue
 200
 201             prefix = loc[0][0]
 202
 203             # Extract and check busname
 204             if loc[0][1].isdigit():
 205                 sizelocation = ""
 206                 busnamelocation = int(loc[0][1:])
 207             else:
 208                 sizelocation = loc[0][1]
 209                 busnamelocation = int(loc[0][2:])
 210             if busnamelocation != busname:
 211                 continue # A ne pas remplacer par un message d'erreur
 212
 213             # Extract and check nodeid
 214             nodeid = int(loc[1])
 215             if nodeid not in nodelist.SlaveNodes.keys():
 216                 continue
 217             node = nodelist.SlaveNodes[nodeid]["Node"]
 218
 219             # Extract and check index and subindex
 220             index = int(loc[2])
 221             subindex = int(loc[3])
 222             if not node.IsEntry(index, subindex):
 223                 continue
 224             subentry_infos = node.GetSubentryInfos(index, subindex)
 225
 226             if subentry_infos and subentry_infos["pdo"]:
 227                 if sizelocation == "X" and len(loc) > 4:
 228                     numbit = loc[4]
 229                 elif sizelocation != "X" and len(loc) > 4:
 230                     continue
 231                 else:
 232                     numbit = None
 233
 234                 locationtype = DicoTypes[locationtype]
 235                 entryinfos = node.GetSubentryInfos(index, subindex)
 236                 if entryinfos["type"] != locationtype:
 237                     raise ValueError, "Invalid type for location \"%s\"" % name
 238
 239                 typeinfos = node.GetEntryInfos(locationtype)
 240                 DictLocations[name] = {"type":locationtype, "pdotype":SlavePDOType[prefix],
 241                                        "nodeid": nodeid, "index": index,"subindex": subindex,
 242                                        "bit": numbit, "size": typeinfos["size"], "busname": busname, "sizelocation": sizelocation}
 243
 244     # Create DictCobID with variables already mapped and add them in DictValidLocations
 245     for name, locationinfos in DictLocations.items():
 246         node = nodelist.SlaveNodes[locationinfos["nodeid"]]["Node"]
 247         result = SearchSlavePDOMapping(locationinfos, node)
 248         if result != None:
 249             index, subindex = result
 250             cobid = nodelist.GetSlaveNodeEntry(locationinfos["nodeid"], index - 0x200, 1)
 251             if cobid not in DictCobID.keys():
 252                 DefaultNodeTransmitType = node.GetEntry(index - 0x200, 2)
 253                 if not 1 <= DefaultNodeTransmitType <= 240:
 254                     result = GenerateMappingDCF(cobid, index - 0x200, None, True)
 255                     data, nbaddedparams = GenerateMappingDCF(cobid, index - 0x200, None, True)
 256                     nodeDCF = nodelist.GetMasterNodeEntry(0x1F22, locationinfos["nodeid"])
 257                     if nodeDCF != None and nodeDCF != '':
 258                         tmpnbparams = [i for i in nodeDCF[:4]]
 259                         tmpnbparams.reverse()
 260                         nbparams = int(''.join(["%2.2x"%ord(i) for i in tmpnbparams]), 16)
 261                         dataparams = nodeDCF[4:]
 262                     else:
 263                         nbparams = 0
 264                         dataparams = ""
 265                     dataparams += data
 266                     nbparams += nbaddedparams
 267                     dcf = LE_to_BE(nbparams, 0x04) + dataparams
 268                     manager.SetCurrentEntry(0x1F22, locationinfos["nodeid"], dcf, "value", None)
 269
 270                 mapping = [None]
 271                 values = node.GetEntry(index)
 272                 for value in values[1:]:
 273                     mapping.append(value % 0x100)
 274                 DictCobID[cobid] = {"type" : InvertPDOType[locationinfos["pdotype"]], "mapping" : mapping}
 275
 276             DictCobID[cobid]["mapping"][subindex] = (locationinfos["type"], name)
 277
 278         else:
 279             if locationinfos["nodeid"] not in DictLocationsNotMapped.keys():
 280                 DictLocationsNotMapped[locationinfos["nodeid"]] = {TPDO : [], RPDO : []}
 281             DictLocationsNotMapped[locationinfos["nodeid"]][locationinfos["pdotype"]].append((name, locationinfos))
 282
 283     # Check Master Pdo parameters for cobid already used and remove it in ListCobIDAvailable
 284     ListPdoParams = [idx for idx in masternode.GetIndexes() if 0x1400 <= idx <= 0x15FF or  0x1800 <= idx <= 0x19FF]
 285     for idx in ListPdoParams:
 286         cobid = manager.GetCurrentEntry(idx, 0x01)
 287         if cobid not in DictCobID.keys():
 288             ListCobIDAvailable.pop(cobid)
 289
 290
 291     #-------------------------------------------------------------------------------
 292     #                         Build concise DCF for the others locations
 293     #-------------------------------------------------------------------------------
 294
 295     for nodeid, locations in DictLocationsNotMapped.items():
 296
 297         # Get current concise DCF
 298         node = nodelist.SlaveNodes[nodeid]["Node"]
 299         nodeDCF = nodelist.GetMasterNodeEntry(0x1F22, nodeid)
 300
 301         if nodeDCF != None and nodeDCF != '':
 302             tmpnbparams = [i for i in nodeDCF[:4]]
 303             tmpnbparams.reverse()
 304             nbparams = int(''.join(["%2.2x"%ord(i) for i in tmpnbparams]), 16)
 305             dataparams = nodeDCF[4:]
 306         else:
 307             nbparams = 0
 308             dataparams = ""
 309
 310         for pdotype in (TPDO, RPDO):
 311             pdosize = 0
 312             pdomapping = []
 313             for name, loc_infos in locations[pdotype]:
 314                 pdosize += loc_infos["size"]
 315                 # If pdo's size > 64 bits
 316                 if pdosize > 64:
 317                     result = GetNewCobID(nodeid, pdotype)
 318                     if result:
 319                         SlavesPdoNumber[nodeid][pdotype] += 1
 320                         new_cobid, new_idx = result
 321                         data, nbaddedparams = GenerateMappingDCF(new_cobid, new_idx, pdomapping, False)
 322                         dataparams += data
 323                         nbparams += nbaddedparams
 324                         DictCobID[new_cobid] = {"type" : InvertPDOType[pdotype], "mapping" : GenerateMasterMapping(pdomapping)}
 325                     pdosize = loc_infos["size"]
 326                     pdomapping = [(name, loc_infos)]
 327                 else:
 328                     pdomapping.append((name, loc_infos))
 329             if len(pdomapping) > 0:
 330                 result = GetNewCobID(nodeid, pdotype)
 331                 if result:
 332                     SlavesPdoNumber[nodeid][pdotype] += 1
 333                     new_cobid, new_idx = result
 334                     data, nbaddedparams = GenerateMappingDCF(new_cobid, new_idx, pdomapping, False)
 335                     dataparams += data
 336                     nbparams += nbaddedparams
 337                     DictCobID[new_cobid] = {"type" : InvertPDOType[pdotype], "mapping" : GenerateMasterMapping(pdomapping)}
 338
 339         dcf = LE_to_BE(nbparams, 0x04) + dataparams
 340         manager.SetCurrentEntry(0x1F22, nodeid, dcf, "value", None)
 341
 342
 343     #-------------------------------------------------------------------------------
 344     #                         Master Node Configuration
 345     #-------------------------------------------------------------------------------
 346
 347
 348     # Configure Master's SDO parameters entries
 349     for nodeid, SDOparams in DictSDOparams.items():
 350         SdoClient_index = [0x1280 + nodeid]
 351         manager.ManageEntriesOfCurrent(SdoClient_index,[])
 352         if SDOparams["RSDO"] != None:
 353             RSDO_cobid = SDOparams["RSDO"]
 354         else:
 355             RSDO_cobid = 0x600 + nodeid
 356
 357         if SDOparams["TSDO"] != None:
 358             TSDO_cobid = SDOparams["TSDO"]
 359         else:
 360             TSDO_cobid = 0x580 + nodeid
 361
 362         manager.SetCurrentEntry(SdoClient_index[0], 0x01, RSDO_cobid, "value", None)
 363         manager.SetCurrentEntry(SdoClient_index[0], 0x02, TSDO_cobid, "value", None)
 364         manager.SetCurrentEntry(SdoClient_index[0], 0x03, nodeid, "value", None)
 365
 366     # Configure Master's PDO parameters entries and set cobid, transmit type
 367     for cobid, pdo_infos in DictCobID.items():
 368         current_idx = CurrentPDOParamsIdx[pdo_infos["type"]]
 369         addinglist = [current_idx, current_idx + 0x200]
 370         manager.ManageEntriesOfCurrent(addinglist, [])
 371         manager.SetCurrentEntry(current_idx, 0x01, cobid, "value", None)
 372         manager.SetCurrentEntry(current_idx, 0x02, DefaultTransmitType, "value", None)
 373         if len(pdo_infos["mapping"]) > 2:
 374             manager.AddSubentriesToCurrent(current_idx + 0x200, len(pdo_infos["mapping"]) - 2)
 375
 376         # Create Master's PDO mapping
 377         for subindex, variable in enumerate(pdo_infos["mapping"]):
 378             if subindex == 0:
 379                 continue
 380             new_index = False
 381
 382             if type(variable) != IntType:
 383
 384                 typeidx, varname = variable
 385                 indexname = DictNameVariable[DictLocations[variable[1]]["pdotype"]][0] + DictLocations[variable[1]]["sizelocation"] + str(DictLocations[variable[1]]["busname"]) + "_" + str(DictLocations[variable[1]]["nodeid"])
 386                 mapvariableidx = DictNameVariable[DictLocations[variable[1]]["pdotype"]][1] +  DictNameVariable[DictLocations[variable[1]]["sizelocation"]] * DictNameVariable["increment"]
 387
 388                 if not manager.IsCurrentEntry(mapvariableidx):
 389                     manager.AddMapVariableToCurrent(mapvariableidx, indexname, 3, 1)
 390                     new_index = True
 391                     nbsubentries = manager.GetCurrentEntry(mapvariableidx, 0x00)
 392                 else:
 393                     nbsubentries = manager.GetCurrentEntry(mapvariableidx, 0x00)
 394                     mapvariableidxbase = mapvariableidx
 395                     while mapvariableidx < (mapvariableidxbase + 0x1FF) and nbsubentries == 0xFF:
 396                         mapvariableidx += 0x800
 397                         if not manager.IsCurrentEntry(mapvariableidx):
 398                             manager.AddMapVariableToCurrent(mapvariableidx, indexname, 3, 1)
 399                             new_index = True
 400                         nbsubentries = manager.GetCurrentEntry(mapvariableidx, 0x00)
 401                 if mapvariableidx < 0x6000:
 402                     if DictLocations[variable[1]]["bit"] != None:
 403                         subindexname = "_" + str(DictLocations[variable[1]]["index"]) + "_" + str(DictLocations[variable[1]]["subindex"]) + "_" + str(DictLocations[variable[1]]["bit"])
 404                     else:
 405                         subindexname = "_" + str(DictLocations[variable[1]]["index"]) + "_" + str(DictLocations[variable[1]]["subindex"])
 406                     if not new_index:
 407                         manager.AddSubentriesToCurrent(mapvariableidx, 1)
 408                         nbsubentries += 1
 409                     manager.SetCurrentEntry(mapvariableidx, nbsubentries, subindexname, "name", None)
 410                     manager.SetCurrentEntry(mapvariableidx, nbsubentries, typeidx, "type", None)
 411
 412                     # Map Variable
 413                     typeinfos = manager.GetEntryInfos(typeidx)
 414                     if typeinfos != None:
 415                         value = (mapvariableidx << 16) + ((nbsubentries) << 8) + typeinfos["size"]
 416                         manager.SetCurrentEntry(current_idx + 0x200, subindex, value, "value", None)
 417             else:
 418                 manager.SetCurrentEntry(current_idx + 0x200, subindex, TrashVariableValue[variable], "value", None)
 419
 420         CurrentPDOParamsIdx[pdo_infos["type"]] += 1
 421
 422 if __name__ == "__main__":
 423     from nodemanager import *
 424     from nodelist import *
 425     import sys
 426
 427     manager = NodeManager(sys.path[0])
 428     nodelist = NodeList(manager)
 429     result = nodelist.LoadProject("/home/deobox/Desktop/TestMapping")
 430
 431     if result != None:
 432         print result
 433     else:
 434         print "MasterNode :"
 435         manager.CurrentNode.Print()
 436         for nodeid, node in nodelist.SlaveNodes.items():
 437             print "SlaveNode name=%s id=0x%2.2X :"%(node["Name"], nodeid)
 438             node["Node"].Print()
 439
 440     #filepath = "/home/deobox/beremiz/test_nodelist/listlocations.txt"
 441     filepath = "/home/deobox/Desktop/TestMapping/listlocations.txt"
 442
 443     file = open(filepath,'r')
 444     locations = [location.split(' ') for location in [line.strip() for line in file.readlines() if len(line) > 0]]
 445     file.close()
 446     GenerateConciseDCF(locations, 32, nodelist)
 447     print "MasterNode :"
 448     manager.GetCurrentNode().Print()