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

from __future__ import absolute_import

    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

def _runtime_etherlab_init():

   118

    global KMSGPollThread, StopKMSGThread

   119

    StopKMSGThread = False

   120

    KMSGPollThread = Thread(target=KMSGPollThreadProc)

   121

    KMSGPollThread.start()

   122

   123

   124

def _runtime_etherlab_cleanup():

   125

    global KMSGPollThread, StopKMSGThread, SDOThread

   126

    try:

   127

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

   128

    except Exception:

   129

        pass

   130

    SDOThread = None

   131

    StopKMSGThread = True

   132

    KMSGPollThread = None