diff --git a/site_ansto/instrument/TEST_SICS/fakeFermi/SIM_FERMI.py b/site_ansto/instrument/TEST_SICS/fakeFermi/SIM_FERMI.py index 41875ba1..153c3084 100755 --- a/site_ansto/instrument/TEST_SICS/fakeFermi/SIM_FERMI.py +++ b/site_ansto/instrument/TEST_SICS/fakeFermi/SIM_FERMI.py @@ -15,12 +15,28 @@ # MBAP = [TID][PID][len][UID] # PDU = [FC][Data] +import sys +import os import binascii from struct import * from collections import namedtuple from twisted.internet import reactor, protocol from twisted.protocols.basic import LineReceiver +# Reopen STDOUT unbuffered +sys.stdout = os.fdopen(sys.stdout.fileno(), 'w',0) + +# Debug flags +INFO = 1 +COMS = 2 +RREG = 4 +WREG = 8 +RCOIL = 16 +WCOIL = 32 +RHREGS = 64 +WMREGS = 128 + + # Coil addresses per command CSTART = 0 CSTOP = 1 @@ -71,6 +87,7 @@ RVAL = { class Fermi_Prot(LineReceiver): def __init__(self): + self.flags = INFO self.mbhlen = 7 self.fcbyte = 7 self.datstart=8 @@ -133,6 +150,10 @@ class Fermi_Prot(LineReceiver): 1016: dir } + def debug(self, flags, *args): + if (self.flags & flags): + print args + def getFR(self, SA,QR): uid = self.mbap.UID type = self.RegInfo[SA][0] @@ -141,10 +162,10 @@ class Fermi_Prot(LineReceiver): else: NE = QR EA = 2*NE + SA - print 'NE = %d EA = %d ' % (NE,EA) + self.debug(RREG, 'NE = %d EA = %d ' % (NE,EA)) data = [] for a in range(SA, EA, 2): - print 'reg %d = %s' % (a, self.RegInfo[a][1]) + self.debug(RREG, 'reg %d = %s' % (a, self.RegInfo[a][1])) data += [self.RegVal[uid][a]] return data @@ -158,34 +179,30 @@ class Fermi_Prot(LineReceiver): NE = QR fs = '>%dH' EA = 2*NE + SA - print 'NE = %d EA = %d ' % (NE,EA) + self.debug(WREG, 'NE = %d EA = %d ' % (NE,EA)) t = unpack(fs % NE,data) - print 'setFR(): t = ', map(hex, t) + self.debug(WREG, 'setFR(): t = ', map(hex, t)) i = 0 for a in range(SA, EA, 2): n = t[i] - print 'setFR() a = ', a, ' i = ', i, 'setFR() n = ', n - print 'set reg %d = %s' % (a, self.RegInfo[a][1]), ' to ', hex(n) + self.debug(WREG, 'setFR() a = ', a, ' i = ', i, 'setFR() n = ', n) + self.debug(INFO, 'set reg %d = %s' % (a, self.RegInfo[a][1]), ' to ', hex(n)) self.RegVal[uid][a] = n i += 1 return def rawDataReceived(self, ADU): -# print "Received ADU " + binascii.hexlify(ADU) -# print "Received ADU({}) {!r}".format(len(ADU),ADU) - print "Received ADU ", ADU.encode('hex') + self.debug(COMS, "Received ADU ", ADU.encode('hex')) self.ADU = ADU dl = len(ADU) self.mbap = self.MBAP._make(unpack('>3HB',ADU[:self.mbhlen])) self.fcode = unpack('>B', ADU[self.fcbyte])[0] - print self.mbap, 'fcode = ', self.fcode + self.debug(COMS, self.mbap, 'fcode = ', self.fcode) self.MBFN[self.fcode]() -# if (dl == self.mbhlen + self.mbap.len - 1): -# print self.MBAP._make(unpack('> def connectionMade(self): - print "Connection made" + self.debug(INFO, "Connection made") def getPDU(self): return self.ADU[self.datstart:] @@ -201,7 +218,7 @@ class Fermi_Prot(LineReceiver): uid = self.mbap.UID PDU = self.getPDU() (SA, QC) = unpack('>2H',PDU) - print 'rcoils:SA=%d, QC=%d' % (SA,QC) + self.debug(RCOIL, 'rcoils:SA=%d, QC=%d' % (SA,QC)) hdr = self.mbap._replace(len = 4) BC = 1 # ByteCount #TODO Coil values should be an array of bytes with each bit representing a coil @@ -210,62 +227,76 @@ class Fermi_Prot(LineReceiver): else: data = 0 resp = pack('>3HB', *hdr) + pack('>2B%dB' % BC, self.fcode, BC, data) - print 'resp = ', resp.encode('hex') + self.debug(RCOIL, 'resp = ', resp.encode('hex')) self.sendLine(resp) def wcoil(self): uid = self.mbap.UID PDU = self.getPDU() (OA, OV) = unpack('>2H', self.ADU[self.datstart:]) - print 'wcoil:OA=%d, OV=%X' % (OA,OV) + self.debug(WCOIL, 'wcoil:OA=%d, OV=%X' % (OA,OV)) self.FermiCoil[uid][OA] = OV if (OA == CSTART): + self.debug(INFO, "START") # Set RotSpeed value self.RegVal[uid][14] = self.RegVal[uid][1000] if (self.RegVal[uid][34] == _PHASE): + self.debug(INFO, 'UP TO SPEED, RUNNING, LEVITATING, PHASELOCKED') self.RegVal[uid][10] |= (_UP_TO_SPEED|_RUN|_LEV|_PHLOCK) else: + self.debug(INFO, 'UP TO SPEED, RUNNING, LEVITATING') self.RegVal[uid][10] |= (_UP_TO_SPEED|_RUN|_LEV) elif (OA == CSTOP): + self.debug(INFO, "STOP") self.RegVal[uid][14] = 0 if (self.RegVal[uid][34] == _PHASE): + self.debug(INFO, 'NOT UP TO SPEED, NOT RUNNING, NOT LEVITATING, NOT PHASELOCKED') self.RegVal[uid][10] &= ~(_UP_TO_SPEED|_RUN|_LEV|_PHLOCK) else: + self.debug(INFO, 'NOT UP TO SPEED, NOT RUNNING, NOT LEVITATING') self.RegVal[uid][10] &= ~(_UP_TO_SPEED|_RUN|_LEV) elif (OA == CIDLE): - print "Set IDLE" + self.debug(INFO, "TOGGLE IDLE") + if ( (self.RegVal[uid][10] & _UP_TO_SPEED) > 0): + self.debug(INFO, "SET IDLE SPEED") + self.RegVal[uid][14] = 0 + else: + self.debug(INFO, "SET RUN SPEED") + self.RegVal[uid][14] = self.RegVal[uid][1000] + self.debug(INFO, "TOGGLE _UP_TO_SPEED") + self.RegVal[uid][10] ^= _UP_TO_SPEED elif (OA == CRESET): - print "RESET" - print 'resp = ', self.ADU.encode('hex') + self.debug(INFO, "RESET") + self.debug(WCOIL, 'resp = ', self.ADU.encode('hex')) self.sendLine(self.ADU) def rhregs(self): uid = self.mbap.UID PDU = self.getPDU() (SA, QR) = unpack('>2H', self.ADU[self.datstart:]) - print 'rhregs:SA=%d, QR=%d' % (SA,QR) + self.debug(RHREGS, 'rhregs:SA=%d, QR=%d' % (SA,QR)) regval = self.getFR(SA,QR) - print 'rhregs:data = ', regval + self.debug(RHREGS, 'rhregs:data = ', regval) hdr = self.mbap._replace(len = 3 + 2*QR) - print 'hdr = ', hdr + self.debug(RHREGS, 'hdr = ', hdr) type = self.RegInfo[SA][0] if (type == 'U32' or type == 'F32'): NE = QR/2 resp = pack('>3HB', *hdr) + pack('>2B%dI' % NE, self.fcode, 2*QR, *regval) else: resp = pack('>3HB', *hdr) + pack('>2B%dH' % QR, self.fcode, 2*QR, *regval) - print 'resp = ', resp.encode('hex') + self.debug(RHREGS, 'resp = ', resp.encode('hex')) self.sendLine(resp) def wmregs(self): PDU = self.getPDU() (SA, QR, BC) = unpack('>2HB',PDU[:5]) data = PDU[5:] - print 'wmregs:SA=%d, QR=%d, BC=%d' % (SA,QR,BC), 'data = ', map(hex, unpack('>%dH' % QR, data)) + self.debug(WMREGS, 'wmregs:SA=%d, QR=%d, BC=%d' % (SA,QR,BC), 'data = ', map(hex, unpack('>%dH' % QR, data)) ) self.setFR(SA, QR, data) hdr = self.mbap._replace(len = 7) resp = pack('>3HB', *hdr) + pack('>B2H', self.fcode, SA, QR) - print 'resp = ', resp.encode('hex') + self.debug(WMREGS, 'resp = ', resp.encode('hex')) self.sendLine(resp)