def test_updateNodeMsgRcvdStatus(self):
"""Test updateNodeMsgRcvdStatus method."""
nodeStatus = [NodeState(node + 1) for node in range(5)]
## Test that node status updated
testHeader = {'sourceId': 1, 'cmdId': NodeCmds['NoOp']}
# Pre-test conditions
assert (nodeStatus[0].present == False)
assert (nodeStatus[0].lastMsgRcvdTime <
self.nodeParams.clock.getTime())
updateNodeMsgRcvdStatus(nodeStatus, testHeader, self.nodeParams.clock)
# Post-test conditions
assert (nodeStatus[0].present == True)
assert (math.fabs(nodeStatus[0].lastMsgRcvdTime -
self.nodeParams.clock.getTime()) <= 0.1)
## Test that node status not updated for relayed cmds
nodeStatus = [NodeState(node + 1) for node in range(5)]
testHeader = {'sourceId': 1, 'cmdId': NodeCmds['GCSCmd']}
# Pre-test conditions
assert (nodeStatus[0].present == False)
assert (nodeStatus[0].lastMsgRcvdTime <
self.nodeParams.clock.getTime())
updateNodeMsgRcvdStatus(nodeStatus, testHeader, self.nodeParams.clock)
# Post-test conditions
assert (nodeStatus[0].present == False)
assert (nodeStatus[0].lastMsgRcvdTime <
self.nodeParams.clock.getTime())
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_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 setupParams(self):
self.cmdCounterMax = 255 # DEPRECATED - TODO - DELETE
self.cmdCounterThreshold = 10 # DEPRECATED - TODO - DELETE
self.commStartTime = []
#self.cmdRelayBuffer = []
self.cmdHistory = deque(
maxlen=50) # FIFO list of last commands received
# Node status
self.nodeStatus = [
NodeState(node + 1) for node in range(self.config.maxNumNodes)
]
# Formation clock
self.clock = FormationClock()
# TDMA Failsafe status
self.tdmaStatus = TDMAStatus.nominal
self.frameStartTime = [] # DEPRECATED - REMOVE
self.tdmaFailsafe = False
self.timeOffsetTimer = None
if (self.config.commConfig['fpga'] == True):
# Setup FPGA failsafe status pin
self.fpgaFailsafePin = self.config.commConfig['fpgaFailsafePin']
GPIO.setup(self.fpgaFailsafePin, "in")
else:
self.fpgaFailsafePin = []
# Comm link status
self.linkStatus = [[
LinkStatus.NoLink for i in range(self.config.maxNumNodes)
] for j in range(self.config.maxNumNodes)]
def test_processMsgs(self):
"""Test processMsgs method of NodeComm."""
# Create messages
cmdId1 = PixhawkCmds['FormationCmd']
cmdMsg1 = packHeader(testCmds[cmdId1].header) + testCmds[cmdId1].body
cmdId2 = PixhawkCmds['GCSCmd']
testCmds[cmdId2].header['header'][
'sourceId'] = self.nodeParams.config.gcsNodeId # reset source Id to 0 so that command will be accepted
cmdMsg2 = packHeader(testCmds[cmdId2].header) + testCmds[cmdId2].body
self.nodeParams.config.nodeId
# Send messages
self.nodeComm.sendMsg(cmdMsg1)
self.nodeComm.sendMsg(cmdMsg2)
time.sleep(0.1)
# Process messages
nodeStatus = [NodeState(node + 1) for node in range(5)]
clock = FormationClock()
self.nodeComm.processMsgs(
args={
'logFile': [],
'nav': [],
'nodeStatus': nodeStatus,
'clock': clock,
'comm': self.nodeComm
})
print(self.nodeComm.commProcessor.cmdQueue)
assert (cmdId1 in self.nodeComm.commProcessor.cmdQueue
) # Test that correct message added to cmdQueue
assert (cmdId2 in self.nodeComm.commProcessor.cmdQueue
) # Test that correct message added to cmdQueue
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 initNodeStatus(self):
# Node status
self.nodeStatus = [
NodeState(node + 1) for node in range(self.config.maxNumNodes)
]
# Comm link status
self.linkStatus = [[
LinkStatus.NoLink for i in range(self.config.maxNumNodes)
] for j in range(self.config.maxNumNodes)]
def setup_monitorNodeUpdates(self):
# Create nodeStatus to test
self.nodeStatus = [
NodeState(node + 1) for node in range(
self.nodeController.nodeParams.config.maxNumNodes)
]
clock = FormationClock()
self.nodeStatus[1].lastStateUpdateTime = time.time()
self.nodeController.nodeParams.nodeStatus = self.nodeStatus
self.nodeController.clock = clock
self.nodeController.monitorNodeUpdates()
def setup_method(self, method):
self.nodeStatus = [NodeState(i + 1) for i in range(5)]
self.nodeParams = NodeParams(configFile=configFilePath)
msgParser = MsgParser(
{'parseMsgMax': self.nodeParams.config.parseMsgMax}, SLIPMsg(256))
radio = Radio(
[], {
'uartNumBytesToRead':
self.nodeParams.config.uartNumBytesToRead,
'rxBufferSize': 2000
})
self.comm = TDMAComm([TDMACmdProcessor], radio, msgParser,
self.nodeParams)
def test_monitorFormationStatus(self):
"""Test monitorFormationStatus method of NodeController."""
# Create nodeStatus to test
nodeStatus = [NodeState(node + 1) for node in range(5)]
clock = FormationClock()
nodeStatus[4].lastStateUpdateTime = time.time()
self.nodeController.nodeParams.nodeStatus = nodeStatus
self.nodeController.clock = clock
self.nodeController.monitorFormationStatus()
# Check that updated node shows True while others show False
for node in nodeStatus[:-1]:
assert (node.updating == False)
assert (self.nodeController.nodeParams.nodeStatus[4].updating == True)
def setupParams(self):
self.configConfirmed = False
#self.commStartTime = None
#self.cmdRelayBuffer = []
self.cmdHistory = deque(maxlen=100) # FIFO list of last commands received
# Node status
self.nodeStatus = [NodeState(node+1) for node in range(self.config.maxNumNodes)]
# Formation clock
self.clock = FormationClock()
# Comm link status
self.linkStatus = [[LinkStatus.NoLink for i in range(self.config.maxNumNodes)] for j in range(self.config.maxNumNodes)]
def test_processMsg(self):
"""Test that valid message is processed."""
nodeStatus = [NodeState(node + 1) for node in range(5)]
clock = FormationClock()
cmdId = PixhawkCmds['FormationCmd']
cmdMsg = packHeader(testCmds[cmdId].header) + testCmds[cmdId].body
self.commProcessor.processMsg(cmdMsg,
args={
'logFile': [],
'nav': [],
'nodeStatus': nodeStatus,
'clock': clock,
'comm': self.serialComm
})
assert (cmdId in self.commProcessor.cmdQueue
) # Test that correct message added to cmdQueue
def test_processHeader(self):
"""Test processHeader method."""
# Test that node status and command history updated
nodeStatus = [NodeState(node + 1) for node in range(5)]
assert (processHeader(self.comm,
testCmds[NodeCmds['NoOp']].header['header'],
testCmds[NodeCmds['NoOp']].serialize(),
nodeStatus, self.nodeParams.clock,
self.comm) == True)
assert ((testCmds[NodeCmds['NoOp']].header['header']['cmdCounter']
in self.nodeParams.cmdHistory) == True)
sourceId = testCmds[NodeCmds['NoOp']].header['header']['sourceId']
assert (math.fabs(nodeStatus[sourceId - 1].lastMsgRcvdTime -
self.nodeParams.clock.getTime()) <= 0.1)
# Test for stale command counter indication
assert (processHeader(self.comm,
testCmds[NodeCmds['NoOp']].header['header'],
testCmds[NodeCmds['NoOp']].serialize(),
nodeStatus, self.nodeParams.clock,
self.comm) == False)
'dataLength': 2,
'paramValue': struct.pack('=H', 500)
}
cmd = Command(cmdId, cmdData, [cmdId, nodeId, nodeParams.get_cmdCounter()])
testCmds.update({cmdId: cmd})
### TDMACmds
# TDMACmds['MeshStatus']
cmdId = TDMACmds['MeshStatus']
cmdData = {'commStartTimeSec': int(time.time()), 'status': TDMAStatus.nominal}
cmd = Command(cmdId, cmdData, [cmdId, nodeId])
testCmds.update({cmdId: cmd})
# TDMACmds['TimeOffset']
cmdId = TDMACmds['TimeOffset']
nodeState = NodeState(1)
nodeState.timeOffset = 0.40
cmdData = {'nodeStatus': nodeState}
cmd = Command(cmdId, cmdData, [cmdId, nodeId])
testCmds.update({cmdId: cmd})
# TDMACmds['LinkStatus']
cmdId = TDMACmds['LinkStatus']
linkStatus = [[
LinkStatus.NoLink for i in range(nodeParams.config.maxNumNodes)
] for j in range(nodeParams.config.maxNumNodes)]
linkStatus[nodeId -
1] = [LinkStatus.IndirectLink] * nodeParams.config.maxNumNodes
cmdData = {'linkStatus': linkStatus, 'nodeId': 1}
cmd = Command(cmdId, cmdData, [cmdId, nodeId])
testCmds.update({cmdId: cmd})