class TestFormationClock:
def setup_method(self, method):
# Create FormationClock instance
if method.__name__ == "test_referenceClock":
self.clock = FormationClock(time.time())
else:
self.clock = FormationClock()
def test_standardClock(self):
"""Test non-referenced clock functionality."""
startTime = time.time()
clockTime = self.clock.getTime()
endTime = time.time()
assert (clockTime >= startTime and clockTime <= endTime
) # Check that returned time is correct
def test_referenceClock(self):
"""Test referenced clock functionality."""
assert (self.clock.getTime() <= time.time() - self.clock.referenceTime)
def test_getOffset(self):
"""Test time offset functionality."""
offset = self.clock.getOffset()
assert (offset == None or (isinstance(offset, Real) and offset > 0))
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)]
class TestFormationClock:
def setup_method(self, method):
# Create FormationClock instance
if method.__name__ == "test_referenceClock":
self.clock = FormationClock(time.time())
else:
self.clock = FormationClock()
def test_nonreferenceClock(self):
"""Test non-referenced clock functionality."""
startTime = time.time()
clockTime = self.clock.getTime()
endTime = time.time()
assert (clockTime >= startTime and clockTime <= endTime
) # Check that returned time is correct
def test_referenceClock(self):
"""Test referenced clock functionality."""
assert (self.clock.getTime() <= time.time() - self.clock.referenceTime)
def test_getOffset(self):
"""Test time offset functionality."""
# Test zero offset returned when no time source
self.clock.timeSource = None
offset = self.clock.getOffset()
assert (abs(offset) < 0.00001)
def setup_method(self, method):
# Create FormationClock instance
if method.__name__ == "test_referenceClock":
self.clock = FormationClock(time.time())
else:
self.clock = FormationClock()
def setupParams(self):
self.configConfirmed = False
#self.commStartTime = None
#self.cmdRelayBuffer = []
self.cmdHistory = deque(
maxlen=100) # FIFO list of last commands received
self.cmdResponse = dict()
# Initialize node status
self.initNodeStatus()
# Formation clock
self.clock = FormationClock()
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_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 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_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()
class NodeParams():
def __init__(self, configFile=[], config=[]):
if configFile:
self.config = NodeConfig(configFile)
elif config:
self.config = config
self.setupParams()
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 get_cmdCounter(self):
#if self.commStartTime: # time-based counter
# return int((self.clock.getTime() - self.commStartTime)*1000)
#else: # random counter
return random.randint(1, 65536)
def updateStatus(self):
"""Update status information."""
self.nodeStatus[self.config.nodeId-1].status = 0
if (self.configConfirmed == True):
self.nodeStatus[self.config.nodeId-1].status += 64 # bit 6
def checkNodeLinks(self):
"""Checks status of links to other nodes."""
thisNode = self.config.nodeId - 1
for i in range(self.config.maxNumNodes):
# Check for direct link
if (self.nodeStatus[i].present and (self.clock.getTime() - self.nodeStatus[i].lastMsgRcvdTime) < self.config.commConfig['linkTimeout']):
self.linkStatus[thisNode][i] = LinkStatus.GoodLink
# Check for indirect link
elif (self.nodeStatus[i].updating == True): # state data is updating, so at least an indirect link
self.linkStatus[thisNode][i] = LinkStatus.IndirectLink
else: # no link
self.linkStatus[thisNode][i] = LinkStatus.NoLink
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 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
class NodeParams():
def __init__(self, configFile=[], config=[]):
if configFile:
self.config = NodeConfig(configFile)
elif config:
self.config = config
self.setupParams()
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 get_cmdCounter(self):
#if self.commStartTime: # time-based counter
# return int((self.clock.getTime() - self.commStartTime)*1000)
#else: # random counter
return random.randint(1, 65536)
def checkTimeOffset(self, offset=None):
if (self.config.commType == "TDMA"): # TDMA time offset failsafe
if self.config.commConfig[
'fpga'] and self.fpgaFailsafePin: # TDMA time controlled by FPGA
if (GPIO.input(self.fpgaFailsafePin) == 0): # failsafe not set
self.timeOffsetTimer = None # reset timer
else: # failsafe condition set
if self.timeOffsetTimer:
if self.clock.getTime(
) - self.timeOffsetTimer > self.config.commConfig[
'offsetTimeout']: # No time offset reading for longer than allowed
self.tdmaFailsafe = True # Set TDMA failsafe flag
return 3
else: # start timer
self.timeOffsetTimer = self.clock.getTime()
else:
if offset == None: # offset not provided so attempt to get offset from clock
offset = self.clock.getOffset()
if offset != None: # time offset available
self.timeOffsetTimer = None # reset time offset timer
self.nodeStatus[self.config.nodeId - 1].timeOffset = offset
if abs(self.nodeStatus[self.config.nodeId - 1].timeOffset
) > self.config.commConfig['operateSyncBound']:
return 1
else: # no offset available
self.nodeStatus[self.config.nodeId -
1].timeOffset = 127 # Error value
# Check time offset timer
if self.timeOffsetTimer:
#print(self.clock.getTime() - self.timeOffsetTimer)
if self.clock.getTime(
) - self.timeOffsetTimer > self.config.commConfig[
'offsetTimeout']: # No time offset reading for longer than allowed
self.tdmaFailsafe = True # Set TDMA failsafe flag
return 2
else: # start timer
self.timeOffsetTimer = self.clock.getTime()
class NodeParams():
def __init__(self, configFile=[], config=[]):
if configFile:
self.config = NodeConfig(configFile)
elif config:
self.config = config
# Configuration update holder
self.newConfig = None
# Mesh status
self.restartTime = None
self.restartRequested = False
self.restartConfirmed = False
self.setupParams()
def setupParams(self):
self.configConfirmed = False
#self.commStartTime = None
#self.cmdRelayBuffer = []
self.cmdHistory = deque(
maxlen=100) # FIFO list of last commands received
self.cmdResponse = dict()
# Initialize node status
self.initNodeStatus()
# Formation clock
self.clock = FormationClock()
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 get_cmdCounter(self):
#if self.commStartTime: # time-based counter
# return int((self.clock.getTime() - self.commStartTime)*1000)
#else: # random counter
cmdCounter = random.randint(1, 65536)
# Add counter value to history
self.cmdHistory.append(cmdCounter)
return cmdCounter
def loadConfig(self, newConfig, hashValue):
'''Verify and queue new configuration for loading.'''
# Convert from protobuf to json
jsonConfig = NodeConfig.fromProtoBuf(newConfig)
jsonConfig['node'][
'nodeId'] = self.config.nodeId # Don't overwrite node id via update
# Create, verify, and store new configuration
newConfig = NodeConfig(configData=jsonConfig)
if (newConfig.calculateHash() == hashValue
and newConfig.loadSuccess): # configuration verified
#self.newConfig = newConfig
return [True, newConfig]
else:
#self.newConfig = None
return [False, None]
def updateConfig(self):
retValue = False
if (self.newConfig and self.newConfig.loadSuccess
): # load pending configuration update
print("Node " + str(self.config.nodeId) +
": Updating to new configuration")
self.config = self.newConfig
retValue = True
self.newConfig = None
return retValue
def updateStatus(self):
"""Update status information."""
self.nodeStatus[self.config.nodeId - 1].status = 0
if (self.configConfirmed == True):
self.nodeStatus[self.config.nodeId - 1].status += 64 # bit 6
def checkNodeLinks(self):
"""Checks status of links to other nodes."""
thisNode = self.config.nodeId - 1
for i in range(self.config.maxNumNodes):
# Check for direct link
if (self.nodeStatus[i].present and
(self.clock.getTime() - self.nodeStatus[i].lastMsgRcvdTime) <
self.config.commConfig['linkTimeout']):
self.linkStatus[thisNode][i] = LinkStatus.GoodLink
# Check for indirect link
elif (self.nodeStatus[i].updating == True
): # state data is updating, so at least an indirect link
self.linkStatus[thisNode][i] = LinkStatus.IndirectLink
else: # no link
self.linkStatus[thisNode][i] = LinkStatus.NoLink
def addCmdResponse(self, cmdCounter, cmdResponse, sourceId):
if (cmdCounter in self.cmdResponse): # update existing responses
self.cmdResponse[cmdCounter][sourceId] = cmdResponse
else: # add new command response
self.cmdResponse[cmdCounter] = dict()
self.cmdResponse[cmdCounter][sourceId] = cmdResponse