1

#!/usr/bin/env python

     2

# -*- coding: utf-8 -*-

     3

     4

# This file is part of Beremiz

     5

#

     6

# Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT

     7

#                          RTES Lab : CRKim, JBLee, youcu

     8

#                          Higen Motor : Donggu Kang

     9

#

    10

# See COPYING file for copyrights details.

    11

    12

import os,subprocess,sys,ctypes

    13

from threading import Thread

    14

import ctypes,time,re

    15

from targets.typemapping import LogLevelsDict

    16

    17

SDOAnswered = PLCBinary.SDOAnswered

    18

SDOAnswered.restype = None

    19

SDOAnswered.argtypes = []

    20

    21

SDOThread = None

    22

SDOProc = None

    23

Result = None

    24

    25

def SDOThreadProc(*params):

    26

    global Result, SDOProc

    27

    if params[0] == "upload":

    28

        cmdfmt = "ethercat upload -p %d -t %s 0x%.4x 0x%.2x"

    29

    else:

    30

        cmdfmt = "ethercat download -p %d -t %s 0x%.4x 0x%.2x %s"

    31

    32

    command = cmdfmt % params[1:]

    33

    SDOProc = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)

    34

    res = SDOProc.wait()

    35

    output = SDOProc.communicate()[0]

    36

    37

    if params[0] == "upload":

    38

        Result = None

    39

        if res == 0:

    40

            if params[2] in ["float", "double"]:

    41

                Result = float(output)

    42

            elif params[2] in ["string", "octet_string", "unicode_string"]:

    43

                Result = output

    44

            else:

    45

                hex_value, dec_value = output.split()

    46

                if int(hex_value, 16) == int(dec_value):

    47

                    Result = int(dec_value)

    48

    else:

    49

        Result = res == 0

    50

    51

    SDOAnswered()

    52

    if res != 0 :

    53

        PLCObject.LogMessage(

    54

            LogLevelsDict["WARNING"], 

    55

            "%s : %s"%(command,output))

    56

    57

def EthercatSDOUpload(pos, index, subindex, var_type):

    58

    global SDOThread

    59

    SDOThread = Thread(target=SDOThreadProc, args=["upload", pos, var_type, index, subindex])

    60

    SDOThread.start()

    61

    62

def EthercatSDODownload(pos, index, subindex, var_type, value):

    63

    global SDOThread

    64

    SDOThread = Thread(target=SDOThreadProc, args=["download", pos, var_type, index, subindex, value])

    65

    SDOThread.start()

    66

    67

def GetResult():

    68

    global Result

    69

    return Result

    70

    71

KMSGPollThread=None

    72

StopKMSGThread=False

    73

def KMSGPollThreadProc():

    74

    """

    75

    Logs Kernel messages starting with EtherCAT

    76

    Uses GLibc wrapper to Linux syscall "klogctl"

    77

    Last 4 KB are polled, and lines compared to last 

    78

    captured line to detect new lines

    79

    """

    80

    global StopKMSGThread

    81

    libc=ctypes.CDLL("libc.so.6")

    82

    klog = libc.klogctl

    83

    klog.argtypes = [ctypes.c_int, ctypes.c_char_p, ctypes.c_int]

    84

    klog.restype = ctypes.c_int

    85

    s=ctypes.create_string_buffer(4*1024)

    86

    last = None

    87

    while not StopKMSGThread:

    88

        l = klog(3,s,len(s)-1)

    89

        log = s.value[:l-1]

    90

        if last :

    91

            log = log.rpartition(last)[2]

    92

        if log : 

    93

            last = log.rpartition('\n')[2]

    94

            for lvl,msg in re.findall(

    95

                            r'<(\d)>\[\s*\d*\.\d*\]\s*(EtherCAT\s*.*)$',

    96

                            log, re.MULTILINE):

    97

                PLCObject.LogMessage(

    98

                    LogLevelsDict[{

    99

                        "4":"WARNING",

   100

                        "3":"CRITICAL"}.get(lvl,"DEBUG")],

   101

                    msg)

   102

        time.sleep(0.5) 

   103

   104

def _runtime_etherlab_init():

   105

    global KMSGPollThread, StopKMSGThread

   106

    StopKMSGThread = False

   107

    KMSGPollThread = Thread(target = KMSGPollThreadProc)

   108

    KMSGPollThread.start()

   109

   110

def _runtime_etherlab_cleanup():

   111

    global KMSGPollThread, StopKMSGThread, SDOProc, SDOThread

   112

    try:

   113

        os.kill(SDOProc.pid, SIGTERM)

   114

    except:

   115

        pass

   116

    SDOThread = None

   117

    StopKMSGThread = True

   118

    KMSGPollThread = None

   119