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 packHeader(self):
header = bytearray()
# Serialize command data
if self.header: # Pack header
header = packHeader(self.header)
return header
def serialize(self, timestamp):
self.lastTxTime = timestamp
header = bytearray()
if self.header: # Pack header
header = packHeader(self.header)
return header + self.serializeMethod(
self.cmdData,
timestamp) # combine header with command payload data
def test_packHeader(self):
"""Test packing of all defined header types."""
# NodeHeader
nodeHeader = createHeader(self.nodeHeaderIn)
packedHeader = pack(headers[self.nodeHeaderIn[0]]['format'],
*self.nodeHeaderIn[1])
assert (packHeader(nodeHeader) == packedHeader)
# MinimalHeader
minimalHeader = createHeader(self.minimalHeaderIn)
packedHeader = pack(headers[self.minimalHeaderIn[0]]['format'],
*self.minimalHeaderIn[1])
assert (packHeader(minimalHeader) == packedHeader)
# SourceHeader
sourceHeader = createHeader(self.sourceHeaderIn)
packedHeader = pack(headers[self.sourceHeaderIn[0]]['format'],
*self.sourceHeaderIn[1])
assert (packHeader(sourceHeader) == packedHeader)
def test_packHeader(self):
"""Test packHeader method of Command class."""
command = Command(self.cmdId, self.cmdData, self.header)
# Test with header
header = command.packHeader()
assert (header == packHeader(command.header))
# Test without header
command.header = []
header = command.packHeader()
assert (len(header) == 0)
def test_processMsg(self):
"""Test processMsg method of TDMACmdProcessor."""
# Test processing of all TDMACmds
for cmdId in cmdsToTest:
cmdMsg = packHeader(testCmds[cmdId].header) + testCmds[cmdId].body
self.commProcessor.processMsg(cmdMsg,
args={
'nodeStatus': self.nodeStatus,
'comm': self.comm,
'clock': self.nodeParams.clock
})
if cmdId == TDMACmds['TimeOffset']:
sourceId = testCmds[cmdId].header['header']['sourceId']
assert (
self.nodeStatus[sourceId -
1].timeOffset == testCmds[cmdId].body[0] /
100.0)
elif cmdId == TDMACmds['TimeOffsetSummary']:
for i in range(len(testCmds[cmdId].cmdData['nodeStatus'])):
assert (self.nodeStatus[i].timeOffset == testCmds[cmdId].
cmdData['nodeStatus'][i].timeOffset)
elif cmdId == TDMACmds['MeshStatus']:
assert (self.nodeParams.commStartTime ==
testCmds[cmdId].cmdData['commStartTimeSec'])
# Resend and test that commStartTime is not updated once it has previously been set
cmdId = TDMACmds['MeshStatus']
self.nodeParams.commStartTime = testCmds[cmdId].cmdData[
'commStartTimeSec'] - 1
cmdMsg = packHeader(testCmds[cmdId].header) + testCmds[cmdId].body
self.commProcessor.processMsg(cmdMsg,
args={
'nodeStatus': self.nodeStatus,
'comm': self.comm,
'clock': self.nodeParams.clock
})
assert (self.nodeParams.commStartTime !=
testCmds[cmdId].cmdData['commStartTimeSec'])
def test_malformedCmd(self):
# Test command with improper message contents
badMsg = packHeader(
createHeader([
CmdDict[NodeCmds['GCSCmd']].header,
[NodeCmds['GCSCmd'], 0,
self.nodeParams.get_cmdCounter()]
])) # header only
assert (self.comm.processMsg(badMsg,
args={
'nodeStatus': self.nodeStatus,
'comm': self.comm,
'clock': self.nodeParams.clock
}) == False)
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_initComm(self):
"""Test initComm method of TDMAComm."""
# Confirm radio is off
assert (self.radio.mode == RadioMode.off)
# Init comm and check radio is now in receive
self.tdmaComm.initComm()
assert (self.radio.mode == RadioMode.receive)
# Send MeshStatus message and confirm that commStartTime is updated
meshStatusMsg = packHeader(testCmds[TDMACmds['MeshStatus']].header
) + testCmds[TDMACmds['MeshStatus']].body
commStartTime = testCmds[
TDMACmds['MeshStatus']].cmdData['commStartTimeSec']
assert (self.nodeParams.commStartTime != commStartTime
) # check that comm start times do not match
self.tdmaComm.bufferTxMsg(meshStatusMsg)
self.tdmaComm.sendBuffer()
time.sleep(0.1)
self.tdmaComm.initComm()
assert (self.nodeParams.commStartTime == commStartTime
) # check that comm start times now match
def test_checkCmdCounter(self):
"""Test checkCmdCounter method."""
# Check that command relay buffer is empty
assert (self.comm.cmdRelayBuffer == bytearray())
### Test method with command that includes a command counter
cmdMsg = packHeader(testCmds[PixhawkCmds['GCSCmd']].header) + testCmds[
PixhawkCmds['GCSCmd']].body
checkCmdCounter(self.commProcessor,
testCmds[PixhawkCmds['GCSCmd']].header['header'],
cmdMsg, self.comm)
encodedMsg = createEncodedMsg(cmdMsg)
assert (self.comm.cmdRelayBuffer == encodedMsg)
# Resend with new counter to check cmd is appended to buffer
cmdCounter = testCmds[
PixhawkCmds['GCSCmd']].header['header']['cmdCounter']
header, cmdMsg = self.updateCmdCounterValue(
cmdCounter + 1, deepcopy(testCmds[PixhawkCmds['GCSCmd']].header),
testCmds[PixhawkCmds['GCSCmd']].body)
checkCmdCounter(self.commProcessor, header['header'], cmdMsg,
self.comm)
encodedMsg2 = createEncodedMsg(cmdMsg)
assert (self.comm.cmdRelayBuffer == encodedMsg + encodedMsg2)
## Test various counter values to test acceptance behavior
# Command counter == 1, stored counter == 0
self.comm.cmdRelayBuffer = bytearray()
self.nodeParams.cmdHistory.append(
0) # place known cmd counter value in history
header, cmdMsg = self.updateCmdCounterValue(
1, deepcopy(testCmds[PixhawkCmds['GCSCmd']].header),
testCmds[PixhawkCmds['GCSCmd']].body)
checkCmdCounter(self.commProcessor, header['header'], cmdMsg,
self.comm)
assert (len(self.comm.cmdRelayBuffer) > 0) # cmd put in relay buffer
# Command counter == 1, stored counter == 1
self.comm.cmdRelayBuffer = bytearray()
checkCmdCounter(self.commProcessor, header['header'], cmdMsg,
self.comm)
assert (len(self.comm.cmdRelayBuffer) == 0
) # cmd not put in relay buffer
# Command counter == 255, stored counter == 1
self.comm.cmdRelayBuffer = bytearray()
header, cmdMsg = self.updateCmdCounterValue(
255, deepcopy(testCmds[PixhawkCmds['GCSCmd']].header),
testCmds[PixhawkCmds['GCSCmd']].body)
checkCmdCounter(self.commProcessor, header['header'], cmdMsg,
self.comm)
assert (len(self.comm.cmdRelayBuffer) > 0) # cmd put in relay buffer
# Command counter == 1, stored counter == 255
header, cmdMsg = self.updateCmdCounterValue(
1, deepcopy(testCmds[PixhawkCmds['GCSCmd']].header),
testCmds[PixhawkCmds['GCSCmd']].body)
checkCmdCounter(self.commProcessor, header['header'], cmdMsg,
self.comm)
assert (len(self.comm.cmdRelayBuffer) > 0) # cmd put in relay buffer
# Command counter == cmdCounterThreshold, storedcounter == 255 (DEPRECATED)
#self.nodeParams.set_cmdCounter(255)
#self.comm.cmdRelayBuffer = bytearray()
#header, cmdMsg = self.updateCmdCounterValue(self.nodeParams.cmdCounterThreshold, deepcopy(testCmds[PixhawkCmds['GCSCmd']].header), testCmds[PixhawkCmds['GCSCmd']].body)
#assert(len(self.comm.cmdRelayBuffer) == 0) # cmd not put in relay buffer
### Send command that should not be relayed
self.comm.cmdRelayBuffer = bytearray()
self.nodeParams._cmdCounter = 0
cmdMsg = packHeader(testCmds[PixhawkCmds['FormationCmd']].header
) + testCmds[PixhawkCmds['FormationCmd']].body
checkCmdCounter(self.commProcessor,
testCmds[PixhawkCmds['FormationCmd']].header['header'],
cmdMsg, self.comm)
assert (len(self.comm.cmdRelayBuffer) == 0
) # cmd not put in relay buffer
def updateCmdCounterValue(self, newCounter, header, body):
header['header']['cmdCounter'] = newCounter
return header, packHeader(header) + body
def setup_method(self, method):
# Test command
self.cmdId = PixhawkCmds['FormationCmd']
self.command = testCmds[self.cmdId]
self.headerBytes = packHeader(self.command.header)
self.msgBytes = self.headerBytes + self.command.body