def __init__(self, head, payload):
EOP = bytes([0xf1]) + bytes([0xf2]) + bytes([0xf3]) + bytes([0xf4])
eop_stuffed = bytes([0x00]) + bytes([0xf1]) + bytes([0x00]) + bytes([0xf2])\
+ bytes([0x00]) + bytes([0xf3]) + bytes([0x00]) + bytes([0xf4])
#==============================================================
# stuff the payload
payload = payload.replace(EOP, eop_stuffed)
#==============================================================
# make the size of the payload 2**7
totalSize = len(payload)
self.leftovers = bytearray()
if totalSize > 128:
payload = payload[:128]
self.leftovers = payload[128:]
#==============================================================
# adding the payload size to the head
payload_size = len(payload)
# print(f"payload size: {payload_size}")
head = head + bytes([payload_size])
#==============================================================
crcData0 = Crc16.calc(payload)
crcData0_bytes = int(crcData0).to_bytes(2, "big")
head = head + crcData0_bytes
# create the package with head payload and EOP
self.package = head + payload + EOP
def _sendCommand(self, command, payload, timeout=SERIAL_COMMAND_TIMEOUT):
if not command in SERIAL_COMMANDS:
print(
"The command provided {} is not a valid serial command".format(
command))
return FAILURE_CODE
sendBuffer = bytearray()
sendBuffer.append(STARTING_BYTE)
sendString = command + ":" + payload
sendBuffer.extend(sendString.encode("utf-8"))
sendBuffer.append(0x00)
newChecksum = Crc16()
# print("Checksum Calc based on {}".format(sendBuffer[1:]))
newChecksum.process(sendBuffer[1:])
checksumBytes = newChecksum.finalbytes()
sendBuffer.extend(checksumBytes)
print(len(sendBuffer))
for _ in range(SERIAL_COMMAND_MAX_TRIALS):
t = time.time()
self.serialPort.write(sendBuffer)
self.serialTimeoutTimer.setInterval(timeout)
self.serialTimeoutTimer.start()
succeeded, string = self.getSerialAck()
print(
"The time spent from sending a command to receiving a reply (or timeouting) is ",
time.time() - t)
if succeeded:
return string
elif not succeeded and "EXCEPTION" in string:
break
return FAILURE_CODE
def __init__(self, package, head):
self.package = package
self.package_size = len(package)
self.payload_size = head.get_payload_size()
eop = bytes([0xf1]) + bytes([0xf2]) + bytes([0xf3]) + bytes([0xf4])
eop_stuffed = bytes([0x00]) + bytes([0xf1]) + bytes([0x00]) + bytes([0xf2]) \
+ bytes([0x00]) + bytes([0xf3]) + bytes([0x00]) + bytes([0xf4])
payload_EOP = self.package[:]
#=======================================================
self.EOP_position = payload_EOP.find(eop)
payload_stuffed = payload_EOP[:self.EOP_position]
crcData0 = Crc16.calc(payload_stuffed)
crcBytes = crcData0.to_bytes(2, "big")
print(f"crc calc : {crcBytes}")
print(f"crc received: {head.get_crc()}")
self.payload = payload_stuffed.replace(eop_stuffed, eop)
if self.EOP_position != self.payload_size or crcBytes != head.get_crc():
self.message_sent = bytes([0x06])
else:
self.message_sent = bytes([0x04])
def getSerialAck(self):
string = ""
succeeded = False
self.serialState = SerialState.WaitingToStart
currentSerialString = ""
currentCheckSum = bytearray(2)
while(self.serialTimeoutTimer.remainingTime()>0):
QCoreApplication.processEvents()
self.processCheckStopRequest()
if self.serialState == SerialState.WaitingToStart:
newByte = self.serialPort.read()
if len(newByte) == 1:
if newByte[0] == STARTING_BYTE:
self.serialState = SerialState.WaitingForString
if self.serialState == SerialState.WaitingForString:
newBytes = self.serialPort.read_until(b'\0')
if len(newBytes) >= 1:
for i in range (len(newBytes)):
if newBytes[i] == STARTING_BYTE:
pass
else:
currentSerialString = currentSerialString + newBytes[i:].decode("utf-8")
if newBytes[-1] == 0x00:
self.serialState = SerialState.WaitingForChecksum1
break
if self.serialState == SerialState.WaitingForChecksum1:
newByte = self.serialPort.read()
if len(newByte) == 1:
currentCheckSum[0] = newByte[0]
self.serialState = SerialState.WaitingForChecksum2
if self.serialState == SerialState.WaitingForChecksum2:
newByte = self.serialPort.read()
if len(newByte) == 1:
currentCheckSum[1] = newByte[0]
self.serialState = SerialState.CommandDone
if self.serialState == SerialState.CommandDone:
# check the message integrity
receivedCommandCrc = Crc16()
receivedCommandCrc.process(currentSerialString.encode('utf-8'))
receivedCommandCrcBytes = receivedCommandCrc.finalbytes()
# print("Checksum Calc based on {}".format(currentSerialString.encode('utf-8')))
# print("Checksum Received: {}, Calculated: {}".format(currentCheckSum, receivedCommandCrcBytes))
if receivedCommandCrcBytes == currentCheckSum:
succeeded = True
string = currentSerialString.split(":")[1].rstrip(' \t\r\n\0')
if string == "None":
string = ""
else:
self.log("Acknowledgment Failed, received: {}".format(currentSerialString.rstrip("\t\r\n\0")), type="ERROR")
string = currentSerialString
break
return succeeded, string
def _sendCommand(self, command, payload, timeout=SERIAL_COMMAND_TIMEOUT, max_retry=SERIAL_COMMAND_MAX_TRIALS):
if not command in SERIAL_COMMANDS:
print("The command provided {} is not a valid serial command".format(command))
return FAILURE_CODE
sendBuffer = bytearray()
sendBuffer.append(STARTING_BYTE)
sendString = command + ":" + payload
sendBuffer.extend(sendString.encode("utf-8"))
sendBuffer.append(0x00)
newChecksum = Crc16()
# print("Checksum Calc based on {}".format(sendBuffer[1:]))
newChecksum.process(sendBuffer[1:])
checksumBytes = newChecksum.finalbytes()
sendBuffer.extend(checksumBytes)
# print(len(sendBuffer))
for attempt in range(max_retry):
if attempt != 0:
self.log(f"Attempt #{attempt+1} to send the command {command} with payload {payload}", type="DEBUG")
QCoreApplication.processEvents()
# t = time.time()
try:
self.serialPort.flushInput()
self.serialPort.write(sendBuffer)
except SerialTimeoutException:
self.serialPort.flushOutput()
continue
self.serialTimeoutTimer.setInterval(timeout)
self.serialTimeoutTimer.start()
succeeded, string = self.getSerialAck()
# print("The time spent from sending a command to receiving a reply (or timeouting) is ",time.time()-t)
if succeeded:
return string
elif not succeeded and "EXCEPTION" in string:
break
return FAILURE_CODE
def create_crc(self, n):
'''
Cria o crc
'''
crc = Crc16.calc(self.payloads[n - 1])
crc = crc.to_bytes(2, byteorder="big")
print(crc)
return crc
def calc_crc(data: bytes) -> int:
"""Calculate CRC from the data.
:param data: data to calculate CRC from
:type data: bytes
:return: calculated CRC
:rtype: int
"""
return Crc16.calc(data)
def parseData(data):
byte_data = bytearray(data)
header = byte_data[:26]
sop = header[0]
camera_id = header[1]
image_type = header[2]
width = (header[4] & 0xff) << 8 | \
(header[3] & 0xff)
height = (header[6] & 0xff) << 8 | \
(header[5] & 0xff)
utc_ts = (header[10] & 0xff) << 24 | \
(header[9] & 0xff) << 16 | \
(header[8] & 0xff) << 8 | \
(header[7] & 0xff)
payload_size = (header[14] & 0xff) << 24 | \
(header[13] & 0xff) << 16 | \
(header[12] & 0xff) << 8 | \
(header[11] & 0xff)
image_size = (header[18] & 0xff) << 24 | \
(header[17] & 0xff) << 16 | \
(header[16] & 0xff) << 8 | \
(header[15] & 0xff)
payload_offset_in_image = (header[22] & 0xff) << 24 | \
(header[21] & 0xff) << 16 | \
(header[20] & 0xff) << 8 | \
(header[19] & 0xff)
compression_ratio = header[23]
compression_algo = header[24]
header_crc8 = header[25]
crc8 = Crc8.calc(header[:25])
if crc8 != header_crc8:
logging.error("Header CRC mismatch:\n header crc8: %x\n calculated crc8: %x" % (header_crc8, crc8))
payload = data[26:payload_size + 26]
package_crc16 = (byte_data[-1] & 0xff) << 8 | \
(byte_data[-2] & 0xff)
crc16 = Crc16.calc(byte_data[:payload_size + 26])
if crc16 != package_crc16:
logging.error("Package CRC mismatch:\n package crc16: %x\n calculated crc16: %x" % (package_crc16, crc16))
header_data = {
'sop': sop,
'camera_id': camera_id,
'image_type': image_type,
'width': width,
'height': height,
'utc_ts': utc_ts,
'payload_size': payload_size,
'image_size': image_size,
'payload_offset_in_image': payload_offset_in_image,
'compression_ratio': compression_ratio,
'compression_algo': compression_algo,
'header_crc8': header_crc8,
'package_crc16': package_crc16
}
return header_data, payload
def sendSerialAck(self, result=None):
outBuffer = bytearray()
outBuffer.append(STARTING_BYTE)
if result == None:
result = "None"
ackBytes = ("ACK:" + result).encode("utf-8")
outBuffer.extend(ackBytes)
outBuffer.append(0x00)
newChecksum = Crc16()
newChecksum.process(outBuffer[1:])
checksumBytes = newChecksum.finalbytes()
outBuffer.extend(checksumBytes)
self.port.write(outBuffer)
def readSerial(self):
if self.serialState == SerialState.WaitingToStart:
newByte = self.port.read()
if len(newByte) == 1:
if newByte[0] == STARTING_BYTE:
self.serialState = SerialState.WaitingForString
if self.serialState == SerialState.WaitingForString:
newBytes = self.port.read_until(b'\0')
if len(newBytes) >= 1:
for i in range(len(newBytes)):
if newBytes[i] == STARTING_BYTE:
pass
else:
self._currentSerialString = self._currentSerialString + newBytes[
i:].decode("utf-8")
if newBytes[-1] == 0x00:
self.serialState = SerialState.WaitingForChecksum1
break
print(len(self._currentSerialString))
if self.serialState == SerialState.WaitingForChecksum1:
newByte = self.port.read()
if len(newByte) == 1:
self._currentCheckSum[0] = newByte[0]
self.serialState = SerialState.WaitingForChecksum2
if self.serialState == SerialState.WaitingForChecksum2:
newByte = self.port.read()
if len(newByte) == 1:
self._currentCheckSum[1] = newByte[0]
self.serialState = SerialState.CommandDone
if self.serialState == SerialState.CommandDone:
# check the command integrity
receivedCommandCrc = Crc16()
receivedCommandCrc.process(
self._currentSerialString.encode('utf-8'))
receivedCommandCrcBytes = receivedCommandCrc.finalbytes()
print("Checksum Calc based on {}".format(
self._currentSerialString.encode('utf-8')))
print("Checksum Received: {}, Calculated: {}".format(
self._currentCheckSum, receivedCommandCrcBytes))
if receivedCommandCrcBytes == self._currentCheckSum:
self.processSerialCommand(self._currentSerialString)
self._currentSerialString = ""
self._currentCheckSum[0] = 0x00
self._currentCheckSum[1] = 0x00
self.serialState = SerialState.WaitingToStart
def from_bytearray(byte_array) -> 'Packet':
assert byte_array[0] == Packet.SYNC_CHAR_1
assert byte_array[1] == Packet.SYNC_CHAR_2
if byte_array[2] == DataPayload.packet_class and byte_array[
3] == DataPayload.packet_id:
payload_len = int.from_bytes(byte_array[4:6], byteorder="big")
payload = DataPayload.from_bytearray(byte_array[6:6 + payload_len])
checksum = Crc16.calc(byte_array[:6 + payload_len])
assert checksum == int.from_bytes(byte_array[-2:], byteorder="big")
else:
raise ValueError(
f"Unknown packet class {byte_array[2]} and ID {byte_array[3]}")
return Packet(payload)
def to_bytearray(self) -> bytearray:
packet = bytearray()
# Packet headers
packet.extend(self.SYNC_CHAR_1.to_bytes(1, byteorder="big"))
packet.extend(self.SYNC_CHAR_2.to_bytes(1, byteorder="big"))
packet.extend(self.payload.packet_class.to_bytes(1, byteorder="big"))
packet.extend(self.payload.packet_id.to_bytes(1, byteorder="big"))
# Packet payload
payload_bytes = self.payload.to_bytearray()
packet.extend(len(payload_bytes).to_bytes(2, byteorder="big"))
packet.extend(payload_bytes)
# Checksum
packet.extend(Crc16.calc(packet).to_bytes(2, byteorder="big"))
return packet
def fragment(self, file, desc):
plSize = 114
nBytes = len(file)
nPacks = nBytes // plSize
payloads = []
packages = []
if nPacks < 1:
payloads.append(file)
else:
exPackSize = nBytes % plSize
startPoint = 0
for pack in range(0, nPacks):
payload = self.makePayload(file, startPoint, plSize)
payloads.append(payload)
startPoint += 114
payloads.append(self.makePayload(file, startPoint, exPackSize))
for index in range(len(payloads)):
# Header structure []
pl = payloads[index]
packSize = len(payloads)
header = bytes([
desc["type"], desc["sensorID"], desc["serverID"], packSize,
index,
len(pl), 0
])
if (index == 0):
header += bytes([index])
else:
header += bytes([index - 1])
header += Crc16.calc(pl).to_bytes(2, "big")
package = self.makePackage(header, pl)
packages.append(package)
return packages
def cmd(self, cmd, arg, rlen=0, data=None):
cmdb = struct.pack('>BI', cmd | 0x40, arg)
crc7 = Crc7.calc(cmdb)
txb = cmdb + bytes([(crc7 << 1) | 1])
with self:
self.spi.transfer(txb)
rxb = self._read_response(rlen + 1)
status = rxb[0]
rxb = rxb[1:]
error = status & 0xfe != 0
print("CMD%d: %s -> %x %s" % (cmd, txb.hex(), status, rxb.hex()))
if not error and data:
crc16 = Crc16.calcbytes(data)
data_start = 0xfe
ind = self.spi.transfer([data_start])
print("data start: %x" % data_start)
print("data read: %s" % ind.hex())
while len(data) > 0:
chunk = data[0:16]
data = data[16:]
ind = self.spi.transfer(chunk)
print("data write: %s" % chunk.hex())
print("data read: %s" % ind.hex())
ind = self.spi.transfer(crc16)
print("data crc: %s" % crc16.hex())
print("data read: %s" % ind.hex())
data_resp = self._read_response(4)[0]
self._wait_busy()
if data_resp & 0b00011111 != 0b00000101:
raise RuntimeError('data write error %x' % data_resp)
if rlen > 0:
return status, rxb
else:
return status
def packagePayload(payload,
width, height, utc_ts,
image_size, payload_offset_in_image,
compression_ratio, compression_algo):
# init header
header = bytearray(26)
# putting data to header
header[0] = 0x47 # sop
header[1] = 0x00 # camera id
header[2] = 0x01 # image type
header[3] = (width & 0xff) # width
header[4] = (width >> 8) & 0xff
header[5] = (height & 0xff) # height
header[6] = (height >> 8) & 0xff
header[7] = (utc_ts & 0xff) # utc timestamp
header[8] = (utc_ts >> 8) & 0xff
header[9] = (utc_ts >> 16) & 0xff
header[10] = (utc_ts >> 24) & 0xff
payload_size = len(payload)
header[11] = (payload_size & 0xff) # payload size
header[12] = (payload_size >> 8) & 0xff
header[13] = (payload_size >> 16) & 0xff
header[14] = (payload_size >> 24) & 0xff
header[15] = (image_size & 0xff) # image_size size
header[16] = (image_size >> 8) & 0xff
header[17] = (image_size >> 16) & 0xff
header[18] = (image_size >> 24) & 0xff
header[19] = (payload_offset_in_image & 0xff) # payload offset in image
header[20] = (payload_offset_in_image >> 8) & 0xff
header[21] = (payload_offset_in_image >> 16) & 0xff
header[22] = (payload_offset_in_image >> 24) & 0xff
header[23] = compression_ratio
header[24] = compression_algo
header[25] = Crc8.calc(header[:25])
data = header + payload
crc16 = Crc16.calc(data).to_bytes(2, byteorder="little")
# append package crc16
data += crc16
return data
def makeHeader(self, tipo, tamPayload, pckAtual=0, payload=b'\x01'):
if self.fileType.lower() == 'png':
img_typ = b'\x00'
elif self.fileType.lower() == 'jpg':
img_typ = b'\x01'
elif self.fileType.lower() == 'jpeg':
img_typ = b'\x02'
CRC = Crc16.calc(payload)
CRCbytes = CRC.to_bytes(2, byteorder='little')
quantStuff = self.quantStuff.to_bytes(1, byteorder='little')
pacote_atual = pckAtual.to_bytes(2, byteorder='little')
tamPayloadBT = tamPayload.to_bytes(1, byteorder='little')
head = tipo.to_bytes(1, byteorder='little') + self.destino + self.qPck + pacote_atual + tamPayloadBT + CRCbytes + img_typ + quantStuff + b'\x00'
return head
def verificaCRC(self,CRC,payload):
newCRC = Crc16.calc(payload)
if newCRC==int.from_bytes(CRC, byteorder='little'):
return True
return False
def main():
try:
#declaramos um objeto do tipo enlace com o nome "com". Essa é a camada inferior à aplicação. Observe que um parametro
#para declarar esse objeto é o nome da porta.
server = Tinker(serialName)
pacman = Squire()
serverLog = open("./serverLog.txt", "w")
serverLog.write(f"{dt.now()} / Communication started!\n")
IDs = {"sensorID": 42, "serverID": 1}
timeOut = pacman.makeAnswer(5, IDs)
running = True
orion = True
cont = 0
lastIndex = 0
print("-" * 25)
print("Port enabled")
print(f"Port: {server.comPort.fisica.name}")
print("-" * 25)
print("Waiting for Client to show up..")
# Loooop to receive handshake
while running:
if orion:
time.sleep(3)
if server.isEmpty():
print("-" * 25)
print("Buffer seems to be empty\nTrying again...")
time.sleep(0.5)
else:
handshake, thumb = server.getAnswer()
pacman.logging(serverLog, "r", handshake)
clientID = thumb["serverID"]
print("-" * 25)
print("There it is!")
# Check if the IDs are the same
if (clientID == IDs["serverID"]) and (thumb["type"] == 1):
orion = False
nPacks = thumb["nPacks"]
ans = pacman.makeAnswer(2, IDs)
server.sendPackage(ans)
pacman.logging(serverLog, "s", ans)
print("-" * 25)
print("Sent it the answer!")
else:
print("-" * 25)
print("It seems like the IDs don't match :(")
print(f"Client's: {clientID}")
print(f"Server's: {IDs['serverID']}")
continue
time.sleep(1)
if not orion:
if cont < nPacks:
timeTryAgain = time.time() + 2
timeOut = time.time() + 20
while True:
# if cont == 4:
# time.sleep(5)
if not server.isEmpty() and time.time() < timeOut:
head = server.comPort.getData(10)
headDict = pacman.readHeader(head)
if (headDict["type"] == 3):
iReceived = headDict["packIndex"]
# if (iReceived + 1) == headDict["nPacks"]:
# break
server.getPackage(head)
pacman.logging(serverLog, "r", server.pack)
print("-" * 25)
print(f"Pack of number {iReceived+1} received")
first = (iReceived == 0)
theOneAfter = (iReceived - lastIndex == 1)
rightEop = server.eopCheck()
crcCalc = Crc16.calc(server.payload).to_bytes(
2, "big").hex().upper()
crcReceived = headDict["CRC"]
rightCrc = crcCalc == crcReceived
print(rightCrc)
# rightCRC =
if (first or theOneAfter) and (rightEop):
print("Its the RIGHT pack")
server.addPayload(server.pack)
ans = pacman.makeAnswer(4, IDs, lastIndex)
server.sendPackage(ans)
pacman.logging(serverLog, "s", ans)
lastIndex = iReceived
cont += 1
else:
print("Its the WRONG pack")
ans = pacman.makeAnswer(6, IDs, lastIndex)
cont = lastIndex + 1
server.sendPackage(ans)
print("Telling Client to send again..",
lastIndex)
pacman.logging(serverLog, "s", ans)
break
elif (headDict["type"] == 1):
print("-" * 25)
print("That was the handshake again!")
ans = pacman.makeAnswer(2, IDs)
server.sendPackage(ans)
pacman.logging(serverLog, "s", ans)
print("You can start sending already...")
elif (headDict["type"] == 5):
print("TimeOut!!")
sys.exit()
else:
time.sleep(1)
if time.time() > timeOut:
orion = True
ans = pacman.makeAnswer(5, IDs)
server.sendPackage(ans)
pacman.logging(serverLog, "s", ans)
server.finnish()
sys.exit()
else:
if time.time() > timeTryAgain:
print("#" * 25)
ans = pacman.makeAnswer(6, IDs, lastIndex)
server.sendPackage(ans)
pacman.logging(serverLog, "s", ans)
else:
print("Holy shit, it worked!")
serverLog.write(f"{dt.now()} / Communication finished!")
serverLog.close()
running = False
imageW = "./imgReceived.png"
print("-------------------------")
print("Saving in directory:")
print(" - {}".format(imageW))
f = open(imageW, 'wb')
print("Hello There!")
server.assembleData()
f.write(server.fullArchive)
# Fecha arquivo de imagem
f.close()
print("Image saved!")
server.finnish()
except:
print("ops! :-\\")
server.finnish()