27

# Translation between IEC types and Can Open types

    33

DictNameVariable = { "" : 1, "X": 2, "B": 3, "W": 4, "D": 5, "L": 6, "increment": 0x100, 1:("__I", 0x2000), 2:("__Q", 0x4000)}

    45

def LE_to_BE(value, size):

    46

    """

    47

    Convert Little Endian to Big Endian

    48

    @param value: value expressed in integer

    49

    @param size: number of bytes generated

    50

    @return: a string containing the value converted

    51

    """

    52

    53

    data = ("%" + str(size * 2) + "." + str(size * 2) + "X") % value

    54

    list_car = [data[i:i+2] for i in xrange(0, len(data), 2)]

    55

    list_car.reverse()

    56

    return "".join([chr(int(car, 16)) for car in list_car])

    57

    58

def GetNodePDOIndexes(node, type, parameters = False):

    59

    """

    60

    Find the PDO indexes of a node

    61

    @param node: node 

    62

    @param type: type of PDO searched (RPDO or TPDO or both)

    63

    @param parameters: indicate which indexes are expected (PDO paramaters : True or PDO mappings : False)

    64

    @return: a list of indexes found

    65

    """

    66

    69

        indexes.extend([idx for idx in node.GetIndexes() if 0x1400 <= idx <= 0x15FF])

    71

        indexes.extend([idx for idx in node.GetIndexes() if 0x1800 <= idx <= 0x19FF])

    77

def SearchNodePDOMapping(loc_infos, node):

    78

    """

    79

    Find the PDO indexes of a node

    80

    @param node: node 

    81

    @param type: type of PDO searched (RPDO or TPDO or both)

    82

    @param parameters: indicate which indexes are expected (PDO paramaters : True or PDO mappings : False)

    83

    @return: a list of indexes found

    84

    """

    85

    86

    typeinfos = node.GetEntryInfos(loc_infos["type"])

    88

    89

    for PDOidx in GetNodePDOIndexes(node, loc_infos["pdotype"]):

    90

        values = node.GetEntry(PDOidx)

    97

class ConciseDCFGenerator:

    98

    99

    def __init__(self, nodelist):

   100

        # Dictionary of location informations classed by name

   101

        self.DictLocations = {}

   102

        # Dictionary of location informations classed by name

   103

        self.DictCobID = {}

   104

        # Dictionary of location that have not been mapped yet

   105

        self.DictLocationsNotMapped = {}

   106

        # List of COB IDs available

   107

        self.ListCobIDAvailable = range(0x180, 0x580)

   108

        self.SlavesPdoNumber = {}

   109

        # Dictionary of mapping value where unexpected variables are stored

   110

        TrashVariableValue = {}

   111

   112

        self.NodeList = nodelist

   113

        self.Manager = self.NodeList.Manager

   114

        self.MasterNode = self.Manager.GetCurrentNodeCopy()

   115

        self.PrepareMasterNode()

   116

   117

    def RemoveUsedNodeCobId(self, node):

   118

        """

   119

        Remove all PDO COB ID used by the given node from the list of available COB ID

   120

        @param node: node

   121

        @return: a tuple of number of RPDO and TPDO for the node

   122

        """

   123

   124

        # Get list of all node TPDO and RPDO indexes

   125

        nodeRpdoIndexes = GetNodePDOIndexes(node, RPDO, True)

   126

        nodeTpdoIndexes = GetNodePDOIndexes(node, TPDO, True)

   127

   128

        # Mark all the COB ID of the node already mapped PDO as not available

   129

        for PdoIdx in nodeRpdoIndexes + nodeTpdoIndexes:

   130

            pdo_cobid = node.GetEntry(PdoIdx, 0x01)

   131

            # Extract COB ID, if PDO isn't active

   132

            if pdo_cobid > 0x600 :

   133

                pdo_cobid -= 0x80000000

   134

            # Remove COB ID from the list of available COB ID

   135

            if pdo_cobid in self.ListCobIDAvailable:

   136

                self.ListCobIDAvailable.remove(pdo_cobid)

   137

   138

        return len(nodeRpdoIndexes), len(nodeTpdoIndexes)

   139

   140

    def PrepareMasterNode(self):

   141

        """

   142

        Add mandatory entries for DCF generation into MasterNode.

   143

        """

   144

   145

        # Adding DCF entry into Master node

   146

        if not self.MasterNode.IsEntry(0x1F22):

   147

            self.MasterNode.AddEntry(0x1F22, 1, "")

   148

        self.Manager.AddSubentriesToCurrent(0x1F22, 127, masternode)

   149

   150

        # Adding trash mappable variables for unused mapped datas

   151

        idxTrashVariables = 0x2000 + masternode.GetNodeID()

   152

        # Add an entry for storing unexpected all variable

   153

        self.Manager.AddMapVariableToCurrent(idxTrashVariables, "trashvariables", 3, len(TrashVariableSizes), self.MasterNode)

   154

        for subidx, (size, typeidx) in enumerate(TrashVariableSizes.items()):

   155

            # Add a subentry for storing unexpected variable of this size

   156

            self.Manager.SetCurrentEntry(idxTrashVariables, subidx + 1, "TRASH%d" % size, "name", None, self.MasterNode)

   157

            self.Manager.SetCurrentEntry(idxTrashVariables, subidx + 1, typeidx, "type", None, self.MasterNode)

   158

            # Store the mapping value for this entry

   159

            self.TrashVariableValue[size] = (idxTrashVariables << 16) + ((subidx + 1) << 8) + size

   160

   161

        RPDOnumber, TPDOnumber = self.RemoveUsedNodeCobId(self.MasterNode)

   162

   163

        # Store the indexes of the first RPDO and TPDO available for MasterNode

   164

        self.CurrentPDOParamsIdx = {RPDO : 0x1400 + RPDOnumber, TPDO : 0x1800 + TPDOnumber}

   165

   166

        # Prepare MasterNode with all nodelist slaves

   167

        for nodeid, nodeinfos in self.NodeList.SlaveNodes.items():

   168

            node = nodeinfos["Node"]

   169

            node.SetNodeID(nodeid)

   170

   171

            RPDOnumber, TPDOnumber = self.RemoveUsedNodeCobId(node)

   172

   173

            # Store the number of TPDO and RPDO for this node

   174

            self.SlavesPdoNumber[nodeid] = {RPDO : RPDOnumber, TPDO : TPDOnumber}

   175

   176

            # Get Slave's default SDO server parameters

   177

            RSDO_cobid = node.GetEntry(0x1200,0x01)

   178

            if not RSDO_cobid:

   179

                RSDO_cobid = 0x600 + nodeid

   180

            TSDO_cobid = node.GetEntry(0x1200,0x02)

   181

            if not TSDO_cobid:

   182

                TSDO_cobid = 0x580 + nodeid

   183

   184

            # Configure Master's SDO parameters entries

   185

            self.Manager.ManageEntriesOfCurrent([0x1280 + nodeid], [], self.MasterNode)

   186

            self.MasterNode.SetEntry(0x1280 + nodeid, 0x01, RSDO_cobid)

   187

            self.MasterNode.SetEntry(0x1280 + nodeid, 0x02, TSDO_cobid)

   188

            self.MasterNode.SetEntry(0x1280 + nodeid, 0x03, nodeid)        

   189

   190

    def GetMasterNode(self):

   191

        """

   192

        Return MasterNode.

   193

        """

   194

        return self.MasterNode

   195

   196

    # Build concise DCF

   197

    def GenerateMappingDCF(cobid, idx, pdomapping, mapped):

   198

   199

        # Create entry for RPDO or TPDO parameters and Disable PDO

   200

        dcfdata = LE_to_BE(idx, 2) + LE_to_BE(0x01, 1) + LE_to_BE(0x04, 4) + LE_to_BE((0x80000000 + cobid), 4)

   201

        # Set Transmit type synchrone

   202

        dcfdata += LE_to_BE(idx, 2) + LE_to_BE(0x02, 1) + LE_to_BE(0x01, 4) + LE_to_BE(DefaultTransmitTypeSlave, 1)

   203

        # Re-Enable PDO

   204

        #         ---- INDEX -----   --- SUBINDEX ----   ----- SIZE ------   ------ DATA ------

   205

        dcfdata += LE_to_BE(idx, 2) + LE_to_BE(0x01, 1) + LE_to_BE(0x04, 4) + LE_to_BE(0x00000000 + cobid, 4)

   206

        nbparams = 3

   207

        if mapped == False and pdomapping != None:

   208

        # Map Variables

   209

            for subindex, (name, loc_infos) in enumerate(pdomapping):

   210

                value = (loc_infos["index"] << 16) + (loc_infos["subindex"] << 8) + loc_infos["size"]

   211

                dcfdata += LE_to_BE(idx + 0x200, 2) + LE_to_BE(subindex + 1, 1) + LE_to_BE(0x04, 4) + LE_to_BE(value, 4)

   212

                nbparams += 1

   213

        return dcfdata, nbparams

   214

   215

    # Return a cobid not used

   216

    def GetNewCobID(self, nodeid, type):

   217

        """

   218

        Select a COB ID from the list of those available

   219

        @param nodeid: id of the slave node

   220

        @param type: type of PDO (RPDO or TPDO)

   221

        @return: a tuple of the COD ID and PDO index or None

   222

        """

   223

        # Verify that there is still some cobid available

   224

        if len(self.ListCobIDAvailable) == 0:

   225

            return None

   226

   227

        # Get the number of PDO of the type given for the node

   228

        nbSlavePDO = self.SlavesPdoNumber[nodeid][type]

   229

        if type == RPDO:

   230

            if nbSlavePDO < 4:

   231

                # For the four first RPDO -> cobid = 0x200 + ( numPdo parameters * 0x100) + nodeid

   232

                newcobid = (0x200 + nbSlavePDO * 0x100 + nodeid)

   233

                # Return calculated cobid if it's still available

   234

                if newcobid in self.ListCobIDAvailable:

   235

                    self.ListCobIDAvailable.remove(newcobid)

   236

                    return newcobid, 0x1400 + nbSlavePDO

   237

            # Return the first cobid available if no cobid found

   238

            return self.ListCobIDAvailable.pop(0), 0x1400 + nbSlavePDO

   239

   240

        elif type == TPDO:

   241

            if nbSlavePDO < 4:

   242

                # For the four first TPDO -> cobid = 0x180 + ( numPdo parameters * 0x100) + nodeid

   243

                newcobid = (0x180 + nbSlavePDO * 0x100 + nodeid)

   244

                # Return calculated cobid if it's still available

   245

                if newcobid in self.ListCobIDAvailable:

   246

                    self.ListCobIDAvailable.remove(newcobid)

   247

                    return newcobid, 0x1800 + nbSlavePDO

   248

            # Return the first cobid available if no cobid found

   249

            return self.ListCobIDAvailable.pop(0), 0x1800 + nbSlavePDO

   250

   252

   253

    def GenerateDCF(self, locations, current_location, sync_TPDOs):

   254

        """

   255

        Generate Concise DCF of MasterNode for the locations list given

   256

        @param locations: list of locations to be mapped

   257

        @param current_location: tuple of the located prefixes not to be considered

   258

        @param sync_TPDOs: indicate if TPDO must be synchronous

   259

        """

   260

   261

        # Get list of locations check if exists and mappables -> put them in DictLocations

   262

        for location in locations:

   263

            COlocationtype = DicoTypes[location["IEC_TYPE"]]

   264

            name = location["NAME"]

   265

            if name in DictLocations:

   266

                if DictLocations[name]["type"] != COlocationtype:

   267

                    raise ValueError, "Conflict type for location \"%s\"" % name 

   268

            else:

   269

                # Get only the part of the location that concern this node

   270

                loc = location["LOC"][len(current_location):]

   271

                # loc correspond to (ID, INDEX, SUBINDEX [,BIT])

   272

                if len(loc) not in (3, 4):

   273

                    raise ValueError, "Bad location size : %s"%str(loc)

   274

   275

                direction = location["DIR"]

   276

   277

                sizelocation = location["SIZE"]

   278

   279

                # Extract and check nodeid

   280

                nodeid, index, subindex = loc[:3]

   281

   282

                # Check Id is in slave node list

   283

                if nodeid not in nodelist.SlaveNodes.keys():

   284

                    raise ValueError, "Non existing node ID : %d (variable %s)" % (nodeid,name)

   285

   286

                # Get the model for this node (made from EDS)

   287

                node = nodelist.SlaveNodes[nodeid]["Node"]

   288

   289

                # Extract and check index and subindex

   290

                if not node.IsEntry(index, subindex):

   291

                    raise ValueError, "No such index/subindex (%x,%x) in ID : %d (variable %s)" % (index,subindex,nodeid,name)

   292

   293

                #Get the entry info

   294

                subentry_infos = node.GetSubentryInfos(index, subindex)

   295

   296

                # If a PDO mappable

   297

                if subentry_infos and subentry_infos["pdo"]:

   298

                    if sizelocation == "X" and len(loc) > 3:

   299

                        numbit = loc[4]

   300

                    elif sizelocation != "X" and len(loc) > 3:

   301

                        raise ValueError, "Cannot set bit offset for non bool '%s' variable (ID:%d,Idx:%x,sIdx:%x))" % (name,nodeid,index,subindex)

   302

                    else:

   303

                        numbit = None

   304

   305

                    entryinfos = node.GetSubentryInfos(index, subindex)

   306

                    if entryinfos["type"] != COlocationtype:

   307

                        raise ValueError, "Invalid type \"%s\"-> %d != %d  for location\"%s\"" % (location["IEC_TYPE"], COlocationtype, entryinfos["type"] , name)

   308

   309

                    typeinfos = node.GetEntryInfos(COlocationtype)

   310

                    DictLocations[name] = {"type":COlocationtype, "pdotype":SlavePDOType[direction],

   311

                                           "nodeid": nodeid, "index": index,"subindex": subindex,

   312

                                           "bit": numbit, "size": typeinfos["size"], "sizelocation": sizelocation}

   313

                else:

   314

                    raise ValueError, "Not PDO mappable variable : '%s' (ID:%d,Idx:%x,sIdx:%x))" % (name,nodeid,index,subindex)

   315

   316

        # Create DictCobID with variables already mapped and add them in DictValidLocations

   317

        for name, locationinfos in DictLocations.items():

   318

            node = self.NodeList.SlaveNodes[locationinfos["nodeid"]]["Node"]

   319

            result = SearchNodePDOMapping(locationinfos, node)

   320

            if result != None:

   321

                index, subindex = result

   322

                cobid = nodelist.GetSlaveNodeEntry(locationinfos["nodeid"], index - 0x200, 1)

   323

   324

                transmittype = nodelist.GetSlaveNodeEntry(locationinfos["nodeid"], index - 0x200, 2)

   325

   326

                if transmittype != sync_TPDOs:

   327

                    if sync_TPDOs : # Change TransmitType to ASYCHRONE

   328

                        node = nodelist.SlaveNodes[nodeid]["Node"]

   329

                        nodeDCF = masternode.GetEntry(0x1F22, nodeid)

   330

   331

                        if nodeDCF != None and nodeDCF != '':

   332

                            tmpnbparams = [i for i in nodeDCF[:4]]

   333

                            tmpnbparams.reverse()

   334

                            nbparams = int(''.join(["%2.2x"%ord(i) for i in tmpnbparams]), 16)

   335

                            dataparams = nodeDCF[4:]

   336

                        else:

   337

                            nbparams = 0

   338

                            dataparams = ""

   339

   340

                    else: # Change TransmitType to SYNCHRONE  

   341

                        pass

   342

   343

   344

                if cobid not in DictCobID.keys():

   345

                    mapping = [None]

   346

                    values = node.GetEntry(index)

   347

                    for value in values[1:]:

   348

                        mapping.append(value % 0x100)

   349

                    DictCobID[cobid] = {"type" : InvertPDOType[locationinfos["pdotype"]], "mapping" : mapping}

   350

   351

                DictCobID[cobid]["mapping"][subindex] = (locationinfos["type"], name)

   352

   353

            else:

   354

                if locationinfos["nodeid"] not in DictLocationsNotMapped.keys():

   355

                    DictLocationsNotMapped[locationinfos["nodeid"]] = {TPDO : [], RPDO : []}

   356

                DictLocationsNotMapped[locationinfos["nodeid"]][locationinfos["pdotype"]].append((name, locationinfos))

   357

   358

        #-------------------------------------------------------------------------------

   359

        #                         Build concise DCF for the others locations

   360

        #-------------------------------------------------------------------------------

   361

   362

        for nodeid, locations in DictLocationsNotMapped.items():

   363

            # Get current concise DCF

   364

            node = nodelist.SlaveNodes[nodeid]["Node"]

   365

            nodeDCF = masternode.GetEntry(0x1F22, nodeid)

   366

   367

            if nodeDCF != None and nodeDCF != '':

   368

                tmpnbparams = [i for i in nodeDCF[:4]]

   369

                tmpnbparams.reverse()

   370

                nbparams = int(''.join(["%2.2x"%ord(i) for i in tmpnbparams]), 16)

   371

                dataparams = nodeDCF[4:]

   372

            else:

   373

                nbparams = 0

   374

                dataparams = ""

   375

   376

            for pdotype in (TPDO, RPDO):

   377

                pdosize = 0

   378

                pdomapping = []

   379

                for name, loc_infos in locations[pdotype]:

   380

                    pdosize += loc_infos["size"]

   381

                    # If pdo's size > 64 bits

   382

                    if pdosize > 64:

   383

                        result = GetNewCobID(nodeid, pdotype)

   384

                        if result:

   385

                            SlavesPdoNumber[nodeid][pdotype] += 1

   386

                            new_cobid, new_idx = result

   387

                            data, nbaddedparams = GenerateMappingDCF(new_cobid, new_idx, pdomapping, False)

   388

                            dataparams += data

   389

                            nbparams += nbaddedparams

   390

                            DictCobID[new_cobid] = {"type" : InvertPDOType[pdotype], "mapping" : GenerateMasterMapping(pdomapping)}

   391

                        pdosize = loc_infos["size"]

   392

                        pdomapping = [(name, loc_infos)]

   393

                    else:

   394

                        pdomapping.append((name, loc_infos))

   395

                if len(pdomapping) > 0:

   396

                    result = GetNewCobID(nodeid, pdotype)

   397

                    if result:

   398

                        SlavesPdoNumber[nodeid][pdotype] += 1

   399

                        new_cobid, new_idx = result

   400

                        data, nbaddedparams = GenerateMappingDCF(new_cobid, new_idx, pdomapping, False)

   401

                        dataparams += data

   402

                        nbparams += nbaddedparams

   403

                        DictCobID[new_cobid] = {"type" : InvertPDOType[pdotype], "mapping" : GenerateMasterMapping(pdomapping)}

   404

   405

            dcf = LE_to_BE(nbparams, 0x04) + dataparams

   406

            masternode.SetEntry(0x1F22, nodeid, dcf)

   407

   408

   409

        #-------------------------------------------------------------------------------

   410

        #                         Master Node Configuration

   411

        #-------------------------------------------------------------------------------

   412

   413

   414

   415

        # Configure Master's PDO parameters entries and set cobid, transmit type

   416

        for cobid, pdo_infos in DictCobID.items():

   417

            current_idx = CurrentPDOParamsIdx[pdo_infos["type"]]

   418

            addinglist = [current_idx, current_idx + 0x200]

   419

            manager.ManageEntriesOfCurrent(addinglist, [], masternode)

   420

            masternode.SetEntry(current_idx, 0x01, cobid)

   421

            masternode.SetEntry(current_idx, 0x02, DefaultTransmitTypeMaster)

   422

            if len(pdo_infos["mapping"]) > 2:

   423

                manager.AddSubentriesToCurrent(current_idx + 0x200, len(pdo_infos["mapping"]) - 2, masternode)

   424

   425

            # Create Master's PDO mapping

   426

            for subindex, variable in enumerate(pdo_infos["mapping"]):

   427

                if subindex == 0:

   428

                    continue

   429

                new_index = False

   430

   431

                if type(variable) != IntType:

   432

   433

                    typeidx, varname = variable

   434

                    indexname = \

   435

                        DictNameVariable[DictLocations[variable[1]]["pdotype"]][0] + \

   436

                        DictLocations[variable[1]]["sizelocation"] + \

   437

                        '_'.join(map(str,current_location)) + \

   438

                        "_" + \

   439

                        str(DictLocations[variable[1]]["nodeid"])

   440

                    mapvariableidx = DictNameVariable[DictLocations[variable[1]]["pdotype"]][1] +  DictNameVariable[DictLocations[variable[1]]["sizelocation"]] * DictNameVariable["increment"]

   441

   442

                    #indexname = DictNameVariable[DictLocations[variable[1]]["pdotype"]][0] + DictLocations[variable[1]]["sizelocation"] + str(DictLocations[variable[1]]["prefix"]) + "_" + str(DictLocations[variable[1]]["nodeid"])

   443

                    #mapvariableidx = DictNameVariable[DictLocations[variable[1]]["pdotype"]][1] +  DictNameVariable[DictLocations[variable[1]]["sizelocation"]] * DictNameVariable["increment"]

   444

   445

                    if not masternode.IsEntry(mapvariableidx):

   446

                        manager.AddMapVariableToCurrent(mapvariableidx, indexname, 3, 1, masternode)

   447

                        new_index = True

   448

                        nbsubentries = masternode.GetEntry(mapvariableidx, 0x00)

   449

                    else:

   450

                        nbsubentries = masternode.GetEntry(mapvariableidx, 0x00)

   451

                        mapvariableidxbase = mapvariableidx 

   452

                        while mapvariableidx < (mapvariableidxbase + 0x1FF) and nbsubentries == 0xFF:

   453

                            mapvariableidx += 0x800

   454

                            if not manager.IsCurrentEntry(mapvariableidx):

   455

                                manager.AddMapVariableToCurrent(mapvariableidx, indexname, 3, 1, masternode)

   456

                                new_index = True

   457

                            nbsubentries = masternode.GetEntry(mapvariableidx, 0x00)

   458

   459

                    if mapvariableidx < 0x6000:

   460

                        if DictLocations[variable[1]]["bit"] != None:

   461

                            subindexname = str(DictLocations[variable[1]]["index"]) + "_" + str(DictLocations[variable[1]]["subindex"]) + "_" + str(DictLocations[variable[1]]["bit"])

   462

                        else:

   463

                            subindexname = str(DictLocations[variable[1]]["index"]) + "_" + str(DictLocations[variable[1]]["subindex"])

   464

                        if not new_index:

   465

                            manager.AddSubentriesToCurrent(mapvariableidx, 1, masternode)

   466

                            nbsubentries += 1

   467

                        masternode.SetMappingEntry(mapvariableidx, nbsubentries, values = {"name" : subindexname})

   468

                        masternode.SetMappingEntry(mapvariableidx, nbsubentries, values = {"type" : typeidx})

   469

   470

                        # Map Variable

   471

                        typeinfos = manager.GetEntryInfos(typeidx)

   472

                        if typeinfos != None:

   473

                            value = (mapvariableidx << 16) + ((nbsubentries) << 8) + typeinfos["size"]

   474

                            masternode.SetEntry(current_idx + 0x200, subindex, value)

   475

                else:

   476

                    masternode.SetEntry(current_idx + 0x200, subindex, TrashVariableValue[variable])

   477

   478

            CurrentPDOParamsIdx[pdo_infos["type"]] += 1

   479

   480

   495

    dcfgenerator = ConciseDCFGenerator(nodelist)

   496

    dcfgenerator.GenerateDCF(locations, current_location, sync_TPDOs)

   497

    return dcfgenerator.GetMasterNode()

   498

   499