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