PlatSim_Genova/PlatformSimulator/bin/proxybusdriver.py

from __future__ import print_function
import proxybus
import time
import threading
import sys

import xml.etree.ElementTree as ET
import os

def normalize_tags(elem):
    elem.tag = elem.tag.lower()
    for attr, value in elem.attrib.items():
        norm_attr = attr.lower()
        if norm_attr != attr:
            elem.set(norm_attr,value)
            elem.attrib.pop(attr)
    for child in elem:
        normalize_tags(child)


def parse_and_get_normalized_root(xmlfile):
    if os.path.exists(xmlfile):
        tree = ET.parse(xmlfile)
        root = tree.getroot()
        normalize_tags(root)
    else:
        root = None
    return root


debug = False
class ProxyBusDriver(object):
    def __init__(self):
        self.name= 'Abstract Bus Driver'
        self.t= None
        self.mutex = threading.Lock()
        self.stopRequest= True
        self.usclockperiod = pow(2,32) - 1
        self.icdxmlfile = proxybus.__proxy_icdxmlfile__
        self.version = proxybus.__proxy_version__
        self.injectable_errors= ()
        proxybus.setStartCallback(proxybus.__proxy_identifier__,self.start)
        proxybus.setStopCallback(proxybus.__proxy_identifier__,self.stop)
        proxybus.setSendCallback(proxybus.__proxy_identifier__,self.send)
        proxybus.setUsClockPeriodCallback(proxybus.__proxy_identifier__,lambda : int(self.usclockperiod))
        proxybus.setOnErrorInjectionListCallback(proxybus.__proxy_identifier__,lambda : (self.injectable_errors))

        if sys.version_info.major < 3:
            setattr(self,'timeclock',getattr(time,'clock'))
        else:
            setattr(self,'timeclock',getattr(time,'perf_counter'))
        self.ticktime = self.getticktime()

        self.overclock_factor = None

    __del__ = lambda self: None

    def start(self):
        print (self.name, "start...",end='')
        if not self.open_comm(): return False
        print (self.name, "thread start...",end='')
        self.clean()
        if self.t is not None and self.t.is_alive():
            self.raiseError('when running! thread id=%d'%self.t.ident,3)
        else:
            self.stopRequest= False
            self.t= threading.Thread(name=self.name, target=self._backgroundLoop) #, args=(self,))
            self.t.start()
            print ('new thread id=', self.t.ident)
        return self.t.is_alive()

    def stop(self):
        print (self.name, "stop...")
        self.acquireLock()
        self.stopRequest= True
        self.releaseLock()
        return True

    def clean(self):
        self.acquireLock()
        if self.t is not None and self.t.is_alive():
            print ('thread id=', self.t.ident, self.t.is_alive(),end='')
            self.t.join(1.0)
            if self.t.is_alive():
                print ('busy')
                self.raiseError("Unable to join python thread...",3)
            else:
                print ('Ok')
        else:
            print ('but no thread alive')
        self.releaseLock()

    def raiseError(self,msg,severiy):
        proxybus.onBusError(proxybus.__proxy_identifier__,str(msg),int(severiy))

    def forward_msg(self, m_id, m_data):
        proxybus.onReceive(proxybus.__proxy_identifier__,m_id,m_data)

    def set_log_auto(self, m_auto):
        proxybus.onSetBusLogAuto(proxybus.__proxy_identifier__,m_auto)

    def log_rx(self, m_id, data ):
        proxybus.onBusLog(proxybus.__proxy_identifier__,m_id,False,data)

    def log_tx(self, m_id, data ):
        proxybus.onBusLog(proxybus.__proxy_identifier__,m_id,True,data)

    def acquireLock(self):
        ret =False
        if self.mutex:
            ret= self.mutex.acquire(False)
        return ret

    def releaseLock(self):
        if self.mutex:
            try:
                self.mutex.release()
            except threading.ThreadError:
                print("already unlocked...")

    def getticktime(self):
        """!
        @brief return average time ( sec ) for os tick time
        @param self     : The object pointer
        """
        total=0
        for i in range(10):
           start=time.time()
           time.sleep(0.001)
           total += time.time() - start
        return total/10

    def sleep(self,sec,inaccuracy=None):
        """!
        @brief perform a more precise sleep based on inaccuracy due to os tick time or inaccuracy passed as argument
        @param self       : The object pointer
        @param inaccuracy : the amount of time to reach by using open loop cpu time
        """
        begin=self.timeclock()
        end=begin + sec
        s=sec - self.ticktime if inaccuracy is None else inaccuracy
        time.sleep(s if(s>0) else 0)
        while self.timeclock() < end:
            continue
        return self.timeclock()-begin

    def setNotifyOverClock(self,factor):
        """!
        @brief set the parameter to calculate the overclock time and notify the when the extra time
               of the loop iteration is greater
        @param self   : The object pointer
        @param factor : floating: with range [0:1] used to calculate the extra iteration time
                        None: reset to default (no check is performed)
        """
        if factor is not None and ( factor > 0 and factor <= 1 ):
            self.overclock_factor = factor
        else:
            self.overclock_factor = None

    def setLogTimeTagStringClbk(self,callback):
        proxybus.setLogTimeTagStringCallback(proxybus.__proxy_identifier__,callback)

    def _backgroundLoop(self):
        """!
        @brief it is the thread function that implement the open loop with period time based on usclockperiod
        """
        sleep_time = (self.usclockperiod/1000000.0)
        over = None if self.overclock_factor is None else sleep_time * self.overclock_factor
        #print self.usclockperiod,sleep_time
        while True:
            begin=self.timeclock()
            if self.acquireLock():
                try:
                    self.iteration()
                except:
                    self.raiseError(sys.exc_info(),3)
                    print (self.name, 'error, thread id=', self.t.ident, 'exiting',sys.exc_info())
                    break
                finally:
                    self.releaseLock()
            if self.stopRequest: break
            end = self.timeclock()
            if debug is True : print (end - begin)
            dif = sleep_time - (end - begin)
            if dif > 0 : self.sleep(dif)
            else:
               if over is not None and abs(dif) > over:
                  s="Bus Overclock {} {}".format(over * 100,abs(dif))
                  print(s)
                  self.raiseError(s,2)

            proxybus.onClock(proxybus.__proxy_identifier__)

        print ('closing connection....')
        self.close_comm()

    def send(self, m_id, m_data, m_err):
        return False

    def open_comm(self):
        return False

    def close_comm(self):
        pass

    def iteration(self):
        pass