def _service(self):
if self._dev is None:
self._connect()
else:
self._enqueue_poll_msgs()
while not self._tx_queue.empty():
msg = self._tx_queue.get_nowait()
packed_msg = um.pack(msg)
slipped_msg = slip(packed_msg)
try:
self._dev.write(slipped_msg)
except:
self._disconnect()
return
try:
self._rx_bytes += self._dev.read(4096)
except:
self._disconnect()
return
while self._rx_bytes != b'':
msg_bytes, self._rx_bytes = unslip_from(self._rx_bytes)
if msg_bytes == b'':
break
try:
msg = um.unpack(msg_bytes)
except:
warnings.warn('Unknown message %s' % msg_bytes)
continue
self.msg_received.emit(msg)
def __init__(self, port, logPipe):
self._slip = slip.slip(port)
self._stats = simple_stats.SimpleStats()
# The MAVLink uplink address is learned the first time we get a
# MAVLink packet from Solo.
self._mavUplinkAddress = None
# The pairing socket address is learned the first time we get a
# pair request packet from the pairing server module.
self._pairingAddress = None
# Message handlers.
# Receiver thread uses these to process messages received from STM32.
self.rc = stm32_rc(self)
self.mavlink = stm32_mavlink(self)
self.sysinfo = stm32_sysinfo(self)
self.pairCnf = stm32_pairConfirm(self)
# debug counts
self._msgInCounts = {}
self._msgOutCounts = {}
self._junkCounts = {}
# control how often we log progress and junk messages
self._logProgressInterval = datetime.timedelta(seconds=30) # =None to never log
self._logProgressLast = None
self._logJunkInterval = datetime.timedelta(seconds=5) # =None to never log
self._logJunkLast = None
# Packet log
self._logPipe = logPipe
# Sender thread (sends messages to STM32)
self._inSocks = [ ]
# A packet arriving on one of these UDP ports is sent to the STM32
# with the given packet ID
self.addInput(mavDestPort, PKT_ID_MAVLINK)
self.addInput(sysDestPort, PKT_ID_SYSINFO)
self.addInput(pairReqDestPort, PKT_ID_PAIR_REQUEST)
self.addInput(pairResDestPort, PKT_ID_PAIR_RESULT)
# Receiver thread (receives messages from STM32)
self._quit = False
self._timeout = False
self._receiver = threading.Thread(name = 'stm32.receiver',
target = stm32.receiver,
args = (self, ))
self._receiver.daemon = True
self._sender = threading.Thread(name = 'stm32.sender',
target = stm32.sender,
args = (self, ))
self._sender.daemon = True
# Start thread
self._receiver.start()
self._sender.start()
def testSLIPDecodeEncode(self):
nebSlip = slip.slip()
testFile = open("testdata.bin", "rb")
initialPackets = nebSlip.decodePackets(testFile)
testFile.close()
testFile = open("encodedPackets.bin", "wb")
for packet in initialPackets:
encodedPacket = self.slip.encode(packet)
testFile.write(encodedPacket)
testFile.close()
testFile = open("encodedPackets.bin", "rb")
newPackets = nebSlip.decodePackets(testFile)
testFile.close()
for idx,newPacket in enumerate(newPackets):
self.assertEqual(newPacket, initialPackets[idx])
udp_server_address = ('',UDP_PORT_IN) #udp server
udp_broadcast=('<broadcast>',UDP_PORT_OUT) #broadcast address
#serial=open(SERIAL_PORT,'r+b') #open the file corrisponding to YUN serial port
serial = serial.Serial(
port=SERIAL_PORT,
baudrate=921600)
tty.setraw(serial) #this avoids the terminal to change bytes with value 10 in 13 10
udp_socket = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
udp_socket.bind(udp_server_address)
if BROADCAST_MODE:
udp_socket.setsockopt(socket.SOL_SOCKET,socket.SO_BROADCAST,1)
slip_processor=slip.slip()
while True:
(rlist, wlist, xlist) = select.select([udp_socket,serial], [], [])
if serial in rlist:
serial_data = serial.read(1)
slip_processor.append(serial_data)
slip_packets=slip_processor.decode()
for packet in slip_packets:
if BROADCAST_MODE:
udp_socket.sendto(packet,udp_broadcast)
else:
udp_socket.sendto(packet,udp_client_address)
if udp_socket in rlist:
udp_data,udp_client = udp_socket.recvfrom(MAX_UDP_PACKET)
def main():
# File Name Strings
directoryName = "SampleData/"
packetTypeName = "Pedometer"
binaryFileSuffix = "Stream.bin"
# Construct the file names for different output types
testInputFileName = directoryName + packetTypeName + binaryFileSuffix
testOutputCopyFileName = directoryName + packetTypeName + "Regen" + binaryFileSuffix
testCRCChangeFileName = directoryName + packetTypeName + binaryFileSuffix
outputBytesList = []
nebSlip = slip.slip()
testFile = open(testInputFileName, "rb")
# Determine where to start decoding packets
firstByte = testFile.read(1)
startPosition = 1
firstPacketBytes = b''
# Find the first packet location
if firstByte != b'\xc0':
while (firstByte != b'\xc0'):
firstPacketBytes += firstByte
firstByte = testFile.read(1)
startPosition = startPosition + 1
firstPacketBytes += b'\xc0'
testFile.seek(startPosition)
else:
testFile.seek(0)
# Once the location is found, start decoding packets
fileStream = testFile
packets = nebSlip.decodePackets(fileStream)
testFile.close()
# Start writing to the copy and CRC changed packets
testCopyFile = open(testOutputCopyFileName, "wb")
testCRCChangeFile = open(testCRCChangeFileName, "wb")
testCopyFile.write(firstPacketBytes)
testCRCChangeFile.write(firstPacketBytes)
for packet in packets:
packetBytes = [packet[ii:ii + 1] for ii in range(len(packet))]
genCRC(packetBytes)
contBytes = b''
for byte in packetBytes:
contBytes += byte
encodedPacket = nebSlip.encode(packet)
changedEncodedPacket = nebSlip.encode(contBytes)
testCopyFile.write(encodedPacket)
testCRCChangeFile.write(changedEncodedPacket)
testCopyFile.close()
# Check to see if there are any differences between the original bytes
# and the SLIP encoded bytes
file1 = open(testInputFileName, "rb")
file2 = open(testOutputCopyFileName, "rb")
origBytes = file1.read()
regenBytes = file2.read()
origBytes = [origBytes[ii:ii + 1] for ii in range(len(origBytes))]
regenBytes = [regenBytes[ii:ii + 1] for ii in range(len(regenBytes))]
for idx, byte in enumerate(origBytes):
if (byte != regenBytes[idx]):
print('{0} == {1}'.format(origBytes[idx - 1], regenBytes[idx - 1]))
print('origBytes {0} != regenBytes {1} at idx={2}'.format(
byte, regenBytes[idx], idx))
break
BROADCAST_MODE=False #set to True if you want a broadcast replay instead of unicast
udp_client_address=('127.0.0.1',UDP_PORT_OUT) #where to forward packets coming from serial port
udp_server_address = ('',UDP_PORT_IN) #udp server
udp_broadcast=('<broadcast>',UDP_PORT_OUT) #broadcast address
serial=open(SERIAL_PORT,'r+b') #open the file corrisponding to YUN serial port
tty.setraw(serial) #this avoids the terminal to change bytes with value 10 in 13 10
udp_socket = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
udp_socket.bind(udp_server_address)
if BROADCAST_MODE:
udp_socket.setsockopt(socket.SOL_SOCKET,socket.SO_BROADCAST,1)
slip_processor=slip.slip()
while True:
(rlist, wlist, xlist) = select.select([udp_socket,serial], [], [])
if serial in rlist:
serial_data = serial.read(1)
slip_processor.append(serial_data)
slip_packets=slip_processor.decode()
for packet in slip_packets:
if BROADCAST_MODE:
udp_socket.sendto(packet,udp_broadcast)
else:
udp_socket.sendto(packet,udp_client_address)
if udp_socket in rlist:
udp_data,udp_client = udp_socket.recvfrom(MAX_UDP_PACKET)
def __init__(self, serialcom):
self.comslip = slip.slip()
self.sc = serialcom
def __init__(self, bridge):
self.bridge = bridge
# self.bridge.setSerialServer(self)
self.slip_processor = slip.slip()
def main():
# File Name Strings
directoryName = "SampleData/"
packetTypeName = "Pedometer"
binaryFileSuffix = "Stream.bin"
# Construct the file names for different output types
testInputFileName = directoryName+packetTypeName+binaryFileSuffix
testOutputCopyFileName = directoryName+packetTypeName+"Regen"+binaryFileSuffix
testCRCChangeFileName = directoryName+packetTypeName+binaryFileSuffix
outputBytesList = []
nebSlip = slip.slip()
testFile = open(testInputFileName, "rb")
# Determine where to start decoding packets
firstByte = testFile.read(1)
startPosition = 1
firstPacketBytes = b''
# Find the first packet location
if firstByte != b'\xc0':
while (firstByte != b'\xc0'):
firstPacketBytes += firstByte
firstByte = testFile.read(1)
startPosition = startPosition + 1
firstPacketBytes += b'\xc0'
testFile.seek(startPosition)
else:
testFile.seek(0)
# Once the location is found, start decoding packets
fileStream = testFile
packets = nebSlip.decodePackets(fileStream)
testFile.close()
# Start writing to the copy and CRC changed packets
testCopyFile = open(testOutputCopyFileName, "wb")
testCRCChangeFile = open(testCRCChangeFileName, "wb")
testCopyFile.write(firstPacketBytes)
testCRCChangeFile.write(firstPacketBytes)
for packet in packets:
packetBytes = [packet[ii:ii+1] for ii in range(len(packet))]
genCRC(packetBytes)
contBytes = b''
for byte in packetBytes:
contBytes += byte
encodedPacket = nebSlip.encode(packet)
changedEncodedPacket = nebSlip.encode(contBytes)
testCopyFile.write(encodedPacket)
testCRCChangeFile.write(changedEncodedPacket)
testCopyFile.close()
# Check to see if there are any differences between the original bytes
# and the SLIP encoded bytes
file1 = open(testInputFileName, "rb")
file2 = open(testOutputCopyFileName, "rb")
origBytes = file1.read()
regenBytes = file2.read()
origBytes = [origBytes[ii:ii+1] for ii in range(len(origBytes))]
regenBytes = [regenBytes[ii:ii+1] for ii in range(len(regenBytes))]
for idx,byte in enumerate(origBytes):
if(byte != regenBytes[idx]):
print('{0} == {1}'.format(
origBytes[idx-1], regenBytes[idx-1] ))
print('origBytes {0} != regenBytes {1} at idx={2}'
.format(byte, regenBytes[idx], idx))
break
def setUp(self):
self.nebSlip = slip.slip()
def setUp(self):
self.nebSlip = slip.slip()
def __init__(self, serialcom):
self.comslip = slip.slip()
self.sc = serialcom
def setUp(self): self.slip = slip.slip()
listenSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
listenSock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
listenSock.bind(("", opts.port))
listenSock.listen(1)
while True:
print "waiting for connection on port", opts.port
try:
dataSock, remoteAddress = listenSock.accept()
except:
# typically ctrl-C
break
print "connection from", remoteAddress
slipIf = slip.slip(sock1(dataSock))
# start threads
rcThread = threading.Thread(name="rcThread", target=rcThreadRun, args=(slipIf,))
rcThread.daemon = True
rcThread.start()
msgThread = threading.Thread(name="msgThread", target=msgThreadRun, args=(slipIf,))
msgThread.daemon = True
msgThread.start()
rcThread.join()
msgThread.join()
dataSock.close()
# break
# msg = struct.unpack('>BHH', data)
# print('%04o: %05o%o' % (msg[1], msg[2] >> 1, msg[2] & 1))
# if msg[1] == 0o1777:
# done = True
# break
# break
# for i in range(0o2000):
# msg = slip(b'\x91' + struct.pack('>H', i) + struct.pack('>H', i))
# dev.write(msg)
with open('dump.bin', 'wb') as f:
for i in range(0o44 * 1024):
msg = slip(b'\x11' + struct.pack('>H', i))
dev.write(msg)
res = ''
while not res:
res = dev.read(64)
data, rem = unslip_from(res)
msg = struct.unpack('>BHH', data)
d = (msg[2] & 0x8000) | ((msg[2] & 0x1) << 14) | (
(msg[2] & 0x7FFE) >> 1)
f.write(struct.pack('>H', d))
# dev.write('\xC0\x20\x00\x64\xC0')
# res = ''
# while not res:
# res = dev.read(64)
# data, rem = unslip_from(res)
