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

    13

import os

    14

import signal

    15

import subprocess

    16

import ctypes

    17

from threading import Thread

    18

import time

    19

import re

    20

    21

import runtime.PLCObject as PLCObject

    22

from runtime.loglevels import LogLevelsDict

    23

    24

SDOAnswered = PLCBinary.SDOAnswered

    25

SDOAnswered.restype = None

    26

SDOAnswered.argtypes = []

    27

    28

SDOThread = None

    29

SDOProc = None

    30

Result = None

    31

    32

    33

def SDOThreadProc(*params):

    34

    global Result, SDOProc

    35

    if params[0] == "upload":

    36

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

    37

    else:

    38

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

    39

    40

    command = cmdfmt % params[1:]

    41

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

    42

    res = SDOProc.wait()

    43

    output = SDOProc.communicate()[0]

    44

    45

    if params[0] == "upload":

    46

        Result = None

    47

        if res == 0:

    48

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

    49

                Result = float(output)

    50

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

    51

                Result = output

    52

            else:

    53

                hex_value, dec_value = output.split()

    54

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

    55

                    Result = int(dec_value)

    56

    else:

    57

        Result = res == 0

    58

    59

    SDOAnswered()

    60

    if res != 0:

    61

        PLCObject.LogMessage(

    62

            LogLevelsDict["WARNING"],

    63

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

    64

    65

    66

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

    67

    global SDOThread

    68

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

    69

    SDOThread.start()

    70

    71

    72

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

    73

    global SDOThread

    74

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

    75

    SDOThread.start()

    76

    77

    78

def GetResult():

    79

    return Result

    80

    81

    82

KMSGPollThread = None

    83

StopKMSGThread = False

    84

    85

    86

def KMSGPollThreadProc():

    87

    """

    88

    Logs Kernel messages starting with EtherCAT

    89

    Uses GLibc wrapper to Linux syscall "klogctl"

    90

    Last 4 KB are polled, and lines compared to last

    91

    captured line to detect new lines

    92

    """

    93

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

    94

    klog = libc.klogctl

    95

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

    96

    klog.restype = ctypes.c_int

    97

    s = ctypes.create_string_buffer(4*1024)

    98

    last = None

    99

    while not StopKMSGThread:

   100

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

   101

        log = s.value[:bytes_to_read-1]

   102

        if last:

   103

            log = log.rpartition(last)[2]

   104

        if log:

   105

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

   106

            for lvl, msg in re.findall(

   107

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

   108

                    log, re.MULTILINE):

   109

                PLCObject.LogMessage(

   110

                    LogLevelsDict[{

   111

                        "4": "WARNING",

   112

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

   113

                    msg)

   114

        time.sleep(0.5)

   115

   116

   117

# TODO : rename to match _runtime_{location}_extname_init() format

   118

def _runtime_etherlab_init():

   119

    global KMSGPollThread, StopKMSGThread

   120

    StopKMSGThread = False

   121

    KMSGPollThread = Thread(target=KMSGPollThreadProc)

   122

    KMSGPollThread.start()

   123

   124

   125

# TODO : rename to match _runtime_{location}_extname_cleanup() format

   126

def _runtime_etherlab_cleanup():

   127

    global KMSGPollThread, StopKMSGThread, SDOThread

   128

    try:

   129

        os.kill(SDOProc.pid, signal.SIGTERM)

   130

    except Exception:

   131

        pass

   132

    SDOThread = None

   133

    StopKMSGThread = True

   134

    KMSGPollThread = None