class TestMsgParser:
def setup_method(self, method):
# Create SerialMsgParser instance
self.msgParser = MsgParser({'parseMsgMax': 10})
def test_parseSerialMsg(self):
"""Test parseSerialMsg method of MsgParser."""
msg = b'12345'
msgStart = 1
self.msgParser.parseSerialMsg(msg, msgStart)
assert (len(self.msgParser.parsedMsgs) == 1)
assert (self.msgParser.parsedMsgs[0] == msg[msgStart:])
def test_encodeMsg(self):
"""Test encode passthrough of message."""
msg = b'12345'
assert (self.msgParser.encodeMsg(msg) == msg)
def test_parseMsgs(self):
"""Test parseMsgs method of MsgParser."""
msg = b'12345'
msg = b'12345'
msg2 = b'6789'
self.msgParser.parseMsgs(msg)
self.msgParser.parseMsgs(msg2)
assert (len(self.msgParser.parsedMsgs) == 2)
assert (self.msgParser.parsedMsgs[0] == msg)
assert (self.msgParser.parsedMsgs[1] == msg2)
class SerialComm(object):
"""Serial communication wrapper class that provides for serializing data to send over serial data links.
This class is the base class for serial communication and provides reading and writing functionality over the provided serial connection. Messages being sent and received are relayed using a SLIP message format which provides a CRC for data quality checking.
Attributes:
uartNumBytesToRead: Maximum number of bytes to attempt to read from the serial connection.
serialConn: Serial connection instance used by this class for communication.
serMsgParser: Serial message parser that parses raw serial data and looks for valid SLIP messages.
lastMsgSentTime: Time that the last serial message was sent over this connection.
msgCounter: Counter of the number of messages sent over this serial connection.
msgOut: SLIPmsg instance used for storing messages prior to sending over serial line.
rxBuffer: Raw serial data read from this connection.
msgEnd: Array location of end of raw serial message decoded from read serial bytes stored in rxBuffer.
"""
def __init__(self, msgProcessors, nodeParams, radio, parser=None):
self.radio = radio
if (parser):
self.msgParser = parser
else: # use default parser
self.msgParser = MsgParser({'parseMsgMax': 10})
self.lastMsgSentTime = 0
self.msgCounter = 0
# Message processing
self.cmdQueue = OrderedDict()
self.msgProcessors = msgProcessors
self.nodeParams = nodeParams
# Command buffers (external commands from node host)
self.cmdBuffer = dict()
self.cmdRelayBuffer = bytearray() # data relay
@property
def radio(self):
return self.__radio
@radio.setter
def radio(self, radio):
if (isinstance(radio, Radio)):
self.__radio = radio
else:
raise InvalidRadio("Invalid Radio input provided to SerialComm.")
def readMsgs(self):
"""Reads and parses any received serial messages."""
# Read bytes
self.readBytes(False)
# Parse messages
self.parseMsgs()
def parseMsgs(self):
# Parse messages
self.msgParser.parseMsgs(self.radio.getRxBytes())
# Clear rx buffer
self.radio.clearRxBuffer()
def readBytes(self, bufferFlag=False):
"""Reads raw bytes from radio"""
self.radio.readBytes(bufferFlag)
def sendBytes(self, msgBytes):
"""Send raw bytes without further packaging."""
self.radio.sendMsg(msgBytes)
def sendMsg(self, msgBytes):
"""Wraps provided data into a SLIPmsg and then sends the message over the serial link
Args:
timestamp: Time of message.
cmdId: Command message type identifier.
msgBytes: Data bytes to be sent in serial message.
"""
if len(msgBytes) > 0:
self.lastMsgSentTime = self.nodeParams.clock.getTime()
self.msgCounter = (self.msgCounter + 1) % 256
msgOut = self.msgParser.encodeMsg(msgBytes)
self.sendBytes(msgOut)
def sendBuffer(self):
"""Send data in transmission buffer over serial connection."""
self.radio.sendBuffer()
def processBuffers(self):
if self.cmdBuffer: # command buffer
#noRepeatCmds = []
for key in self.cmdBuffer:
self.bufferTxMsg(self.cmdBuffer[key]['bytes'])
#if self.cmdBuffer[key]['txInterval'] == 0: # no repeat
# noRepeatCmds.append(key)
#for key in noRepeatCmds: # remove non-repeat commands
# self.cmdBuffer.pop(key)
self.cmdBuffer = dict()
if self.cmdRelayBuffer: # Add commands to tx buffer and clear relay buffer
#for cmd in cmdRelayBuffer:
# self.bufferTxMsg(cmd)
self.radio.bufferTxMsg(self.cmdRelayBuffer)
self.cmdRelayBuffer = bytearray()
def bufferTxMsg(self, msgBytes):
"""Add bytes to transmission buffer."""
if msgBytes:
msgOut = self.msgParser.encodeMsg(msgBytes)
self.radio.bufferTxMsg(msgOut)
def execute(self):
"""Execute communication cycle."""
pass
def processMsgs(self, args=[]):
"""Read and process any received messages."""
self.readMsgs()
if self.msgParser.parsedMsgs:
for i in range(len(self.msgParser.parsedMsgs)):
self.processMsg(self.msgParser.parsedMsgs.pop(0), args)
def processMsg(self, msg, args):
"""Processes parsed serial messages.
Args:
msg: Serial message to be processed.
args: Other arguments needed for processing serial message.
"""
if len(msg) > 0:
# Parse command id
cmdId = unpack('B', msg[0:1])[0]
# Pass command to proper processor
for processor in self.msgProcessors:
if cmdId in processor['cmdList'].values():
return processor['msgProcessor'](self, cmdId, msg, args)
break
return False
class TDMAComm(SerialComm):
def __init__(self, msgProcessors, radio, msgParser, nodeParams):
if not msgProcessors:
msgProcessors = [NodeCmdProcessor, TDMACmdProcessor]
super().__init__(msgProcessors, nodeParams, radio, parser=msgParser)
self.reinit(nodeParams)
def reinit(self, nodeParams, initDelay=None):
self.nodeParams = nodeParams
# TDMA config
self.tdmaMode = TDMAMode.sleep
self.frameStartTime = []
self.commStartTime = None # time that TDMA comm was started - initialized manually by first node or parsed from messages received for nodes joining existing mesh
self.networkConfigConfirmed = None
self.networkConfigRcvd = False
self.maxNumSlots = nodeParams.config.commConfig['maxNumSlots'] # Maximum number of slots
self.enableLength = nodeParams.config.commConfig['enableLength']
self.slotTime = 0.0
self.slotNum = 1
self.slotStartTime = 0.0
self.networkMsgQueue = []
# TDMA Frame variables
self.frameTime = 0.0
self.frameCount = 0
self.frameLength = nodeParams.config.commConfig['frameLength']
self.cycleLength = nodeParams.config.commConfig['cycleLength']
self.adminEnabled = nodeParams.config.commConfig['adminEnable']
self.adminLength = nodeParams.config.commConfig['adminLength']
# TDMA status
self.tdmaStatus = TDMAStatus.nominal
self.tdmaFailsafe = False
self.timeOffsetTimer = None
self.frameExceedanceCount = 0
# Mesh initialization variables
self.inited = False
self.initTimeToWait = nodeParams.config.commConfig['initTimeToWait'] # Time to wait before assuming no existing mesh network
self.initStartTime = None
# Mesh network commands
self.tdmaCmds = dict()
self.tdmaCmdParser = MsgParser({'parseMsgMax': nodeParams.config.parseMsgMax}, SLIPMsg(2048))
# Transmit period variables
self.transmitSlot = nodeParams.config.commConfig['transmitSlot'] # Slot in cycle that this node is schedule to transmit
self.beginTxTime = self.enableLength + nodeParams.config.commConfig['preTxGuardLength']
self.endTxTime = self.beginTxTime + nodeParams.config.commConfig['txLength']
self.transmitComplete = False
# Receive period variables
self.beginRxTime = self.enableLength
self.endRxTime = self.beginRxTime + nodeParams.config.commConfig['rxLength']
self.slotLength = nodeParams.config.commConfig['slotLength'] # total length of slot
self.rxLength = nodeParams.config.commConfig['rxLength']
self.rxReadTime = self.beginTxTime + nodeParams.config.commConfig['rxDelay'] # time to begin reading serial
self.receiveComplete = False
# Current read position in radio rx buffer
self.rxBufferReadPos = 0
# Mesh header information
self.meshPacketHeaderFormat = '<BBHHHB'
self.meshHeaderLen = struct.calcsize(self.meshPacketHeaderFormat)
# Block Tx information
self.blockTx = None
self.blockTxInProgress = False
self.blockTxPacketStatus = dict() # stores transmitted packet status until all receipt requests received
self.blockTxPacketReceipts = []
# Comm enable flag
self.enabled = True
# Mesh data in/out buffers
#self.meshQueueIn = [b''] * (self.maxNumSlots + 1)
self.meshQueueIn = []
self.hostBuffer = bytearray()
self.blockTxOut = dict()
# Network graph
#self.meshGraph = [0] * self.maxNumSlots # timestamps of last received message from each other node
self.lastGraphUpdate = 0.0
self.meshPaths = [[]*self.maxNumSlots] * self.maxNumSlots
self.neighbors = []
# Delay init (for full network restart)
if (initDelay):
time.sleep(initDelay)
# Network metrics
self.bytesSent = 0
self.bytesRcvd = 0
def execute(self):
"""Execute communication functions."""
currentTime = time.time()
# Initialize mesh network
if self.inited == False:
self.init(currentTime)
return
else: # perform TDMA execution logic
self.executeTDMAComm(currentTime)
def updateFrameTime(self, currentTime):
self.frameTime = currentTime - self.frameStartTime
if self.frameTime >= self.frameLength: # Start new frame
#print(str(currentTime) + ": Node " + str(self.nodeParams.config.nodeId) + " - New frame started")
self.syncTDMAFrame(currentTime)
#print("Node " + str(self.nodeParams.config.nodeId) + " - Previous frame data throughput(in/out): ", self.bytesRcvd, self.bytesSent)
self.bytesSent = 0
self.bytesRcvd = 0
if self.frameTime < (self.cycleLength + self.adminLength): # active portion of frame
return 0
else:
return 1
#elif self.frameTime < (self.cycleLength + self.adminLength): # admin period
# framePeriod = 1
#else: # sleep period
# framePeriod = 2
#return framePeriod
def executeTDMAComm(self, currentTime):
"""Execute TDMA communication scheme."""
# Check for block transfers
#self.monitorBlockTx()
# Update frame time
frameStatus = self.updateFrameTime(currentTime)
# Check for mode updates
self.updateMode(self.frameTime)
# Execute sleep
if (frameStatus == 1):
self.sleep()
return
#elif (frameStatus == 2):
# self.sleep()
# return
# Perform mode specific behavior
for case in switch(self.tdmaMode):
if case(TDMAMode.admin):
self.admin()
break
if case(TDMAMode.sleep):
# Set radio to sleep mode
self.radio.setMode(RadioMode.sleep)
break
if case(TDMAMode.init):
# Prepare radio to receive or transmit
if self.slotNum == self.transmitSlot:
# Set radio to transmit mode
self.radio.setMode(RadioMode.transmit)
else:
# Set radio to receive mode
self.radio.setMode(RadioMode.receive)
break
if case(TDMAMode.receive):
# Read data if TDMA message end not yet found
if self.receiveComplete == False and self.slotTime >= self.rxReadTime:
self.radio.setMode(RadioMode.receive) # set radio mode
# Delay so we aren't hammering the radio
#remainingRxTime = self.rxLength - (self.slotTime - self.enableLength)
#time.sleep(remainingRxTime*0.2)
self.receiveComplete = self.readMsgs()
if self.receiveComplete == True:
#print("Node " + str(self.nodeParams.config.nodeId) + " - End of receive slot " + str(self.slotNum))
# Set radio to sleep
self.radio.setMode(RadioMode.sleep)
break
if case(TDMAMode.transmit):
# Send data
if self.transmitComplete == False:
self.radio.setMode(RadioMode.transmit) # set radio mode
self.sendMsgs()
else: # Set radio to sleep
self.radio.setMode(RadioMode.sleep)
break
if case(TDMAMode.failsafe): # Read only failsafe mode
# Enable radio in receive mode and read data
self.radio.setMode(RadioMode.receive)
self.readMsgs()
break
def admin(self):
# Disable radio upon admin completion
if (self.receiveComplete == True or self.transmitComplete == True):
self.radio.setMode(RadioMode.sleep)
return
# Admin period process control
adminTime = self.frameTime - (self.cycleLength)
if (self.blockTxInProgress): # execute block transfer logic
self.executeBlockTx(adminTime)
else:
self.executeAdmin(adminTime)
def executeAdmin(self, adminTime):
adminLength = self.nodeParams.config.commConfig['adminBytesMaxLength'] + self.nodeParams.config.commConfig['msgPayloadMaxLength']
controlNode = int(self.frameCount % (self.maxNumSlots+1) + 1) # each node gets a round as admin controller followed by one open opportunity for all
if (adminTime < self.enableLength): # Initialize radio
if (controlNode == self.nodeParams.config.nodeId): # This node is in control
# Set radio to transmit mode
self.radio.setMode(RadioMode.transmit)
else: # listening this period
# Set radio to receive mode
self.radio.setMode(RadioMode.receive)
else: # execute admin period
if (controlNode == self.nodeParams.config.nodeId): # This node is in control
if (self.transmitComplete == True):
self.radio.setMode(RadioMode.sleep)
elif (adminTime >= self.beginTxTime): # Execute admin transmission
self.radio.setMode(RadioMode.transmit) # set radio mode
adminBytes = self.packageAdminData(adminLength)
packetBytes = self.createMeshPacket(0, b'', adminBytes, self.nodeParams.config.nodeId)
if (packetBytes): # if packet is of non-zero length
self.bufferTxMsg(packetBytes)
#self.radio.bufferTxMsg(HDLC_END_TDMA) # append end of message byte
self.bytesSent += self.sendBuffer()
self.transmitComplete = True
#print("Node " + str(self.nodeParams.config.nodeId) + " - Admin transmit complete")
else: # other nodes in control this admin period
if (self.receiveComplete == True):
self.radio.setMode(RadioMode.sleep)
elif (adminTime >= self.rxReadTime):
self.radio.setMode(RadioMode.receive) # set radio mode
self.receiveComplete = self.readMsgs()
if (self.receiveComplete):
#print("Node " + str(self.nodeParams.config.nodeId) + " - Admin receive complete")
self.radio.setMode(RadioMode.sleep)
def executeBlockTx(self, adminTime):
if (adminTime < self.enableLength): # Initialize radio
if (self.blockTx.srcId == self.nodeParams.config.nodeId): # This node is transmiting
# Set radio to transmit mode
self.radio.setMode(RadioMode.transmit)
else: # receiving
# Set radio to receive mode
self.radio.setMode(RadioMode.receive)
else: # execute block transmit period
if (self.blockTx.srcId == self.nodeParams.config.nodeId): # This node is sending
if (self.transmitComplete == True):
self.radio.setMode(RadioMode.sleep)
elif (adminTime >= self.beginTxTime): # Execute transmission
self.radio.setMode(RadioMode.transmit) # set radio mode
packetBytes = self.sendBlockTxPacket()
if (packetBytes): # if packet is of non-zero length
self.bufferTxMsg(packetBytes)
#self.radio.bufferTxMsg(HDLC_END_TDMA) # append end of message byte
self.bytesSent += self.sendBuffer()
self.transmitComplete = True
else:
self.radio.setMode(RadioMode.sleep)
else:
if (self.receiveComplete == True):
self.radio.setMode(RadioMode.sleep)
elif (adminTime >= self.rxReadTime):
self.radio.setMode(RadioMode.receive) # set radio mode
self.receiveComplete = self.readMsgs()
if (self.receiveComplete):
#print("Node " + str(self.nodeParams.config.nodeId) + " - Block packet receive complete")
self.radio.setMode(RadioMode.sleep)
def init(self, currentTime):
if not self.commStartTime or not self.networkConfigConfirmed: # Mesh not initialized
self.initComm(currentTime)
return
else: # Join existing mesh
self.initMesh()
def initMesh(self, currentTime=time.time()):
"""Initialize node mesh networks."""
# Create tdma comm messages
flooredStartTime = math.floor(self.commStartTime)
self.tdmaCmds[TDMACmds['MeshStatus']] = Command(TDMACmds['MeshStatus'], {'commStartTimeSec': int(flooredStartTime), 'status': self.tdmaStatus, 'configHash': self.nodeParams.config.calculateHash()}, [TDMACmds['MeshStatus'], self.nodeParams.config.nodeId], self.nodeParams.config.commConfig['statusTxInterval'])
self.tdmaCmds[TDMACmds['LinkStatus']] = Command(TDMACmds['LinkStatus'], {'linkStatus': self.nodeParams.linkStatus, 'nodeId': self.nodeParams.config.nodeId}, [TDMACmds['LinkStatus'], self.nodeParams.config.nodeId], self.nodeParams.config.commConfig['linksTxInterval'])
if self.nodeParams.config.nodeId != 0: # stop ground node from broadcasting time offset
self.tdmaCmds[TDMACmds['TimeOffset']] = Command(TDMACmds['TimeOffset'], {'nodeStatus': self.nodeParams.nodeStatus[self.nodeParams.config.nodeId-1]}, [TDMACmds['TimeOffset'], self.nodeParams.config.nodeId], self.nodeParams.config.commConfig['offsetTxInterval'])
# Current network configuration message
self.initialConfigTxTime = self.commStartTime + self.nodeParams.config.nodeId * self.nodeParams.config.commConfig['configTxInterval']
configHash = self.nodeParams.config.calculateHash()
config_pb = NodeConfig.toProtoBuf(self.nodeParams.config.rawConfig).SerializeToString()
self.tdmaCmds[TDMACmds['CurrentConfig']] = Command(TDMACmds['CurrentConfig'], {'config': config_pb, 'configLength': len(config_pb), 'configHash': configHash, 'hashLength': self.nodeParams.config.hashSize}, [TDMACmds['CurrentConfig'], self.nodeParams.config.nodeId], self.maxNumSlots * self.nodeParams.config.commConfig['configTxInterval'])
# Determine where in frame mesh network currently is
self.syncTDMAFrame(currentTime)
self.inited = True
print("Node " + str(self.nodeParams.config.nodeId) + " - Initializing comm")
def initComm(self, currentTime):
if self.initStartTime == None:
# Start mesh initialization timer
self.initStartTime = currentTime
print("Node " + str(self.nodeParams.config.nodeId) + " - Starting initialization timer")
return
elif (currentTime - self.initStartTime) >= self.initTimeToWait and not self.commStartTime:
# Assume no existing mesh and initialize network
self.commStartTime = math.ceil(currentTime)
print("Node " + str(self.nodeParams.config.nodeId) + " - Initializing new mesh network")
self.initMesh()
else: # Wait for initialization timer to lapse
# Turn on radios and check for comm messages
self.checkForInit()
# TODO - Add branch to read correct configuration from network
def checkForInit(self):
# Look for tdma status message
self.radio.setMode(RadioMode.receive)
self.readBytes(True)
if self.radio.bytesInRxBuffer > 0:
self.processMsgs()
#while self.msgParser.parsedMsgs:
# msg = self.msgParser.parsedMsgs.pop(0)
# cmdId = struct.unpack('=B',msg[0:1])[0]
# if cmdId == TDMACmds['MeshStatus']:
# print("Mesh status received")
# self.processMsg(msg, {'nodeStatus': self.nodeParams.nodeStatus, 'comm': self, 'clock': self.nodeParams.clock})
def syncTDMAFrame(self, currentTime=time.time()):
"""Determine where in frame mesh network currently is to ensure time sync."""
self.frameTime = (currentTime - self.commStartTime)%self.frameLength
self.frameStartTime = currentTime - self.frameTime
self.frameCount = math.floor(currentTime - self.commStartTime) / self.frameLength
#print("Node " + str(self.nodeParams.config.nodeId) + " " + str(self.frameStartTime),"- Frame start")
# Update periodic mesh messages
if (TDMACmds['MeshStatus'] in self.tdmaCmds):
self.tdmaCmds[TDMACmds['MeshStatus']].cmdData['status'] = self.tdmaStatus
self.tdmaCmds[TDMACmds['MeshStatus']].cmdData['configHash'] = self.nodeParams.config.calculateHash()
if (TDMACmds['LinkStatus'] in self.tdmaCmds):
self.tdmaCmds[TDMACmds['LinkStatus']].cmdData['linkStatus'] = self.nodeParams.linkStatus
# Reset buffer read position
self.rxBufferReadPos = 0
# Check for tdma failsafe
self.checkTimeOffset()
def sleep(self):
"""Sleep until end of frame."""
#print("Node " + str(self.nodeParams.config.nodeId) + " - In sleep method.")
# Update mesh paths
if ((self.nodeParams.clock.getTime() - self.lastGraphUpdate) > self.nodeParams.config.commConfig['linksTxInterval']):
self.updateShortestPaths()
# Process any received messages
self.processMsgs()
# Update block transmit status
if (self.blockTxInProgress):
# Process block transmit receipts
if (self.blockTx.srcId == self.nodeParams.config.nodeId):
self.processBlockTxReceipts()
self.updateBlockTxStatus()
# Sleep until next frame to save CPU usage
remainingFrameTime = (self.frameLength - (self.nodeParams.clock.getTime() - self.frameStartTime))
if (remainingFrameTime > 0.010):
# Sleep remaining frame length minus some delta to ensure waking in time
time.sleep(remainingFrameTime - 0.010)
elif (remainingFrameTime < -0.010):
print("WARNING: Frame length exceeded! Exceedance- " + str(abs(remainingFrameTime)))
self.frameExceedanceCount += 1
def updateShortestPaths(self):
for node in range(self.maxNumSlots):
self.meshPaths[node] = findShortestPaths(self.maxNumSlots, self.nodeParams.linkStatus, node+1)
# Populate direct mesh network neightbors list for this node
if (node+1 == self.nodeParams.config.nodeId):
for paths in self.meshPaths[node]:
for path in paths:
if (len(path) > 1): # path exists
if (path[1] not in self.neighbors): # new neighbor
self.neighbors.append(path[1])
#print("Node", self.nodeParams.config.nodeId, "- Direct neighbors:", str(self.neighbors))
def updateMode(self, frameTime):
# Update slot
self.resetTDMASlot(frameTime)
# Check for TDMA failsafe
if (self.tdmaFailsafe == True):
self.setTDMAMode(TDMAMode.failsafe)
return
if (frameTime >= self.cycleLength): # Cycle complete
if (self.adminEnabled and frameTime < (self.cycleLength + self.adminLength)): # admin period
self.setTDMAMode(TDMAMode.admin)
else: # sleep period
self.setTDMAMode(TDMAMode.sleep)
#print str(frameTime) + " - Cycle complete, sleeping"
return
# Normal cycle sequence
if self.slotTime < self.enableLength: # Initialize comm at start of slot
self.setTDMAMode(TDMAMode.init)
else:
# Transmit slot
if self.slotNum == self.transmitSlot:
if self.slotTime >= self.beginTxTime:
if self.slotTime < self.endTxTime: # Begin transmitting
self.setTDMAMode(TDMAMode.transmit)
else: # Sleep
self.setTDMAMode(TDMAMode.sleep)
# Receive slot
else:
if self.slotTime >= self.beginRxTime: # begin receiviing
if self.slotTime < self.endRxTime: # receive
self.setTDMAMode(TDMAMode.receive)
else: # Sleep
self.setTDMAMode(TDMAMode.sleep)
#def resetTDMASlot(self, frameTime, currentTime=time.time(), slotNum=[]):
#def resetTDMASlot(self, frameTime, slotNum=None):
def resetTDMASlot(self, frameTime):
# Reset slot number
#if slotNum != None:
# if slotNum >= 1 and slotNum <= self.maxNumSlots:
# self.slotNum = int(slotNum)
# else: # invalid number
# raise InvalidTDMASlotNumber("Provided TDMA slot number is not valid")
#else:
#if self.slotStartTime:
# if (currentTime - self.slotStartTime) >= self.slotLength and self.slotNum < self.maxNumSlots:
# self.slotNum = int(frameTime/self.slotLength) + 1
#else:
if frameTime < self.cycleLength: # during cycle
self.slotNum = int(frameTime/self.slotLength) + 1
else: # post-cycle
self.slotNum = self.maxNumSlots
#self.slotStartTime = currentTime - (frameTime - (self.slotNum-1)*self.slotLength)
#self.slotTime = frameTime - self.slotStartTime
self.slotStartTime = (self.slotNum-1)*self.slotLength
self.slotTime = frameTime - self.slotStartTime
#print("Updating slot number: " + str(self.slotNum))
def setTDMAMode(self, mode):
if self.tdmaMode != mode:
#print("Setting mode:", mode)
self.tdmaMode = mode
#print str(self.slotTime) + " - TDMA mode change: " + str(self.tdmaMode)
if mode == TDMAMode.receive:
self.receiveComplete = False
elif mode == TDMAMode.transmit:
self.transmitComplete = False
elif mode == TDMAMode.admin:
self.receiveComplete = False
self.transmitComplete = False
#print str(time.time()) + ": " + str(frameTime) + " - Node " + str(self.nodeId) + " - Transmitting"
def sendMsgs(self):
if (self.enabled == False): # Don't send anything if disabled
return
# Send buffered and periodic commands
broadcastMsgSent = False
if self.tdmaMode == TDMAMode.transmit:
# Send periodic TDMA commands
#self.sendTDMACmds()
# Check queue for waiting outgoing messages
bytesSent = 0
bytesSent += self.processBuffers() # process relay and command buffers
for msg in self.meshQueueIn:
packetBytes = b''
if (bytesSent + len(msg.msgBytes) > self.nodeParams.config.commConfig['maxTransferSize']): # maximum transmit size reached
break
if (msg.destId == 0):
packetBytes = self.packageMeshPacket(msg.destId, msg.msgBytes)
broadcastMsgSent = True
elif (msg.destId != 0 and msg.msgBytes): # only send non-zero non-broadcast messages
packetBytes = self.packageMeshPacket(msg.destId, msg.msgBytes)
if (packetBytes): # if packet is of non-zero length
self.bufferTxMsg(packetBytes)
# Clear queue
self.meshQueueIn = []
# Send broadcast message
if (broadcastMsgSent == False): # send a broadcast message for network administration
if (bytesSent < self.nodeParams.config.commConfig['maxTransferSize']): # maximum transmit size not reached
packetBytes = self.packageMeshPacket(0, b'')
if (packetBytes): # if packet is of non-zero length
self.bufferTxMsg(packetBytes)
#self.radio.bufferTxMsg(HDLC_END_TDMA) # append end of message byte
#print("Node " + str(self.nodeParams.config.nodeId) + " - Number of bytes sent: " + str(len(self.radio.txBuffer)))
self.bytesSent += self.sendBuffer()
# End transmit period
self.transmitComplete = True
else:
pass
#print "Slot " + str(self.slotNum) + " - Node " + str(self.nodeId) + " - Can't send. Wrong mode: " + str(self.tdmaMode)
def packageMeshPacket(self, destId, msgBytes):
adminBytes = b''
if (destId == 0): # package network admin messages into broadcast message
adminBytes = self.packageAdminData(self.nodeParams.config.commConfig['adminBytesMaxLength'])
#adminBytes = self.sendTDMACmds()
return self.createMeshPacket(destId, msgBytes, adminBytes, self.nodeParams.config.nodeId)
def createMeshPacket(self, destId, msgBytes, adminBytes, sourceId, statusByte=0):
if (len(adminBytes) == 0 and len(msgBytes) == 0): # do not send empty message
return bytearray()
# Create mesh packet header
packetHeader = struct.pack(self.meshPacketHeaderFormat, sourceId, destId, len(adminBytes), len(msgBytes), self.nodeParams.get_cmdCounter(), statusByte)
# Return mesh packet
return bytearray(packetHeader + adminBytes + msgBytes)
def parseMeshPacket(self, packetBytes):
"""Parse out a mesh packet."""
# Parse mesh packet header
packetHeader = dict()
packetHeaderContents = struct.unpack(self.meshPacketHeaderFormat, packetBytes[0:self.meshHeaderLen])
packetHeader = {'sourceId': packetHeaderContents[0], 'destId': packetHeaderContents[1], 'adminLength': packetHeaderContents[2], 'payloadLength': packetHeaderContents[3], 'cmdCounter': packetHeaderContents[4], 'statusByte': packetHeaderContents[5]}
# Validate message length
if (len(packetBytes) == (self.meshHeaderLen + packetHeader['adminLength'] + packetHeader['payloadLength'])): # message length is valid
adminBytes = packetBytes[self.meshHeaderLen:self.meshHeaderLen + packetHeader['adminLength']]
messageBytes = packetBytes[self.meshHeaderLen + packetHeader['adminLength']:]
return True, packetHeader, adminBytes, messageBytes
else:
return False, packetHeader, [], []
def readMsgs(self):
"""Read from serial connection and look for end of message value."""
self.bytesRcvd += self.radio.readBytes(True)
# Look for TDMA message end indicator
#for i in range(self.rxBufferReadPos, self.radio.bytesInRxBuffer):
# self.rxBufferReadPos = i+1
#byte = self.rxBuffer[i:i+1]
# if self.radio.rxBuffer[i:i+1] == HDLC_END_TDMA: # End of transmission found
#print("Node " + str(self.nodeParams.config.nodeId) + " - Bytes in rxBuffer: ", self.radio.bytesInRxBuffer)
# return True
#print("Node " + str(self.nodeParams.config.nodeId) + " - Bytes in rxBuffer: ", self.radio.bytesInRxBuffer)
return False # end of transmission not found
def relayMsg(self, msgBytes):
"""Relay received message. Existing mesh header is maintained with only the source updated."""
packetHeader = struct.unpack(self.meshPacketHeaderFormat, msgBytes[0:self.meshHeaderLen])
sourceId = packetHeader[0]
destId = packetHeader[1]
adminLength = packetHeader[2]
payloadLength = packetHeader[3]
cmdCounter = packetHeader[4]
statusByte = packetHeader[5]
#print("Node " + str(self.nodeParams.config.nodeId) + " relaying message from " + str(sourceId) + " - " + str(cmdCounter))
# Update packet sourceId
msgBytes[0:1] = struct.pack('<B', self.nodeParams.config.nodeId)
# Store in buffer for relay
self.cmdRelayBuffer += self.msgParser.encodeMsg(msgBytes)
def processMsgs(self):
"""Processes parsed mesh network messages. Raw data bytes are then stored for forwarding to the node host for processing."""
# Parse any received bytes
self.parseMsgs()
# Process any received messages
if (self.msgParser.parsedMsgs):
for i in range(len(self.msgParser.parsedMsgs)):
msg = self.msgParser.parsedMsgs.pop(0)
# Parse mesh packet header
packetValid, packetHeader, adminBytes, messageBytes = self.parseMeshPacket(msg)
if (packetValid == False): # packet invalid
continue
# Ignore stale commands
if (packetHeader['cmdCounter'] in self.nodeParams.cmdHistory):
continue
else:
self.nodeParams.cmdHistory.append(packetHeader['cmdCounter']) # update command history
# Update information on direct mesh links based on sourceId
self.nodeParams.nodeStatus[packetHeader['sourceId']-1].present = True
self.nodeParams.nodeStatus[packetHeader['sourceId']-1].lastMsgRcvdTime = self.nodeParams.clock.getTime()
# Extract any mesh messages and process
if (packetHeader['adminLength'] > 0):
self.processMeshMsgs(adminBytes)
# Place raw message bytes in buffer to send to host
if (packetHeader['payloadLength'] > 0):
if (packetHeader['destId'] == self.nodeParams.config.nodeId or self.nodeParams.config.commConfig['recvAllMsgs']):
#print("Placing in hostBuffer: " + str(msg[self.meshHeaderLen + adminLength:]))
self.hostBuffer += messageBytes
# Check for relay
if (self.inited == False): # don't process for relaying if mesh not inited
continue
if (packetHeader['destId'] == 0):
if (packetHeader['statusByte'] != BLOCK_TX_MSG): # broadcast message (don't relay block tx packets)
# All broadcast messages are relayed
self.relayMsg(bytearray(msg))
elif (packetHeader['destId'] != self.nodeParams.config.nodeId): # message for another node
# Only relay if on the shortest path
if (self.checkForRelay(self.nodeParams.config.nodeId, packetHeader['destId'], packetHeader['sourceId']) == True): # message should be relayed
self.relayMsg(bytearray(msg))
else:
pass
def checkForRelay(self, currentNode, destId, sourceId):
"""This method checks if a message should be relayed based on the current mesh graph."""
# No relay if this is the destination
if (currentNode == destId):
return False
try:
lenPathToSource = len(self.meshPaths[currentNode-1][sourceId-1][0]) - 1
lenPathToDest = len(self.meshPaths[currentNode-1][destId-1][0]) - 1
lenSourceToDest = len(self.meshPaths[sourceId-1][destId-1][0]) - 1
# Relay if the total path through this node to destination is equal to or less than shortest path from the source
if (lenSourceToDest >= (lenPathToDest + lenPathToSource)):
return True
else:
return False
except IndexError:
# Path data not available (may not have been updated yet)
return True
def processMeshMsgs(self, meshMsgs):
"""Processes mesh network administration commands and messages received."""
# Process any mesh commands
if (len(meshMsgs) > 0):
# Parse and process individual commands
self.tdmaCmdParser.parseMsgs(meshMsgs)
for i in range(len(self.tdmaCmdParser.parsedMsgs)):
self.processMsg(self.tdmaCmdParser.parsedMsgs.pop(0), {'nodeStatus': self.nodeParams.nodeStatus, 'comm': self, 'clock': self.nodeParams.clock})
def packageAdminData(self, maxLength):
adminBytes = b''
# Send command responses
#for resp in self.nodeParams.cmdResponse:
# cmd = Command(NodeCmds['CmdResponse'], resp, [NodeCmds['CmdResponse'], self.nodeParams.config.nodeId])
# adminBytes += self.tdmaCmdParser.encodeMsg(cmd.serialize(self.nodeParams.clock.getTime()))
# Send TDMA commands
adminBytes += self.sendTDMACmds(maxLength)
return adminBytes
def sendTDMACmds(self, maxLength):
tdmaCmdBytes = b''
# Send TDMA messages
timestamp = self.nodeParams.clock.getTime()
for cmdId in list(self.tdmaCmds.keys()):
cmd = self.tdmaCmds[cmdId]
# Check for admin period only commands
adminCmds = [TDMACmds['CurrentConfig'], TDMACmds['ConfigUpdate']]
if (cmdId in adminCmds and self.tdmaMode != TDMAMode.admin): # do not send admin only commands
continue
# Delay config transmission (initial delay upon mesh startup)
if (cmdId == TDMACmds['CurrentConfig'] and timestamp < self.initialConfigTxTime):
continue
# Check for polling commands (need to be processed by sender)
if (cmdId in [TDMACmds['NetworkRestart'], TDMACmds['ConfigUpdate'], TDMACmds['BlockTxRequest']]):
TDMACmdProcessor['msgProcessor'](self, cmdId, {'cmdId': cmd.cmdId, 'sourceId': self.nodeParams.config.nodeId, 'cmdCounter': cmd.header['header']['cmdCounter']}, cmd.serialize(timestamp), {'nodeStatus': self.nodeParams.nodeStatus, 'clock': self.nodeParams.clock, 'comm': self})
#self.networkMsgQueue.append({"header": {"cmdId": cmd.cmdId, "sourceId": self.nodeParams.config.nodeId, "cmdCounter": cmd.cmdCounter}, "msgContents": cmd.cmdData})
#elif (cmdId == TDMACmds['ConfigUpdate']):
# self.networkMsgQueue.append({"header": {"cmdId": cmd.cmdId, "sourceId": self.nodeParams.config.nodeId, "cmdCounter": cmd.cmdCounter}, "msgContents": {"valid": True, "destId": cmd.cmdData['destId']}})
# Update command counter
if ('cmdCounter' in cmd.header):
cmd.header['cmdCounter'] = self.nodeParams.get_cmdCounter()
# Send periodic commands at prescribed interval
msgBytes = b''
if cmd.txInterval:
if ceil(timestamp*100)/100.0 >= ceil((cmd.lastTxTime + cmd.txInterval)*100)/100.0: # only compare down to milliseconds
#self.bufferTxMsg(cmd.serialize(timestamp))
msgBytes = self.tdmaCmdParser.encodeMsg(cmd.serialize(timestamp))
#print("Node", self.nodeParams.config.nodeId, "- Sending periodic command:", cmd.cmdId)
#tdmaCmdBytes += self.tdmaCmdParser.encodeMsg(cmd.serialize(timestamp))
else: # non-periodic command
#self.bufferTxMsg(cmd.serialize(timestamp))
msgBytes = self.tdmaCmdParser.encodeMsg(cmd.serialize(timestamp))
#tdmaCmdBytes += self.tdmaCmdParser.encodeMsg(cmd.serialize(timestamp))
# Check for alotted size overflow
if (msgBytes and (len(tdmaCmdBytes) + len(msgBytes) <= maxLength)): # append to outgoing bytes
tdmaCmdBytes += msgBytes
if (cmdId == TDMACmds['CurrentConfig']):
print("Node " + str(self.nodeParams.config.nodeId) + " Sending CurrentConfig message.")
if (cmd.txInterval): # update last transmit time
cmd.lastTxTime = timestamp
else: # remove single-time command
del self.tdmaCmds[cmdId]
return tdmaCmdBytes
def startBlockTx(self, reqId, destId, srcId, startTime, length, blockData=None):
if (self.blockTxInProgress == True): # reject new block tx because one already in progress
return False
# Store block transfer details
endTime = startTime + int(length*self.frameLength*self.nodeParams.config.commConfig['blockTxEndMult'])
self.blockTx = BlockTx(reqId, length, srcId, destId, startTime, endTime, blockData)
self.blockTxInProgress = True
print("Node", self.nodeParams.config.nodeId, "- Starting block transmit, length-", self.blockTx.length)
return True
def sendBlockTxPacket(self):
"""Send next block transmit packet."""
# Check for missed packets
packetsToRemove = []
repeatPacket = None
#for entry in range(len(self.blockTxPacketStatus)):
for entry in self.blockTxPacketStatus.keys():
status = self.blockTxPacketStatus[entry]
status.framesSinceTx += 1
# Check for responses from all directly connected nodes
allResponsesRcvd = True
for node in self.neighbors:
if (node not in status.responsesRcvd): # response not received from this node
allResponsesRcvd = False
break
# Check packet status
#print("Responses received, framesSinceTx:", allResponsesRcvd, status.framesSinceTx)
if (allResponsesRcvd == True): # packet successfully sent
print("Node", self.nodeParams.config.nodeId, "- All responses received for block tx packet", status.packetNum)
packetsToRemove.append(entry)
elif (allResponsesRcvd == False and status.framesSinceTx >= self.nodeParams.config.commConfig['blockTxReceiptTimeout']): # resend packet
status.framesSinceTx = 0 # reset frame counter
status.retries += 1
repeatPacket = status.packet
if (status.retries >= self.nodeParams.config.commConfig['blockTxPacketRetry']): # Retry limit met, remove packet from status list
packetsToRemove.append(entry)
break
# Remove entries from packet status list
#self.blockTxPacketStatus = [self.blockTxPacketStatus[entry] for entry in range(len(self.blockTxStatus)) if entry not in packetsToRemove]
for entry in packetsToRemove:
del self.blockTxPacketStatus[entry]
# Check for packet to resend
if (repeatPacket): # packet to resend
print("Node " + str(self.nodeParams.config.nodeId) + " - Resending block transmit packet")
return repeatPacket
## Send next increment of block data
newPacket = self.getBlockTxPacket()
if (newPacket != None): # data to send
# Add new packet to status list
self.blockTxPacketStatus[self.blockTx.packetNum] = BlockTxPacketStatus(newPacket, self.blockTx.packetNum)
elif (len(self.blockTxPacketStatus) == 0): # Check for block transmit completion (all packets sent successfully)
self.blockTx.complete = True
return newPacket
def getBlockTxPacket(self):
# Check for block data
#if (self.blockTxData == None): # no data to send
# return None
# Create mesh packet from next chunk of block data
if (self.blockTx.dataLoc >= len(self.blockTx.data)): # no packets remaining
return None
elif (len(self.blockTx.data) > self.blockTx.dataLoc + self.nodeParams.config.commConfig['blockTxPacketSize']):
newBlockTxDataLoc = self.blockTx.dataLoc + int(self.nodeParams.config.commConfig['blockTxPacketSize'])
blockDataChunk = self.blockTx.data[self.blockTx.dataLoc:newBlockTxDataLoc]
self.blockTx.dataLoc = newBlockTxDataLoc
else: # send remainder of data block
blockDataChunk = self.blockTx.data[self.blockTx.dataLoc:]
self.blockTx.dataLoc = len(self.blockTx.data) # reached end of data block
# Generate new packet command
self.blockTx.packetNum += 1
blockDataCmd = Command(TDMACmds['BlockData'], {'blockReqId': self.blockTx.reqId, 'packetNum': self.blockTx.packetNum, 'dataLength': len(blockDataChunk), 'data': blockDataChunk}, [TDMACmds['BlockData'], self.nodeParams.config.nodeId])
blockDataSerialized = self.tdmaCmdParser.encodeMsg(blockDataCmd.serialize(self.nodeParams.clock.getTime()))
blockPacket = self.createMeshPacket(self.blockTx.destId, b'', blockDataSerialized, self.nodeParams.config.nodeId, BLOCK_TX_MSG)
print("Node " + str(self.nodeParams.config.nodeId) + " - Sending block transmit packet", self.blockTx.packetNum, ". Length-", len(blockPacket))
return blockPacket
def processBlockTxReceipts(self):
# Update block tx packet receipt status
for receipt in self.blockTxPacketReceipts:
if (receipt['blockReqId'] == self.blockTx.reqId and receipt['packetNum'] in self.blockTxPacketStatus):
if (receipt['sourceId'] not in self.blockTxPacketStatus[receipt['packetNum']].responsesRcvd):
self.blockTxPacketStatus[receipt['packetNum']].responsesRcvd.append(receipt['sourceId'])
self.blockTxPacketReceipts = []
def updateBlockTxStatus(self):
# Monitor for block transmit end
if (self.blockTx.complete or self.nodeParams.clock.getTime() >= self.blockTx.endTime): # block transmit ended
if (self.blockTx.srcId == self.nodeParams.config.nodeId): # sender end block tx
# Send block transmit end message
self.tdmaCmds[TDMACmds['BlockTxRequest']] = Command(TDMACmds['BlockTxRequest'], {'blockReqId': self.blockTx.reqId, 'destId': self.blockTx.destId, 'startTime': self.blockTx.startTime, 'length': self.blockTx.length, 'status': 0}, [TDMACmds['BlockTxRequest'], self.nodeParams.config.nodeId, self.nodeParams.get_cmdCounter()])
# End block transmit
self.endBlockTx()
def endBlockTx(self):
print("Node", self.nodeParams.config.nodeId, "- Concluding block transmit.")
# Assemble and pass data block to host
if (self.blockTx.srcId != self.nodeParams.config.nodeId and self.blockTx.destId in [0, self.nodeParams.config.nodeId]): # pass block transmit data to host
self.blockTxOut = {'dataComplete': self.blockTx.dataComplete, 'data': self.blockTx.getData()}
if (len(self.blockTxOut['data']) > 0):
print("Node", self.nodeParams.config.nodeId, "- Sending block tx data to host, length:", len(self.blockTxOut['data']))
# Check for need to relay - TODO - implement logic for relaying block tx to destination node
# Clear block transmit parameters
self.blockTx = None
self.blockTxInProgress = False
self.blockTxPacketStatus = dict() # stores transmitted packet status until all receipt requests received
self.blockTxPacketReceipts = []
def checkTimeOffset(self, offset=None):
if offset == None: # offset not provided so attempt to get offset from clock
offset = self.nodeParams.clock.getOffset()
if offset != None: # time offset available
self.timeOffsetTimer = None # reset time offset timer
self.nodeParams.nodeStatus[self.nodeParams.config.nodeId-1].timeOffset = offset
if abs(self.nodeParams.nodeStatus[self.nodeParams.config.nodeId-1].timeOffset) > self.nodeParams.config.commConfig['operateSyncBound']:
return 1
else: # no offset available
self.nodeParams.nodeStatus[self.nodeParams.config.nodeId-1].timeOffset = 127 # Error value
# Check time offset timer
if self.timeOffsetTimer:
#print(self.clock.getTime() - self.timeOffsetTimer)
if self.nodeParams.clock.getTime() - self.timeOffsetTimer > self.nodeParams.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.nodeParams.clock.getTime()
return 0
class TDMAComm(SerialComm):
def __init__(self, msgProcessors, radio, msgParser, nodeParams):
if not msgProcessors:
msgProcessors = [TDMACmdProcessor]
super().__init__(msgProcessors, nodeParams, radio, parser=msgParser)
self.nodeParams = nodeParams
# TDMA config
self.tdmaMode = TDMAMode.sleep
self.frameStartTime = []
self.commStartTime = None # time that TDMA comm was started - initialized manually by first node or parsed from messages received for nodes joining existing mesh
self.maxNumSlots = nodeParams.config.commConfig[
'maxNumSlots'] # Maximum number of slots
self.enableLength = nodeParams.config.commConfig['enableLength']
self.slotTime = 0.0
self.slotNum = 1
self.slotStartTime = 0.0
# TDMA Frame variables
self.frameTime = 0.0
self.frameLength = nodeParams.config.commConfig['frameLength']
self.cycleLength = nodeParams.config.commConfig['cycleLength']
# TDMA status
self.tdmaStatus = TDMAStatus.nominal
self.tdmaFailsafe = False
self.timeOffsetTimer = None
self.frameExceedanceCount = 0
# Mesh initialization variables
self.inited = False
self.initTimeToWait = nodeParams.config.commConfig[
'initTimeToWait'] # Time to wait before assuming no existing mesh network
self.initStartTime = None
# Mesh network commands
self.tdmaCmds = dict()
self.tdmaCmdParser = MsgParser(
{'parseMsgMax': nodeParams.config.parseMsgMax}, SLIPMsg(256))
# Transmit period variables
self.transmitSlot = nodeParams.config.commConfig[
'transmitSlot'] # Slot in cycle that this node is schedule to transmit
self.beginTxTime = self.enableLength + nodeParams.config.commConfig[
'preTxGuardLength']
self.endTxTime = self.beginTxTime + nodeParams.config.commConfig[
'txLength']
self.transmitComplete = False
# Receive period variables
self.beginRxTime = self.enableLength
self.endRxTime = self.beginRxTime + nodeParams.config.commConfig[
'rxLength']
self.slotLength = nodeParams.config.commConfig[
'slotLength'] # total length of slot
self.rxLength = nodeParams.config.commConfig['rxLength']
self.rxReadTime = self.beginTxTime + nodeParams.config.commConfig[
'rxDelay'] # time to begin reading serial
self.receiveComplete = False
# Current read position in radio rx buffer
self.rxBufferReadPos = 0
# Block TX init
self.resetBlockTxStatus()
self.clearDataBlock()
# Comm enable flag
self.enabled = True
# Mesh data in/out buffers
self.meshQueueIn = [b''] * (self.maxNumSlots + 1)
self.hostBuffer = bytearray()
# Network graph
#self.meshGraph = [0] * self.maxNumSlots # timestamps of last received message from each other node
self.lastGraphUpdate = 0.0
self.meshPaths = [[] * self.maxNumSlots] * self.maxNumSlots
def execute(self):
"""Execute communication functions."""
currentTime = time.time()
# Initialize mesh network
if self.inited == False:
self.init(currentTime)
return
else: # perform TDMA execution logic
self.executeTDMAComm(currentTime)
def updateFrameTime(self, currentTime):
self.frameTime = currentTime - self.frameStartTime
if self.frameTime >= self.frameLength: # Start new frame
#print(str(currentTime) + ": Node " + str(self.nodeParams.config.nodeId) + " - New frame started")
self.syncTDMAFrame(currentTime)
if self.frameTime < self.cycleLength:
cycleEnd = 0
else: # sleep period
cycleEnd = 1
return cycleEnd
def executeTDMAComm(self, currentTime):
"""Execute TDMA communication scheme."""
# Check for block transfers
#self.monitorBlockTx()
# Update frame time
frameStatus = self.updateFrameTime(currentTime)
if (frameStatus == 1):
self.sleep()
return
# Check for mode updates
self.updateMode(self.frameTime)
# Perform mode specific behavior
for case in switch(self.tdmaMode):
if case(TDMAMode.sleep):
# Set radio to sleep mode
self.radio.setMode(RadioMode.sleep)
break
if case(TDMAMode.init):
# Prepare radio to receive or transmit
if self.slotNum == self.transmitSlot:
# Set radio to transmit mode
self.radio.setMode(RadioMode.transmit)
else:
# Set radio to receive mode
self.radio.setMode(RadioMode.receive)
break
if case(TDMAMode.receive):
# Read data if TDMA message end not yet found
if self.receiveComplete == False and self.slotTime >= self.rxReadTime:
self.radio.setMode(RadioMode.receive) # set radio mode
# Delay so we aren't hammering the radio
#remainingRxTime = self.rxLength - (self.slotTime - self.enableLength)
#time.sleep(remainingRxTime*0.2)
self.receiveComplete = self.readMsgs()
if self.receiveComplete == True:
# Set radio to sleep
self.radio.setMode(RadioMode.sleep)
break
if case(TDMAMode.transmit):
# Send data
if self.transmitComplete == False:
self.radio.setMode(RadioMode.transmit) # set radio mode
self.sendMsg()
else: # Set radio to sleep
self.radio.setMode(RadioMode.sleep)
break
if case(TDMAMode.failsafe): # Read only failsafe mode
# Enable radio in receive mode and read data
self.radio.setMode(RadioMode.receive)
self.readMsgs()
break
if case(TDMAMode.blockRx): # Block receive mode
self.radio.setMode(RadioMode.receive)
self.readMsgs()
break
if case(TDMAMode.blockTx): # Block transmit mode
self.radio.setMode(RadioMode.transmit)
self.sendBlock()
break
def init(self, currentTime):
if not self.commStartTime: # Mesh not initialized
self.initComm(currentTime)
return
else: # Join existing mesh
self.initMesh()
def initMesh(self, currentTime=time.time()):
"""Initialize node mesh networks."""
# Create tdma comm messages
flooredStartTime = math.floor(self.commStartTime)
self.tdmaCmds[TDMACmds['MeshStatus']] = Command(
TDMACmds['MeshStatus'], {
'commStartTimeSec': int(flooredStartTime),
'status': self.tdmaStatus
}, [TDMACmds['MeshStatus'], self.nodeParams.config.nodeId],
self.nodeParams.config.commConfig['statusTxInterval'])
self.tdmaCmds[TDMACmds['LinkStatus']] = Command(
TDMACmds['LinkStatus'], {
'linkStatus': self.nodeParams.linkStatus,
'nodeId': self.nodeParams.config.nodeId
}, [TDMACmds['LinkStatus'], self.nodeParams.config.nodeId],
self.nodeParams.config.commConfig['linksTxInterval'])
if self.nodeParams.config.nodeId != 0: # stop ground node from broadcasting time offset
self.tdmaCmds[TDMACmds['TimeOffset']] = Command(
TDMACmds['TimeOffset'], {
'nodeStatus':
self.nodeParams.nodeStatus[self.nodeParams.config.nodeId -
1]
}, [TDMACmds['TimeOffset'], self.nodeParams.config.nodeId],
self.nodeParams.config.commConfig['offsetTxInterval'])
# Determine where in frame mesh network currently is
self.syncTDMAFrame(currentTime)
self.inited = True
print("Node " + str(self.nodeParams.config.nodeId) +
" - Initializing comm")
def initComm(self, currentTime):
if self.initStartTime == None:
# Start mesh initialization timer
self.initStartTime = currentTime
print("Node " + str(self.nodeParams.config.nodeId) +
" - Starting initialization timer")
return
elif (currentTime - self.initStartTime) >= self.initTimeToWait:
# Assume no existing mesh and initialize network
self.commStartTime = math.ceil(currentTime)
print("Initializing new mesh network")
self.initMesh()
else: # Wait for initialization timer to lapse
# Turn on radios and check for comm messages
self.checkForInit()
def checkForInit(self):
# Look for tdma status message
self.radio.setMode(RadioMode.receive)
self.readBytes(True)
if self.radio.bytesInRxBuffer > 0:
self.processMsgs()
#while self.msgParser.parsedMsgs:
# msg = self.msgParser.parsedMsgs.pop(0)
# cmdId = struct.unpack('=B',msg[0:1])[0]
# if cmdId == TDMACmds['MeshStatus']:
# print("Mesh status received")
# self.processMsg(msg, {'nodeStatus': self.nodeParams.nodeStatus, 'comm': self, 'clock': self.nodeParams.clock})
def syncTDMAFrame(self, currentTime=time.time()):
"""Determine where in frame mesh network currently is to ensure time sync."""
self.frameTime = (currentTime - self.commStartTime) % self.frameLength
self.frameStartTime = currentTime - self.frameTime
print(str(self.frameStartTime), "- Frame start")
# Update periodic mesh messages
if (TDMACmds['MeshStatus'] in self.tdmaCmds):
self.tdmaCmds[
TDMACmds['MeshStatus']].cmdData['status'] = self.tdmaStatus
if (TDMACmds['LinkStatus'] in self.tdmaCmds):
self.tdmaCmds[TDMACmds['LinkStatus']].cmdData[
'linkStatus'] = self.nodeParams.linkStatus
# Reset buffer read position
self.rxBufferReadPos = 0
# Check for tdma failsafe
self.checkTimeOffset()
def sleep(self):
"""Sleep until end of frame."""
# Update mesh paths
if ((self.nodeParams.clock.getTime() - self.lastGraphUpdate) >
self.nodeParams.config.commConfig['linksTxInterval']):
self.updateShortestPaths()
# Process any received messages
self.processMsgs()
# Sleep until next frame to save CPU usage
remainingFrameTime = (
self.frameLength -
(self.nodeParams.clock.getTime() - self.frameStartTime))
if (remainingFrameTime > 0.010):
# Sleep remaining frame length minus some delta to ensure waking in time
time.sleep(remainingFrameTime - 0.010)
elif (remainingFrameTime < -0.010):
print("WARNING: Frame length exceeded! Exceedance- " +
str(abs(remainingFrameTime)))
self.frameExceedanceCount += 1
def updateShortestPaths(self):
for node in range(self.maxNumSlots):
self.meshPaths[node] = findShortestPaths(
self.maxNumSlots, self.nodeParams.linkStatus, node + 1)
def updateMode(self, frameTime):
# Update slot
self.resetTDMASlot(frameTime)
# Check for TDMA failsafe
if self.tdmaFailsafe == True:
self.setTDMAMode(TDMAMode.failsafe)
return
if frameTime >= self.cycleLength: # Cycle complete
self.setTDMAMode(TDMAMode.sleep)
#print str(frameTime) + " - Cycle complete, sleeping"
return
# Check for block transmit
if self.blockTxStatus['status'] == TDMABlockTxStatus.active:
if self.blockTxStatus[
'txNode'] == self.nodeParams.config.nodeId: # this node is transmitting
self.setTDMAMode(TDMAMode.blockTx)
else: # this node is receiving
self.setTDMAMode(TDMAMode.blockRx)
return
# Normal cycle sequence
if self.slotTime < self.enableLength: # Initialize comm at start of slot
self.setTDMAMode(TDMAMode.init)
else:
# Transmit slot
if self.slotNum == self.transmitSlot:
if self.slotTime >= self.beginTxTime:
if self.slotTime < self.endTxTime: # Begin transmitting
self.setTDMAMode(TDMAMode.transmit)
else: # Sleep
self.setTDMAMode(TDMAMode.sleep)
# Receive slot
else:
if self.slotTime >= self.beginRxTime: # begin receiviing
if self.slotTime < self.endRxTime: # receive
self.setTDMAMode(TDMAMode.receive)
else: # Sleep
self.setTDMAMode(TDMAMode.sleep)
#def resetTDMASlot(self, frameTime, currentTime=time.time(), slotNum=[]):
#def resetTDMASlot(self, frameTime, slotNum=None):
def resetTDMASlot(self, frameTime):
# Reset slot number
#if slotNum != None:
# if slotNum >= 1 and slotNum <= self.maxNumSlots:
# self.slotNum = int(slotNum)
# else: # invalid number
# raise InvalidTDMASlotNumber("Provided TDMA slot number is not valid")
#else:
#if self.slotStartTime:
# if (currentTime - self.slotStartTime) >= self.slotLength and self.slotNum < self.maxNumSlots:
# self.slotNum = int(frameTime/self.slotLength) + 1
#else:
if frameTime < self.cycleLength: # during cycle
self.slotNum = int(frameTime / self.slotLength) + 1
else: # during sleep period
self.slotNum = self.maxNumSlots
#self.slotStartTime = currentTime - (frameTime - (self.slotNum-1)*self.slotLength)
#self.slotTime = frameTime - self.slotStartTime
self.slotStartTime = (self.slotNum - 1) * self.slotLength
self.slotTime = frameTime - self.slotStartTime
#print("Updating slot number: " + str(self.slotNum))
def setTDMAMode(self, mode):
if self.tdmaMode != mode:
#print("Setting mode:", mode)
self.tdmaMode = mode
#print str(self.slotTime) + " - TDMA mode change: " + str(self.tdmaMode)
if mode == TDMAMode.receive:
self.receiveComplete = False
elif mode == TDMAMode.transmit:
self.transmitComplete = False
#print str(time.time()) + ": " + str(frameTime) + " - Node " + str(self.nodeId) + " - Transmitting"
def queueMeshMsg(self, destId, msgBytes):
"""This function receives messages to be sent over the mesh network and queues them for transmission."""
# Place message in appropriate position in outgoing queue (broadcast messages are stored in the zero position)
self.meshQueueIn[destId] += msgBytes
def sendMsg(self):
if (self.enabled == False): # Don't send anything if disabled
return
# Send buffered and periodic commands
if self.tdmaMode == TDMAMode.transmit:
# Send periodic TDMA commands
#self.sendTDMACmds()
# Check queue for waiting outgoing messages
self.processBuffers() # process relay and command buffers
for destId in range(len(self.meshQueueIn)):
packetBytes = b''
if (destId == 0):
packetBytes = self.packageMeshPacket(
destId, self.meshQueueIn[destId])
elif (destId != 0 and self.meshQueueIn[destId]
): # only send non-zero non-broadcast messages
packetBytes = self.packageMeshPacket(
destId, self.meshQueueIn[destId])
if (packetBytes):
self.bufferTxMsg(packetBytes)
self.meshQueueIn[
destId] = b'' # clear message after transmission
self.radio.bufferTxMsg(HDLC_END_TDMA) # append end of message byte
self.sendBuffer()
# End transmit period
self.transmitComplete = True
else:
pass
#print "Slot " + str(self.slotNum) + " - Node " + str(self.nodeId) + " - Can't send. Wrong mode: " + str(self.tdmaMode)
def packageMeshPacket(self, destId, msgBytes):
adminBytes = b''
if (destId == 0
): # package periodic TDMA commands into broadcast message
adminBytes = self.sendTDMACmds()
return self.createMeshPacket(destId, msgBytes, adminBytes,
self.nodeParams.config.nodeId)
def createMeshPacket(self, destId, msgBytes, adminBytes, sourceId):
if (len(adminBytes) == 0
and len(msgBytes) == 0): # do not send empty message
return bytearray()
# Create mesh packet header
packetHeaderFormat = '<BBHHH'
packetHeader = struct.pack(packetHeaderFormat, sourceId, destId,
len(adminBytes), len(msgBytes),
self.nodeParams.get_cmdCounter())
# Return mesh packet
return bytearray(packetHeader + adminBytes + msgBytes)
def sendBlock(self):
if self.dataBlock:
if len(self.dataBlock[self.dataBlockPos:]
) > self.nodeParams.config.commConfig[
'maxBlockTransferSize']: # buffer portion of data block
blockDataCmd = Command(
TDMACmds['BlockData'], {
'data':
self.dataBlock[self.dataBlockPos:self.dataBlockPos +
self.nodeParams.config.
commConfig['maxBlockTransferSize']]
}, [TDMACmds['BlockData'], self.nodeParams.config.nodeId
]).serialize(self.nodeParams.clock.getTime())
self.dataBlockPos += self.nodeParams.config.commConfig[
'maxBlockTransferSize']
else: # send entire data block
blockDataCmd = Command(
TDMACmds['BlockData'], {
'data': self.dataBlock[self.dataBlockPos:]
}, [TDMACmds['BlockData'], self.nodeParams.config.nodeId
]).serialize(self.nodeParams.clock.getTime())
self.clearDataBlock() # clear stored data block
self.blockTxStatus[
'blockTxComplete'] = True # end block transfer
# Send block data
self.radio.bufferTxMsg(blockDataCmd)
self.radio.bufferTxMsg(HDLC_END_TDMA)
self.radio.sendBuffer(
self.nodeParams.config.commConfig['maxBlockTransferSize'])
else: # end block transfer - no data to send
self.blockTxStatus['blockTxComplete'] = True
def readMsgs(self):
"""Read from serial connection and look for end of message value."""
self.radio.readBytes(True)
for i in range(self.rxBufferReadPos, self.radio.bytesInRxBuffer):
self.rxBufferReadPos = i + 1
#byte = self.rxBuffer[i:i+1]
if self.radio.rxBuffer[
i:i + 1] == HDLC_END_TDMA: # End of transmission found
return True
return False # end of transmission not found
def relayMsg(self, msgBytes):
"""Relay received message. Existing mesh header is maintained with only the source updated."""
# Update packet sourceId
msgBytes[0:1] = struct.pack('<B', self.nodeParams.config.nodeId)
# Store in buffer for relay
self.cmdRelayBuffer += self.msgParser.encodeMsg(msgBytes)
def processMsgs(self):
"""Processes parsed mesh network messages. Raw data bytes are then stored for forwarding to the node host for processing."""
# Parse any received bytes
self.parseMsgs()
# Process any received messages
if (self.msgParser.parsedMsgs):
for i in range(len(self.msgParser.parsedMsgs)):
msg = self.msgParser.parsedMsgs.pop(0)
# Parse mesh packet header
packetHeaderFormat = '<BBHHH'
meshHeaderLen = struct.calcsize(packetHeaderFormat)
packetHeader = struct.unpack(packetHeaderFormat,
msg[0:meshHeaderLen])
sourceId = packetHeader[0]
destId = packetHeader[1]
adminLength = packetHeader[2]
payloadLength = packetHeader[3]
cmdCounter = packetHeader[4]
# Ignore stale commands
if (cmdCounter in self.nodeParams.cmdHistory):
continue
else:
self.nodeParams.cmdHistory.append(
cmdCounter) # update command history
# Validate message
if (len(msg) == (meshHeaderLen + adminLength +
payloadLength)): # message length is valid
# Update information on direct mesh links based on sourceId
self.nodeParams.nodeStatus[sourceId - 1].present = True
self.nodeParams.nodeStatus[
sourceId -
1].lastMsgRcvdTime = self.nodeParams.clock.getTime()
# Extract any mesh messages and process
if (adminLength > 0):
self.processMeshMsgs(msg[meshHeaderLen:meshHeaderLen +
adminLength])
# Place raw message bytes in buffer to send to host
if (payloadLength > 0):
if (destId == self.nodeParams.config.nodeId or self.
nodeParams.config.commConfig['recvAllMsgs']):
self.hostBuffer += msg[meshHeaderLen +
adminLength:]
# Check for relay
if (destId == 0): # broadcast message
# All broadcast messages are relayed
self.relayMsg(bytearray(msg))
elif (destId != self.nodeParams.config.nodeId
): # message for another node
# Only relay if on the shortest path
if (self.checkForRelay(self.nodeParams.config.nodeId,
destId, sourceId) == True
): # message should be relayed
self.relayMsg(bytearray(msg))
else:
pass
def checkForRelay(self, currentNode, destId, sourceId):
"""This method checks if a message should be relayed based on the current mesh graph."""
# No relay if this is the destination
if (currentNode == destId):
return False
try:
lenPathToSource = len(
self.meshPaths[currentNode - 1][sourceId - 1][0]) - 1
lenPathToDest = len(
self.meshPaths[currentNode - 1][destId - 1][0]) - 1
lenSourceToDest = len(
self.meshPaths[sourceId - 1][destId - 1][0]) - 1
# Relay if the total path through this node to destination is equal to or less than shortest path from the source
if (lenSourceToDest >= (lenPathToDest + lenPathToSource)):
return True
else:
return False
except IndexError:
# Path data not available (may not have been updated yet)
return True
def processMeshMsgs(self, meshMsgs):
"""Processes mesh network administration commands and messages received."""
# Process any mesh commands
if (len(meshMsgs) > 0):
# Parse and process individual commands
self.tdmaCmdParser.parseMsgs(meshMsgs)
for i in range(len(self.tdmaCmdParser.parsedMsgs)):
self.processMsg(
self.tdmaCmdParser.parsedMsgs.pop(0), {
'nodeStatus': self.nodeParams.nodeStatus,
'comm': self,
'clock': self.nodeParams.clock
})
def sendTDMACmds(self):
tdmaCmdBytes = b''
# Send TDMA messages
timestamp = self.nodeParams.clock.getTime()
for cmdId in list(self.tdmaCmds.keys()):
cmd = self.tdmaCmds[cmdId]
# Update command counter
if ('cmdCounter' in cmd.header):
cmd.header['cmdCounter'] = self.nodeParams.get_cmdCounter()
# Send periodic commands at prescribed interval
if cmd.txInterval:
if ceil(timestamp * 100) / 100.0 >= ceil(
(cmd.lastTxTime + cmd.txInterval) *
100) / 100.0: # only compare down to milliseconds
#self.bufferTxMsg(cmd.serialize(timestamp))
tdmaCmdBytes += self.tdmaCmdParser.encodeMsg(
cmd.serialize(timestamp))
else: # non-periodic command
#self.bufferTxMsg(cmd.serialize(timestamp))
tdmaCmdBytes += self.tdmaCmdParser.encodeMsg(
cmd.serialize(timestamp))
del self.tdmaCmds[cmdId] # remove single-time command
return tdmaCmdBytes
def resetBlockTxStatus(self):
"""Clears block transmit status."""
self.blockTxStatus = {
'status': TDMABlockTxStatus.false,
'txNode': None,
'startTime': None,
'length': None,
'blockResponseList': {},
'blockReqID': None,
'requestTime': None,
'blockTxComplete': False
}
self.tdmaStatus = TDMAStatus.nominal
def monitorBlockTx(self):
"""Monitors current status of block transmit."""
if self.blockTxStatus['status'] == TDMABlockTxStatus.false:
return
elif self.blockTxStatus[
'status'] == TDMABlockTxStatus.pending: # monitor pending block request
if self.blockTxStatus[
'txNode'] == self.nodeParams.config.nodeId: # this node requested block tx
# Check block request responses
response = self.checkBlockResponse()
if response == True:
# Confirm block tx
blockConfirmCmd = Command(
TDMACmds['BlockTxConfirmed'],
{'blockReqID': self.blockTxStatus['blockReqID']}, [
TDMACmds['BlockTxConfirmed'],
self.nodeParams.config.nodeId,
self.nodeParams.get_cmdCounter()
])
self.radio.bufferTxMsg(
blockConfirmCmd.serialize(
self.nodeParams.clock.getTime()))
self.blockTxStatus['status'] = TDMABlockTxStatus.confirmed
return
elif response == False:
# Cancel request
self.resetBlockTxStatus()
return
# Check for request timeout
if (self.frameStartTime - self.blockTxStatus['requestTime']
) > self.nodeParams.config.commConfig[
'blockTxRequestTimeout'] * self.nodeParams.config.commConfig[
'frameLength']:
# Request timed out - reset status
self.resetBlockTxStatus()
return
if self.frameStartTime >= self.blockTxStatus[
'startTime']: # no block confirmed received
# Cancel pending block
self.resetBlockTxStatus()
return
elif self.blockTxStatus['status'] == TDMABlockTxStatus.confirmed:
# Check for block start
if self.frameStartTime >= self.blockTxStatus[
'startTime']: # start block
self.blockTxStatus['status'] = TDMABlockTxStatus.active
self.tdmaStatus = TDMAStatus.blockTx
# Send block transmit status message
if self.blockTxStatus['txNode'] == self.nodeParams.config.nodeId:
blockStatusCmd = Command(
TDMACmds['BlockTxStatus'], {
'blockReqID': self.blockTxStatus['blockReqID'],
'startTime': self.blockTxStatus['startTime'],
'length': self.blockTxStatus['length']
}, [
TDMACmds['BlockTxStatus'],
self.nodeParams.config.nodeId,
self.nodeParams.get_cmdCounter()
])
self.radio.bufferTxMsg(
blockStatusCmd.serialize(self.nodeParams.clock.getTime()))
elif self.blockTxStatus[
'status'] == TDMABlockTxStatus.active: # block transmit in progress
# Check for end of block transmit
if self.blockTxStatus['blockTxComplete'] or (
self.frameStartTime - self.blockTxStatus['startTime']
) >= self.blockTxStatus[
'length'] * self.nodeParams.config.commConfig[
'frameLength'] or self.tdmaStatus == TDMAStatus.nominal:
# Block transmit ended - reset status
self.resetBlockTxStatus()
def clearDataBlock(self):
"""Clear data stored for block transfer."""
self.dataBlock = bytearray()
self.dataBlockPos = 0
def sendDataBlock(self, dataBlock):
"""Begins block transfer process."""
# Calculate block tx parameters
length = int(
ceil(
len(dataBlock) /
self.nodeParams.config.commConfig['maxBlockTransferSize']))
if length > self.nodeParams.config.commConfig['maxTxBlockSize']:
# Too much data to transmit
return False
startTime = int(self.frameStartTime +
self.nodeParams.config.commConfig['frameLength'] *
self.nodeParams.config.commConfig['minBlockTxDelay'])
# Store data block
self.dataBlock = dataBlock
# Populate response list
self.populateBlockResponseList()
# Send block tx request
blockReqID = random.randint(1, 255) # just a random "unique" number
blockTxCmd = Command(TDMACmds['BlockTxRequest'], {
'blockReqID': blockReqID,
'startTime': startTime,
'length': length
}, [
TDMACmds['BlockTxRequest'], self.nodeParams.config.nodeId,
self.nodeParams.get_cmdCounter()
])
self.bufferTxMsg(blockTxCmd.serialize(self.nodeParams.clock.getTime()))
# Update blockTxStatus
self.blockTxStatus['blockReqID'] = blockReqID
self.blockTxStatus['startTime'] = startTime
self.blockTxStatus['length'] = length
self.blockTxStatus['requestTime'] = self.frameStartTime
self.blockTxStatus['txNode'] = self.nodeParams.config.nodeId
self.blockTxStatus['status'] = TDMABlockTxStatus.pending
return True
def populateBlockResponseList(self):
"""Add currently present nodes to block request response list."""
for i in range(len(self.nodeParams.nodeStatus)):
if self.nodeParams.nodeStatus[i].present:
self.blockTxStatus['blockResponseList'].update({i + 1: None})
def checkBlockResponse(self):
# Check for positive response from all nodes
numPosResponses = 0
for node in self.blockTxStatus['blockResponseList']:
response = self.blockTxStatus['blockResponseList'][node]
if response != None:
if response == False: # False response
return False
elif response == True:
numPosResponses += 1
if numPosResponses == len(self.blockTxStatus['blockResponseList']
): # all responded positive
return True
else: # still awaiting responses
return None
def checkTimeOffset(self, offset=None):
if offset == None: # offset not provided so attempt to get offset from clock
offset = self.nodeParams.clock.getOffset()
if offset != None: # time offset available
self.timeOffsetTimer = None # reset time offset timer
self.nodeParams.nodeStatus[self.nodeParams.config.nodeId -
1].timeOffset = offset
if abs(self.nodeParams.nodeStatus[self.nodeParams.config.nodeId -
1].timeOffset
) > self.nodeParams.config.commConfig['operateSyncBound']:
return 1
else: # no offset available
self.nodeParams.nodeStatus[self.nodeParams.config.nodeId -
1].timeOffset = 127 # Error value
# Check time offset timer
if self.timeOffsetTimer:
#print(self.clock.getTime() - self.timeOffsetTimer)
if self.nodeParams.clock.getTime(
) - self.timeOffsetTimer > self.nodeParams.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.nodeParams.clock.getTime()
return 0