udpPacket.py

Go to the documentation of this file.

import struct
import itertools
import collections

import coordConv
from twisted.internet.protocol import DatagramProtocol
from twisted.internet import reactor

__all__ = ["UDPSender", "getBinaryForUDP"]

class UDPBroadcastProtocol(DatagramProtocol):
    """!A variant of DatagramProtocol that enables broadcasting
    """
    def startProtocol(self):
        self.transport.setBroadcastAllowed(True)

class UDPSender(object):
    def __init__(self, port):
        """!Object which will broadcast UDP Packets containing current position and other data

        The packet format is documented here: http://www.apo.nmsu.edu/Telescopes/TCC/UDPPackets.html

        @param[in] port  port from which to send UDP packets
        """
        self.protocol = UDPBroadcastProtocol()
        self.port = port
        self.udpListen = None

    def stopListening(self):
        """!Shut down the port, to close cleanly
        """
        if self.udpListen is not None:
            self.udpListen.stopListening()
            self.udpListen = None

    def startListening(self):
        """!Open the port to allow writing data
        """
        if self.udpListen is None:
            self.udpListen = reactor.listenUDP(0, self.protocol)

    def sendPacket(self, binaryData):
        """!Write binaryData
        """
        self.protocol.transport.write(binaryData, addr=("<broadcast>", self.port))


# the format used for struct:
udpFmt = "!iiiidd8sdddddddddi4xddddddddddddd" \
    + "i4xi4xi4xi4xi4xi4xdddddddddi4xi4xi4x"

fields = [
    "size",
    "type",
    "majorVers",
    "minorVers",
    "taiDate",
    "slewEndtime",
    "coordSys",
    "epoch",
    "objNetPos.pos1",
    "objNetPos.vel1",
    "objNetPos.pos2",
    "objNetPos.vel2",
    "boresight1",
    "boresight2",
    "boresight3",
    "boresight4",
    "rotType",
    "rotUser.pos",
    "rotUser.vel",
    "objUserInstAng.pos",
    "objUserInstAng.vel",
    "spiderInstAng.pos",
    "spiderInstAng.vel",
    "tccPos.pos1",
    "tccPos.vel1",
    "tccPos.pos2",
    "tccPos.vel2",
    "tccPos.pos3",
    "tccPos.vel3",
    "secFocus",

    "axisCmdState1",
    "axisCmdState2",
    "axisCmdState3",
    "axisErrCode1",
    "axisErrCode2",
    "axisErrCode3",
    "actMount.pos1",
    "actMount.vel1",
    "actMount.t1",
    "actMount.pos2",
    "actMount.vel2",
    "actMount.t2",
    "actMount.pos3",
    "actMount.vel3",
    "actMount.t3",
    "axisStatusWord1",
    "axisStatusWord2",
    "axisStatusWord3",
]

def getBinaryForUDP(inst, obj):
    """Format a binary representation of the state of the actor, for sending via UDP

    The format is described here: http://www.apo.nmsu.edu/Telescopes/TCC/UDPPackets.html

    @param[in] inst: instrument block
    @param[in] obj: object block
    """
    updateTime = obj.updateTime
    netUserEquat = coordConv.PVT()
    netUserPolar = coordConv.PVT()
    obj.netUserPos.getSphPVT(netUserEquat, netUserPolar)
    tccPos = obj.targetMount
    toPack = collections.OrderedDict((
        ("size", 248),         # header, copied from tccdev output
        ("type", -559038737),  # header, copied from tccdev output
        ("majorVers", 2),      # header, copied from tccdev output
        ("minorVers", 4),      # header, copied from tccdev output
        ("taiDate", updateTime),
        ("slewEndtime", obj.slewEndTime),
        ("coordSys", obj.userSys.getName()),
        ("epoch", obj.userSys.getDate()),
        ("objNetPos.pos1", netUserEquat.getPos(updateTime)),
        ("objNetPos.vel1", netUserEquat.vel),
        ("objNetPos.pos2", netUserPolar.getPos(updateTime)),
        ("objNetPos.vel2", netUserPolar.vel),
        ("boresight.pos1", obj.objInstXY[0].getPos(updateTime)),
        ("boresight.vel1", obj.objInstXY[0].vel),
        ("boresight.pos2", obj.objInstXY[1].getPos(updateTime)),
        ("boresight.vel2", obj.objInstXY[1].vel),
        ("rotType", obj.rotType),
        ("rotUser.pos", obj.rotUser.pos),
        ("rotUser.vel", obj.rotUser.vel),
        ("objUserInstAng.pos", obj.objUserInstAng.getPos(updateTime)),
        ("objUserInstAng.vel", obj.objUserInstAng.vel),
        ("spiderInstAng.pos", obj.spiderInstAng.getPos(updateTime)),
        ("spiderInstAng.vel", obj.spiderInstAng.vel),
        ("tccPos.pos1", tccPos[0].getPos(updateTime)),
        ("tccPos.vel1", tccPos[0].vel),
        ("tccPos.pos2", tccPos[1].getPos(updateTime)),
        ("tccPos.vel2", tccPos[1].vel),
        ("tccPos.pos3", tccPos[2].getPos(updateTime)),
        ("tccPos.vel3", tccPos[2].vel),
        ("secFocus", inst.secUserFocus),
        ("axisCmdState1", obj.axisCmdState[0]),
        ("axisCmdState2", obj.axisCmdState[1]),
        ("axisCmdState3", obj.axisCmdState[2]),
        ("axisErrCode1", obj.axisErrCode[0]),
        ("axisErrCode2", obj.axisErrCode[1]),
        ("axisErrCode3", obj.axisErrCode[2]),

        # report actMount at the time reported by the axis controller
        ("actMount.pos1", obj.actMount[0].pos),
        ("actMount.vel1", obj.actMount[0].vel),
        ("actMount.t1", obj.actMount[0].t),
        ("actMount.pos2", obj.actMount[1].pos),
        ("actMount.vel2", obj.actMount[1].vel),
        ("actMount.t2", obj.actMount[1].t),
        ("actMount.pos3", obj.actMount[2].pos),
        ("actMount.vel3", obj.actMount[2].vel),
        ("actMount.t3", obj.actMount[2].t),
        ("axisStatusWord1", obj.axisStatusWord[0]),
        ("axisStatusWord2", obj.axisStatusWord[1]),
        ("axisStatusWord3", obj.axisStatusWord[2]),
    ))
    return struct.pack(udpFmt, *toPack.values())

def unwrapPacket(pkt):
    """!Produce readable text from an encoded UDP packet broadcasted from the tcc
    return the datagram in a dictionary format
    """
    outDict = {}
    out = struct.unpack(udpFmt,pkt)
    print 'unwrappingPacket:'
    for field, data in itertools.izip(fields, out):
        outDict[field] = data
        print "%s: %s" % (field, data)
    print
    return outDict

class BroadcastUDPClient(DatagramProtocol):
    packetsReceived = []
    def startProtocol(self):
        self.transport.setBroadcastAllowed(True)

    def datagramReceived(self, datagram, address):
        self.packetsReceived.append(unwrapPacket(datagram))
        print "Number %i Datagram received from %s" % (len(self.packetsReceived), repr(address),)

class BroadcastUDPClientVMS(BroadcastUDPClient):

    def startProtocol(self):
        self.transport.joinGroup("X.X.X.X")

class UDPListener(object):
    def __init__(self, port, vms=False):
        self.port = port
        # start listening
        print "Listening to UDP broadcasts on port %s" % (self.port,)
        self.broadcastClient = BroadcastUDPClientVMS() if vms else BroadcastUDPClient()
        self.listener = None
        self.startListening()

    def startListening(self):
        """!Create a twisted protocol that listens for UDP output at port
        """
        if self.listener is None:
            self.listener = reactor.listenMulticast(self.port, self.broadcastClient, listenMultiple=True)

    def stopListening(self):
        """!cleanly stop listening to avoid dirty reactors
        """
        if self.listener is not None:
            self.listener.stopListening()
            self.listener = None

    @property
    def packetsReceived(self):
        return self.broadcastClient.packetsReceived