class TestGndCmdProcessor:
def setup_method(self, method):
self.nodeStatus = [NodeState(i + 1) for i in range(5)]
self.nodeParams = NodeParams(configFile=configFilePath)
radio = Radio(
[], {
'uartNumBytesToRead':
self.nodeParams.config.uartNumBytesToRead,
'rxBufferSize': 2000
})
self.comm = SerialComm([GndCmdProcessor], self.nodeParams, radio, [])
def test_processMsg(self):
"""Test processMsg method of GndCmdProcessor."""
# Test processing of all GndCmds
for cmdId in cmdsToTest:
self.comm.processMsg(testCmds[cmdId].serialize(),
args={
'nodeStatus': self.nodeStatus,
'comm': self.comm,
'clock': self.nodeParams.clock
})
if cmdId == GndCmds['TimeOffsetSummary']:
for i in range(len(testCmds[cmdId].cmdData['nodeStatus'])):
assert (self.nodeStatus[i].timeOffset ==
testCmds[cmdId].cmdData['nodeStatus'][i].timeOffset
) # offset pulled from summary message and stored
elif cmdId == GndCmds['LinkStatusSummary']:
msgNodeId = testCmds[cmdId].cmdData['nodeId']
for i in range(0, self.nodeParams.config.maxNumNodes):
for j in range(0, self.nodeParams.config.maxNumNodes):
assert (self.nodeParams.linkStatus[i][i] ==
testCmds[cmdId].cmdData['linkStatus'][i][j])
def setup_method(self, method):
self.nodeStatus = [NodeState(i + 1) for i in range(5)]
self.nodeParams = NodeParams(configFile=configFilePath)
radio = Radio(
[], {
'uartNumBytesToRead':
self.nodeParams.config.uartNumBytesToRead,
'rxBufferSize': 2000
})
self.comm = SerialComm([GndCmdProcessor], self.nodeParams, radio, [])
def test_processNodeCommands(self):
"""Test processNodeCommands method of NodeController."""
# Test processing of ConfigRequest command
configHash = self.nodeController.nodeParams.config.calculateHash()
radio = Radio([], {'uartNumBytesToRead': 100, 'rxBufferSize': 100})
comm = SerialComm([], self.nodeController.nodeParams, radio, [])
comm.cmdQueue[NodeCmds['ConfigRequest']] = configHash
assert (self.nodeController.nodeParams.configConfirmed == False)
self.nodeController.processNodeCommands(comm)
assert (self.nodeController.nodeParams.configConfirmed == True)
def setup_method(self, method):
self.nodeParams = NodeParams(configFile=configFilePath)
self.serialPort = serial.Serial(port=testSerialPort,
baudrate=57600,
timeout=0)
self.radio = Radio(
self.serialPort, {
'uartNumBytesToRead':
self.nodeParams.config.uartNumBytesToRead,
'rxBufferSize': 2000
})
self.serialComm = SerialComm([NodeCmdProcessor], self.nodeParams,
self.radio, [])
def setup_method(self, method):
self.nodeParams = NodeParams(configFile=configFilePath)
self.serialPort = serial.Serial(port=testSerialPort,
baudrate=57600,
timeout=0)
self.radio = Radio(
self.serialPort, {
'uartNumBytesToRead':
self.nodeParams.config.uartNumBytesToRead,
'rxBufferSize': 2000
})
msgParser = SLIPMsgParser(
{'parseMsgMax': self.nodeParams.config.parseMsgMax})
commProcessor = CommProcessor([PixhawkCmdProcessor], self.nodeParams)
self.serialComm = SerialComm(commProcessor, self.radio, msgParser)
def setup_method(self, method):
# Create CommProcessor instance
nodeParams = NodeParams(configFile=configFilePath)
self.commProcessor = CommProcessor([PixhawkCmdProcessor], nodeParams)
self.serialComm = SerialComm([], [], nodeParams.config)
# Truth data
self.crcLength = 2
def setup_method(self, method):
self.nodeParams = NodeParams(configFile=configFilePath)
# Setup comms
self.serialConfig = {
'uartNumBytesToRead': self.nodeParams.config.uartNumBytesToRead,
'rxBufferSize': self.nodeParams.config.rxBufferSize
}
self.serialPort = serial.Serial(port=testSerialPort,
baudrate=57600,
timeout=0)
radio = Radio(self.serialPort, self.serialConfig)
nodeComm = SerialComm([], radio, [])
#self.FCSerialPort = serial.Serial(port=testSerialPort, baudrate=57600, timeout=0)
#radio = Radio(self.FCSerialPort, serialConfig)
FCComm = SerialComm([], radio, [])
# Create executive
self.nodeExecutive = NodeExecutive(self.nodeParams, None, [nodeComm],
FCComm, [])
class TestSerialComm:
def setup_method(self, method):
self.nodeParams = NodeParams(configFile=configFilePath)
self.serialPort = serial.Serial(port=testSerialPort,
baudrate=57600,
timeout=0)
self.radio = Radio(
self.serialPort, {
'uartNumBytesToRead':
self.nodeParams.config.uartNumBytesToRead,
'rxBufferSize': 2000
})
self.serialComm = SerialComm([NodeCmdProcessor], self.nodeParams,
self.radio, [])
def test_readBytes(self):
"""Test receiving bytes."""
self.serialPort.write(b'12345')
time.sleep(0.1)
self.serialComm.readBytes(False)
assert (self.serialComm.radio.getRxBytes() == b'12345')
def test_sendBytes(self):
"""Test sendBytes method of SerialComm."""
msgBytes = b'ABCDEF'
self.serialPort.read(100) # clear serial buffer
self.serialComm.sendBytes(msgBytes)
time.sleep(0.1)
self.serialComm.readBytes()
assert (self.serialComm.radio.getRxBytes() == msgBytes)
def test_parseMsgs(self):
"""Test parseMsgs method of SerialComm."""
# Test parsing of messages
msgBytes = b'ABCDEF'
self.serialComm.radio.bufferRxMsg(
msgBytes, False) # put test bytes into radio rx buffer
self.serialComm.parseMsgs()
assert (len(self.serialComm.msgParser.parsedMsgs) == 1)
assert (self.serialComm.msgParser.parsedMsgs[0] == b'ABCDEF')
# Check that radio buffer was cleared
assert (len(self.serialComm.radio.getRxBytes()) == 0)
def test_readMsgs(self):
"""Test readMsgs method of SerialComm."""
# Send message
msgBytes = b'12345'
self.serialComm.sendMsg(msgBytes)
time.sleep(0.1)
# Read messages
self.serialComm.readMsgs()
assert (len(self.serialComm.msgParser.parsedMsgs) == 1)
assert (self.serialComm.msgParser.parsedMsgs[0] == msgBytes
) # confirm parsed message matches original
def test_sendMsg(self):
"""Test sendMsg method of SerialComm."""
# Create message parser for testing purposes
msg = SLIPMsg(self.nodeParams.config.parseMsgMax)
self.serialComm.msgParser.msg = msg
# Send test message
msgBytes = b'12345'
self.serialPort.read(100) # clear serial buffer
self.serialComm.sendMsg(msgBytes)
time.sleep(0.1)
# Read message back and compare
readBytes = self.serialComm.readMsgs()
assert (len(self.serialComm.msgParser.parsedMsgs) == 1)
assert (self.serialComm.msgParser.parsedMsgs[0] == msgBytes)
def test_bufferTxMsg(self):
"""Test bufferTxMsg method of SerialComm."""
# Create message parser for testing purposes
msg = SLIPMsg(self.nodeParams.config.parseMsgMax)
self.serialComm.msgParser.msg = msg
# Test that encoded message buffered
msgBytes = b'ZYXWVU'
assert (len(self.serialComm.radio.txBuffer) == 0
) # confirm empty tx buffer
self.serialComm.bufferTxMsg(msgBytes)
encoder = SLIPMsg(256)
encoder.encodeMsg(msgBytes)
truthMsg = encoder.encoded
assert (self.serialComm.radio.txBuffer == truthMsg
) # confirm encoded message placed in tx buffer
def test_sendBuffer(self):
"""Test sendBuffer method of SerialComm."""
# Place message in tx buffer
msgBytes = b'1122334455'
self.serialComm.bufferTxMsg(msgBytes)
assert (self.serialComm.radio.txBuffer == msgBytes)
# Send message and compare received bytes to sent
assert (self.serialComm.sendBuffer() == len(msgBytes))
time.sleep(0.1)
assert (len(self.serialComm.radio.txBuffer) == 0)
self.serialComm.readBytes()
assert (self.serialComm.radio.getRxBytes() == msgBytes)
def test_processMsg(self):
"""Test processMsg method of SerialComm."""
# Create message and test processing
nodeStatus = [NodeState(node + 1) for node in range(5)]
clock = FormationClock()
cmdId = NodeCmds['NoOp']
cmdMsg = Command(cmdId, None, [cmdId, 1, 200]).serialize()
assert (self.serialComm.processMsg(cmdMsg,
args={
'logFile': [],
'nav': [],
'nodeStatus': nodeStatus,
'clock': clock,
'comm': self.serialComm
}) == True)
assert (cmdId in self.serialComm.cmdQueue
) # Test that correct message added to cmdQueue
# Confirm proper return when no message processed successfully
assert (self.serialComm.processMsg(b'12345',
args={
'logFile': [],
'nav': [],
'nodeStatus': nodeStatus,
'clock': clock,
'comm': self.serialComm
}) == False)
def test_processMsgs(self):
"""Test processMsgs method of SerialComm."""
# Create message parser for testing purposes
msg = SLIPMsg(self.nodeParams.config.parseMsgMax)
self.serialComm.msgParser.msg = msg
# Create and send test messages
nodeStatus = [NodeState(node + 1) for node in range(5)]
clock = FormationClock()
cmdId1 = NodeCmds['NoOp'] # No op command
cmdMsg1 = Command(cmdId1, None, [cmdId1, 1, 200]).serialize()
cmdId2 = NodeCmds['GCSCmd'] # GCS command
cmdMsg2 = Command(cmdId2, {
'destId': 1,
'mode': 2
}, [cmdId2, 1, 201]).serialize()
self.serialComm.sendMsg(cmdMsg1)
self.serialComm.sendMsg(cmdMsg2)
time.sleep(0.1)
# Test processing
self.serialComm.processMsgs(
args={
'logFile': [],
'nav': [],
'nodeStatus': nodeStatus,
'clock': clock,
'comm': self.serialComm
})
assert (cmdId1 in self.serialComm.cmdQueue
) # Test that correct message added to cmdQueue
assert (cmdId2 in self.serialComm.cmdQueue
) # Test that correct message added to cmdQueue
def test_processBuffers(self):
"""Test processBuffers method of SerialComm."""
# Test command relay buffer
testMsg = b'1234567890'
self.serialComm.cmdRelayBuffer = testMsg
self.serialComm.processBuffers()
assert (len(self.serialComm.cmdRelayBuffer) == 0) # buffer flushed
assert (self.serialComm.radio.txBuffer == testMsg)
self.serialComm.radio.txBuffer = bytearray()
# Test command buffer
testCmd = {'bytes': b'12345'}
self.serialComm.cmdBuffer['key1'] = testCmd
assert (self.serialComm.processBuffers() == len(testCmd['bytes']))
assert (len(self.serialComm.cmdBuffer) == 0) # buffer flushed
assert (self.serialComm.radio.txBuffer == testCmd['bytes'])
def __init__(self, configFile, meshNum, runFlag):
super().__init__(name="CommProcess")
# Node control run flag
self.nodeControlRunFlag = runFlag
# Configuration
self.nodeParams = NodeParams(configFile=configFile)
# Node/Comm interface
interfaceConfig = {
'uartNumBytesToRead': self.nodeParams.config.uartNumBytesToRead,
'rxBufferSize': self.nodeParams.config.rxBufferSize,
'ipAddr': self.nodeParams.config.interface['nodeCommIntIP'],
'readPort': self.nodeParams.config.interface['commRdPort'],
'writePort': self.nodeParams.config.interface['commWrPort']
}
#self.interface = SerialComm([], UDPRadio(interfaceConfig), SLIPMsgParser({'parseMsgMax': self.nodeParams.config.parseMsgMax}))
self.interface = SerialComm(
[], self.nodeParams, UDPRadio(interfaceConfig),
MsgParser({'parseMsgMax': self.nodeParams.config.parseMsgMax},
SLIPMsg(256))) # UDP connection to node control process
# Interprocess data package (Google protocol buffer interface to node control process)
self.dataPackage = NodeThreadMsg()
self.cmdTxLog = {}
self.lastNodeCmdTime = []
## Create comm object
# Serial connection
ser = serial.Serial(port=self.nodeParams.config.meshDevices[meshNum],
baudrate=self.nodeParams.config.meshBaudrate,
timeout=0)
# Radio
radioConfig = {
'uartNumBytesToRead': self.nodeParams.config.uartNumBytesToRead,
'rxBufferSize': self.nodeParams.config.rxBufferSize
}
if (self.nodeParams.config.commConfig['fpga'] == True):
from mesh.generic.fpgaRadio import FPGARadio
radio = FPGARadio(ser, radioConfig)
else:
if self.nodeParams.config.radios[meshNum] == "Xbee":
radio = XbeeRadio(ser, radioConfig, "P8_12")
elif self.nodeParams.config.radios[meshNum] == "Li-1":
radio = Li1Radio(ser, radioConfig)
# Message parser
parserConfig = {'parseMsgMax': self.nodeParams.config.parseMsgMax}
if self.nodeParams.config.msgParsers[meshNum] == "HDLC":
msgParser = MsgParser(parserConfig, HDLCMsg(256))
elif self.nodeParams.config.msgParsers[meshNum] == "standard":
msgParser = MsgParser(parserConfig)
# Create comm
if (self.nodeParams.config.commConfig['fpga'] == True):
from mesh.generic.tdmaComm_fpga import TDMAComm_FPGA as TDMAComm
else:
from mesh.generic.tdmaComm import TDMAComm
self.comm = TDMAComm([], radio, msgParser, self.nodeParams)
# Node control run time bounds
if (self.nodeParams.config.commConfig['fpga'] == False
): # only needed for software-controlled comm
if self.comm.transmitSlot == 1: # For first node, run any time after transmit slot
self.maxNodeControlTime = self.comm.frameLength - self.comm.slotLength
self.minNodeControlTime = self.comm.slotLength
else: # For other nodes, avoid running near transmit slot
self.minNodeControlTime = (
self.comm.transmitSlot - 2
) * self.comm.slotLength # don't run within 1 slot of transmit
self.maxNodeControlTime = self.comm.transmitSlot * self.comm.slotLength
class CommProcess(Process):
def __init__(self, configFile, meshNum, runFlag):
super().__init__(name="CommProcess")
# Node control run flag
self.nodeControlRunFlag = runFlag
# Configuration
self.nodeParams = NodeParams(configFile=configFile)
# Node/Comm interface
interfaceConfig = {
'uartNumBytesToRead': self.nodeParams.config.uartNumBytesToRead,
'rxBufferSize': self.nodeParams.config.rxBufferSize,
'ipAddr': self.nodeParams.config.interface['nodeCommIntIP'],
'readPort': self.nodeParams.config.interface['commRdPort'],
'writePort': self.nodeParams.config.interface['commWrPort']
}
#self.interface = SerialComm([], UDPRadio(interfaceConfig), SLIPMsgParser({'parseMsgMax': self.nodeParams.config.parseMsgMax}))
self.interface = SerialComm(
[], self.nodeParams, UDPRadio(interfaceConfig),
MsgParser({'parseMsgMax': self.nodeParams.config.parseMsgMax},
SLIPMsg(256))) # UDP connection to node control process
# Interprocess data package (Google protocol buffer interface to node control process)
self.dataPackage = NodeThreadMsg()
self.cmdTxLog = {}
self.lastNodeCmdTime = []
## Create comm object
# Serial connection
ser = serial.Serial(port=self.nodeParams.config.meshDevices[meshNum],
baudrate=self.nodeParams.config.meshBaudrate,
timeout=0)
# Radio
radioConfig = {
'uartNumBytesToRead': self.nodeParams.config.uartNumBytesToRead,
'rxBufferSize': self.nodeParams.config.rxBufferSize
}
if (self.nodeParams.config.commConfig['fpga'] == True):
from mesh.generic.fpgaRadio import FPGARadio
radio = FPGARadio(ser, radioConfig)
else:
if self.nodeParams.config.radios[meshNum] == "Xbee":
radio = XbeeRadio(ser, radioConfig, "P8_12")
elif self.nodeParams.config.radios[meshNum] == "Li-1":
radio = Li1Radio(ser, radioConfig)
# Message parser
parserConfig = {'parseMsgMax': self.nodeParams.config.parseMsgMax}
if self.nodeParams.config.msgParsers[meshNum] == "HDLC":
msgParser = MsgParser(parserConfig, HDLCMsg(256))
elif self.nodeParams.config.msgParsers[meshNum] == "standard":
msgParser = MsgParser(parserConfig)
# Create comm
if (self.nodeParams.config.commConfig['fpga'] == True):
from mesh.generic.tdmaComm_fpga import TDMAComm_FPGA as TDMAComm
else:
from mesh.generic.tdmaComm import TDMAComm
self.comm = TDMAComm([], radio, msgParser, self.nodeParams)
# Node control run time bounds
if (self.nodeParams.config.commConfig['fpga'] == False
): # only needed for software-controlled comm
if self.comm.transmitSlot == 1: # For first node, run any time after transmit slot
self.maxNodeControlTime = self.comm.frameLength - self.comm.slotLength
self.minNodeControlTime = self.comm.slotLength
else: # For other nodes, avoid running near transmit slot
self.minNodeControlTime = (
self.comm.transmitSlot - 2
) * self.comm.slotLength # don't run within 1 slot of transmit
self.maxNodeControlTime = self.comm.transmitSlot * self.comm.slotLength
#self.minNodeControlTime = 0.8*((self.comm.transmitSlot-1) * self.comm.slotLength)
def run(self):
while 1:
try:
# Check for loss of node commands
if self.lastNodeCmdTime and (time.time() -
self.lastNodeCmdTime) > 5.0:
# No node interface link so disable comm
self.comm.enabled = False
else:
self.comm.enabled = True
# Check for new messages from node control process
self.interface.readMsgs()
# Parse protocol buffer messages
for msg in self.interface.msgParser.parsedMsgs: # Process received messages
nodeMsg = NodeThreadMsg()
nodeMsg.ParseFromString(msg)
# Check if new message
if (nodeMsg.timestamp > 0.0 and
nodeMsg.timestamp > self.dataPackage.timestamp):
self.lastNodeCmdTime = time.time()
self.dataPackage = nodeMsg
# Parse messages for transmission
if (nodeMsg.cmds): # commands received
for cmd in nodeMsg.cmds:
self.comm.queueMeshMsg(cmd.destId,
cmd.msgBytes)
self.interface.msgParser.parsedMsgs = [
] # clear out processed parsed messages
# Execute comm
self.comm.execute()
# Managed node control run flag (only when comm is running in software)
if (self.nodeParams.config.commConfig['fpga'] == False):
if self.comm.transmitSlot == 1 and (
self.comm.frameTime > self.minNodeControlTime
and self.comm.frameTime < self.maxNodeControlTime):
self.nodeControlRunFlag.value = 1
elif self.comm.transmitSlot != 1 and (
self.comm.frameTime < self.minNodeControlTime
or self.comm.frameTime > self.maxNodeControlTime):
self.nodeControlRunFlag.value = 1
else: # restrict node control process running
self.nodeControlRunFlag.value = 0
# Send any received bytes to node control process
#if (self.nodeParams.config.commConfig['fpga'] == False or self.comm.frameTime > self.comm.cycleLength):
if (self.comm.hostBuffer):
rcvdBytes = self.comm.hostBuffer
self.comm.hostBuffer = bytearray()
self.interface.sendBytes(rcvdBytes)
except KeyboardInterrupt:
print("\nTerminating Comm Process.")
self.terminate()
def __init__(self, configFile, runFlag):
super().__init__(name="NodeControlProcess", )
# Run flag
self.runFlag = runFlag
# Configuration
nodeParams = NodeParams(configFile=configFile)
# Create radios
radios = []
radioConfig = {
'uartNumBytesToRead': nodeParams.config.uartNumBytesToRead,
'rxBufferSize': nodeParams.config.rxBufferSize,
'ipAddr': nodeParams.config.interface['nodeCommIntIP'],
'readPort': nodeParams.config.interface['commWrPort'],
'writePort': nodeParams.config.interface['commRdPort']
}
for i in range(nodeParams.config.numMeshNetworks):
radios.append(
UDPRadio(radioConfig)) # connection to communication processes
# Create message parsers
msgParsers = []
parserConfig = {'parseMsgMax': nodeParams.config.parseMsgMax}
for i in range(nodeParams.config.numMeshNetworks):
#if nodeParams.config.msgParsers[i] == "SLIP":
msgParsers.append(MsgParser(parserConfig, SLIPMsg(256)))
#elif nodeParams.config.msgParsers[i] == "standard":
# msgParsers.append(MsgParser(parserConfig))
# Open logfiles
currentTime = str(time.time())
FCLogFilename = 'fc_' + currentTime + '.log'
self.FCLogFile = open(FCLogFilename, 'w')
nodeCommLogFilename = 'node_' + currentTime + '.log'
self.nodeCommLogFile = open(nodeCommLogFilename, 'w')
# Failsafe LED interval
failsafeLEDTime = 1.0 # seconds
failsafeLEDOnTime = -1.0
failsafeLEDOn = False
# Initialize node and flight computer communication variables
nodeComm = [[]] * nodeParams.config.numMeshNetworks
FCComm = []
# Instantiate specific node software
self.nodeController = []
self.nodeExecutive = []
for case in switch(nodeParams.config.platform
): # Platform specific initializations
if case("SpecificNode"):
pass
else: # generic node
from mesh.generic.serialComm import SerialComm
from demoController import DemoController
from mesh.generic.nodeExecutive import NodeExecutive
print("Initializing generic node")
# Initialize communication variables
for i in range(nodeParams.config.numMeshNetworks):
nodeComm[i] = SerialComm([], nodeParams, radios[i],
msgParsers[i])
# Flight computer comm
if (nodeParams.config.FCCommDevice):
FCSer = serial.Serial(
port=nodeParams.config.FCCommDevice,
baudrate=nodeParams.config.FCBaudrate,
timeout=0)
FCRadio = Radio(FCSer, radioConfig)
FCMsgParser = MsgParser(parserConfig, SLIPMsg(256))
FCComm = SerialComm([], FCRadio, FCMsgParser, nodeParams)
else:
FCComm = None
# Node controller
self.nodeController = DemoController(nodeParams,
self.nodeCommLogFile)
# Node executive
self.nodeExecutive = NodeExecutive(nodeParams,
self.nodeController,
nodeComm, FCComm,
self.FCLogFile)
class CommProcess(Process):
def __init__(self, configFile, meshNum, runFlag):
super().__init__(name="CommProcess")
# Node control run flag
self.nodeControlRunFlag = runFlag
# Configuration
self.nodeParams = NodeParams(configFile=configFile)
# Node/Comm interface
interfaceConfig = {
'uartNumBytesToRead': self.nodeParams.config.uartNumBytesToRead,
'rxBufferSize': self.nodeParams.config.rxBufferSize,
'ipAddr': self.nodeParams.config.interface['nodeCommIntIP'],
'readPort': self.nodeParams.config.interface['commRdPort'],
'writePort': self.nodeParams.config.interface['commWrPort']
}
self.interface = SerialComm(
[], UDPRadio(interfaceConfig),
SLIPMsgParser({'parseMsgMax': self.nodeParams.config.parseMsgMax}))
# Interprocess data package
self.dataPackage = NodeThreadMsg()
self.cmdTxLog = {}
self.lastNodeCmdTime = []
## Create comm object
# Serial connection
ser = serial.Serial(port=self.nodeParams.config.meshDevices[meshNum],
baudrate=self.nodeParams.config.meshBaudrate,
timeout=0)
# Radio
radioConfig = {
'uartNumBytesToRead': self.nodeParams.config.uartNumBytesToRead,
'rxBufferSize': self.nodeParams.config.rxBufferSize
}
if (self.nodeParams.config.commConfig['fpga'] == True):
from mesh.generic.fpgaRadio import FPGARadio
radio = FPGARadio(ser, radioConfig)
else:
if self.nodeParams.config.radios[meshNum] == "Xbee":
radio = XbeeRadio(ser, radioConfig, "P8_12")
elif self.nodeParams.config.radios[meshNum] == "Li-1":
radio = Li1Radio(ser, radioConfig)
# Message parser
parserConfig = {'parseMsgMax': self.nodeParams.config.parseMsgMax}
if self.nodeParams.config.msgParsers[meshNum] == "SLIP":
msgParser = SLIPMsgParser(parserConfig)
elif self.nodeParams.config.msgParsers[meshNum] == "standard":
msgParser = MsgParser(parserConfig)
# Create comm
if (self.nodeParams.config.commConfig['fpga'] == True):
from mesh.generic.tdmaComm_fpga import TDMAComm_FPGA as TDMAComm
else:
from mesh.generic.tdmaComm import TDMAComm
self.comm = TDMAComm([], radio, msgParser, self.nodeParams)
# Node control run time bounds
if (self.nodeParams.config.commConfig['fpga'] == False):
if self.comm.transmitSlot == 1:
self.maxNodeControlTime = self.comm.frameLength - self.comm.slotLength
self.minNodeControlTime = self.comm.transmitSlot * self.comm.slotLength
else:
self.minNodeControlTime = (
self.comm.transmitSlot - 2
) * self.comm.slotLength # don't run within 1 slot of transmit
self.maxNodeControlTime = self.comm.transmitSlot * self.comm.slotLength
#self.minNodeControlTime = 0.8*((self.comm.transmitSlot-1) * self.comm.slotLength)
def run(self):
while 1:
try:
# Check for loss of node commands
if self.lastNodeCmdTime and (time.time() -
self.lastNodeCmdTime) > 5.0:
# No node interface link so disable comm
self.comm.enabled = False
else:
self.comm.enabled = True
# Check for new messages from node control process
self.interface.readMsgs()
# Parse protocol buffer messages
for msg in self.interface.msgParser.parsedMsgs: # Process received messages
nodeMsg = NodeThreadMsg()
nodeMsg.ParseFromString(msg)
# Check if new message
if (nodeMsg.timestamp > 0.0 and
nodeMsg.timestamp > self.dataPackage.timestamp):
self.lastNodeCmdTime = time.time()
self.dataPackage = nodeMsg
# Update link status
if (self.nodeParams.config.nodeId == self.nodeParams.
config.gcsNodeId): # ground node
entry = self.nodeParams.config.maxNumNodes - 1
else:
entry = self.nodeParams.config.nodeId - 1
self.nodeParams.linkStatus[entry] = nodeMsg.linkStatus
if (nodeMsg.cmdRelay): # command relay data
for cmd in nodeMsg.cmdRelay:
self.comm.cmdRelayBuffer.append(cmd)
#print("Cmds to relay:",self.comm.cmdRelayBuffer)
if (nodeMsg.cmds): # commands received
#self.comm.cmdBuffer = [] # clear existing buffer
for cmd in nodeMsg.cmds:
self.comm.cmdBuffer[cmd.cmdId] = {
'bytes': cmd.msgBytes,
'txInterval': cmd.txInterval
}
#self.comm.cmdBuffer.append({'bytes': cmd.msgBytes, 'txInterval': cmd.txInterval})
self.interface.msgParser.parsedMsgs = [
] # clear out processed parsed messages
# Execute comm
self.comm.execute()
# Run node control (only when comm is running in software)
if (self.nodeParams.config.commConfig['fpga'] == False):
if self.comm.transmitSlot == 1 and (
self.comm.frameTime > self.minNodeControlTime
and self.comm.frameTime < self.maxNodeControlTime):
self.nodeControlRunFlag.value = 1
elif self.comm.transmitSlot != 1 and (
self.comm.frameTime < self.minNodeControlTime
or self.comm.frameTime > self.maxNodeControlTime):
self.nodeControlRunFlag.value = 1
else: # restrict node control process running
self.nodeControlRunFlag.value = 0
# Send any received bytes to node control process
if (self.nodeParams.config.commConfig['fpga'] == False
or self.comm.frameTime > self.comm.cycleLength):
if (self.comm.radio.bytesInRxBuffer > 0):
rcvdBytes = self.comm.radio.getRxBytes()
self.comm.radio.clearRxBuffer()
self.interface.send_bytes(rcvdBytes)
except KeyboardInterrupt:
print("\nTerminating Comm Process.")
self.terminate()
class TestSerialComm:
def setup_method(self, method):
self.nodeParams = NodeParams(configFile=configFilePath)
self.serialPort = serial.Serial(port=testSerialPort,
baudrate=57600,
timeout=0)
self.radio = Radio(
self.serialPort, {
'uartNumBytesToRead':
self.nodeParams.config.uartNumBytesToRead,
'rxBufferSize': 2000
})
msgParser = SLIPMsgParser(
{'parseMsgMax': self.nodeParams.config.parseMsgMax})
commProcessor = CommProcessor([PixhawkCmdProcessor], self.nodeParams)
self.serialComm = SerialComm(commProcessor, self.radio, msgParser)
def test_bufferTxMsg(self):
"""Test bufferTxMsg method of SerialComm."""
assert (len(self.serialComm.radio.txBuffer) == 0
) # confirm empty tx buffer
self.serialComm.bufferTxMsg(testMsg)
truthMsg = createEncodedMsg(testMsg)
assert (self.serialComm.radio.txBuffer == truthMsg
) # confirm message placed in tx buffer
def test_sendBuffer(self):
"""Test sendBuffer method of SerialComm."""
# Place message in tx buffer
self.serialComm.bufferTxMsg(testMsg)
assert (len(self.serialComm.radio.txBuffer) > 0)
# Send message and compare received bytes to sent
self.serialComm.sendBuffer()
assert (len(self.serialComm.radio.txBuffer) == 0)
time.sleep(0.1)
self.serialComm.readBytes()
truthMsg = createEncodedMsg(testMsg)
serBytes = self.serialComm.radio.getRxBytes()
assert (serBytes == truthMsg)
def test_sendBytes(self):
"""Test sendBytes method of SerialComm."""
msgBytes = b'ABCDEF'
self.serialPort.read(100) # clear serial buffer
self.serialComm.sendBytes(msgBytes)
time.sleep(0.1)
readBytes = self.serialPort.read(100)
assert (readBytes == msgBytes)
def test_sendMsg(self):
"""Test sendMsg method of SerialComm."""
# Send test message
msgBytes = testMsg
self.serialPort.read(100) # clear serial buffer
self.serialComm.sendMsg(testMsg)
time.sleep(0.1)
readBytes = self.serialPort.read(100)
# Encode test message and compare to bytes read
truthMsg = createEncodedMsg(testMsg)
assert (readBytes == truthMsg)
def test_parseMsgs(self):
"""Test parseMsgs method of SerialComm."""
# Place multiple messages in rx buffer
truthMsg = createEncodedMsg(testMsg)
truthMsg2 = createEncodedMsg(testMsg + b'999')
self.serialComm.radio.rxBuffer[0:len(truthMsg)] = truthMsg
self.serialComm.radio.rxBuffer[len(truthMsg):len(truthMsg) +
len(truthMsg2)] = truthMsg2
self.serialComm.radio.bytesInRxBuffer = len(truthMsg) + len(truthMsg2)
# Parse messages
self.serialComm.parseMsgs()
assert (len(self.serialComm.msgParser.parsedMsgs) == 2)
assert (self.serialComm.msgParser.parsedMsgs[0] == testMsg
) # confirm parsed message matches original
assert (self.serialComm.msgParser.parsedMsgs[1] == testMsg + b'999'
) # confirm parsed message matches original
def test_readMsgs(self):
"""Test readMsgs method of SerialComm."""
# Send messages
self.serialComm.sendMsg(testMsg)
self.serialComm.sendMsg(testMsg + b'999')
time.sleep(0.1)
# Read messages
self.serialComm.readMsgs()
assert (len(self.serialComm.msgParser.parsedMsgs) == 2)
assert (self.serialComm.msgParser.parsedMsgs[0] == testMsg
) # confirm parsed message matches original
assert (self.serialComm.msgParser.parsedMsgs[1] == testMsg + b'999'
) # confirm parsed message matches original
def test_processMsg(self):
"""Test processMsg method of SerialComm."""
# Create message and test processing
nodeStatus = [NodeState(node + 1) for node in range(5)]
clock = FormationClock()
cmdId = PixhawkCmds['FormationCmd']
cmdMsg = packHeader(testCmds[cmdId].header) + testCmds[cmdId].body
self.serialComm.processMsg(cmdMsg,
args={
'logFile': [],
'nav': [],
'nodeStatus': nodeStatus,
'clock': clock,
'comm': self.serialComm
})
assert (cmdId in self.serialComm.commProcessor.cmdQueue
) # Test that correct message added to cmdQueue