def handle_spi(z, payload, get_payload=False):
while True:
b = bytearray()
b.append(DUMMY)
c = z.transfer(b)
if c[0] == READY:
b = bytearray(cobs.encode(payload))
b.append(0x00)
l = len(b)
lb = bytearray()
lb.append((l & 0xff00) >> 8)
lb.append(l & 0x00ff)
z.write(lb)
z.write(b)
elif c[0] == BUSY:
pass
elif c[0] == READ:
b = z.read(2)
length = b[0] << 8 | b[1]
elif c[0] == DUMP_DATA:
b = z.read(length)
if not get_payload:
response = zr.Response()
response.ParseFromString(cobs.decode(bytes(b[:-1])))
if response.status_code != zr.OK:
raise StatusCodeError(response.status_code)
return
else:
return cobs.decode(bytes(b[:-1]))
sleep(0.02)
def fnDecodeCOBS(self, encodedCobsMsg):
"""
Purpose: Wrapper for cobs module to decode message.
Passed: Encoded COBS message.
Returns: Decoded COBS message.
"""
return cobs.decode(encodedCobsMsg)
def decode(package):
# Check package length
if len(package) < 4: # error
return 0, bytearray()
# Check package length
if len(package) > 258: # error
return 0, bytearray()
# Extract and check package ID/Type
id = package[1]
if id == 0: # error
return 0, bytearray()
# Verify package length
if len(package) != package[2] + 3: # error
return 0, bytearray()
# Decode payload
payload_length = len(package) - 4 # COB = Consistent Overhead Byte Stuffing
try:
data = cobs.decode(package[3:])
if len(data) != payload_length:
return 0, bytearray()
return id, data
except:
return 0, bytearray()
def parse_packet(packet_in):
"""
Parse the packet returning a tuple of [int, int, bytes].
If unable to parse the packet, then return 0,0,b''.
:param packet_in: bytes of a full packet
:return: device_id, packet_id, data in bytes.
"""
packet_in = bytearray(packet_in)
if packet_in and len(packet_in) > 3:
try:
decoded_packet = bytearray(cobs.decode(bytes(packet_in)))
except cobs.DecodeError as e:
print("parse_packet(): Cobs Decoding Error, {}".format(e), )
return 0, 0, bytearray()
if decoded_packet[-2] != len(decoded_packet):
print("parse_packet(): Incorrect length: length is {} in {}".format(len(decoded_packet), [str(x) for x in decoded_packet]))
return 0, 0, bytearray()
else:
if CRC8_FUNC(bytes(decoded_packet[:-1])) == decoded_packet[-1]:
rx_data = decoded_packet[:-4]
device_id = decoded_packet[-3]
packet_id = decoded_packet[-4]
rx_data = rx_data
return device_id, packet_id, rx_data
else:
print("parse_packet(): CRC error in packet ")
return 0, 0, bytearray()
return 0, 0, bytearray()
def parse_packet(packet_in: Union[bytes, bytearray]) -> Tuple[int, int, bytes]:
"""
Parse the packet returning a tuple of [int, int, bytes].
If unable to parse the packet, then return 0,0,b''.
:param packet_in: bytes of a full packet
:return: device_id, packet_id, data in bytes.
"""
packet_in = bytearray(packet_in)
if packet_in and len(packet_in) > 3:
try:
decoded_packet: bytes = cobs.decode(packet_in)
except cobs.DecodeError as e:
logger.warning(f"parse_packet(): Cobs Decoding Error, {e}")
return 0, 0, b''
if decoded_packet[-2] != len(decoded_packet):
logger.warning(f"parse_packet(): Incorrect length: length is {len(decoded_packet)} "
f"in {[hex(x) for x in list(decoded_packet)]}")
return 0, 0, b''
else:
if BPLProtocol.CRC8_FUNC(decoded_packet[:-1]) == decoded_packet[-1]:
rx_data = decoded_packet[:-4]
device_id = decoded_packet[-3]
packet_id = decoded_packet[-4]
rx_data = rx_data
return device_id, packet_id, rx_data
else:
logger.warning(f"parse_packet(): CRC error in {[hex(x) for x in list(decoded_packet)]} ")
return 0, 0, b''
return 0, 0, b''
def handle_packet(self, packet: bytes):
"""
implements Packetizer.handle_packet. Shame this can't be private
"""
# decode packet
try:
raw = cobs.decode(packet)
msg = messages.Message.deserialize(raw)
except (messages.DecodeError, cobs.DecodeError) as e:
if packet.startswith(b'!DEBUG'):
packet = packet[6:]
print("Debug: " + packet.decode('utf8'))
else:
# not much we can do about garbage messages, so log and continue
print(
traceback.format_exception_only(type(e), e)[0].strip(),
repr(packet))
return
# update state, depending on type of message
if isinstance(msg, messages.Sensor):
self._adc_reading = msg
elif isinstance(msg, messages.IMUScaled):
self._imu_reading = msg
elif isinstance(msg, messages.ServoPulse):
if self._mode in (ControlMode.Torque, ControlMode.Position):
self._servo_us = np.asarray(msg)
elif isinstance(msg, messages.Ping):
self._ping_recvd = True
def sendRotation(self, distance):
encodedCommand = cobs.encode(b"r")
self.ser.write(encodedCommand + b"\00")
encoded = cobs.encode(struct.pack("f", distance))
self.ser.write(encoded + b"\00")
dataBuffer = b""
startTime = int(round(time.time() * 1000))
currentTime = startTime
while (currentTime - startTime) < 1000:
if self.ser.inWaiting() > 0:
val = self.ser.read(1)
if val == b'\x00':
data = cobs.decode(dataBuffer)
unpackedData = struct.unpack('f', data)[0]
if (unpackedData == 1.0):
return True
else:
return False
dataBuffer = b""
else:
dataBuffer += val
currentTime = int(round(time.time() * 1000))
return False
def receive(self):
data = self.ser.read_until(zeroByte)
n = len(data)
if n > 0:
# clip last byte & decode
data = cobs.decode(data[0:(n - 1)])
if data[0] == 0:
return DebugMessage(data[1:].decode('utf-8'))
if data[0] == 1:
count = data[1]
posArr = [0, 0, 0, 0, 0, 0]
speedArr = [0, 0, 0, 0, 0, 0]
for x in range(count):
ix1 = 2 + x * 6
ix2 = ix1 + 4
pos = int.from_bytes(data[ix1:ix2],
byteorder='little',
signed=True)
speed = int.from_bytes(data[ix2:ix2 + 2],
byteorder='little',
signed=True)
posArr[x] = pos
speedArr[x] = speed
return StatusMessage(posArr, speedArr)
raise "Unknown header: {}".format(data[0])
def _recv_command(self):
"""Reads a full line from the microcontroller
We expect to complete a read when this is invoked, so don't invoke unless
you expect to get data from the microcontroller. we raise a timeout if we
cannot read a command in the alloted timeout interval."""
# we rely on the passed-in timeout
while True:
c = self._serial.read(1)
if not c:
raise serial.SerialTimeoutException(
"Couldn't recv command in %d seconds" %
self.IO_TIMEOUT_SEC)
# finished reading an entire COBS structure
if c == '\x00':
# grab the data and reset the buffer
data = self._read_buf[0:self._read_buf_pos]
self._reset_read_buf()
# return decoded data
return cobs.decode(str(bytearray(data)))
# still got reading to do
else:
self._read_buf[self._read_buf_pos] = c
self._read_buf_pos += 1
# ugh. buffer overflow. wat do?
if self._read_buf_pos == len(self._read_buf):
# resetting the buffer likely means the next recv will fail, too (we lost the start bits)
self._reset_read_buf()
raise RuntimeError("IO read buffer overflow :(")
def run(self):
print "BsClient running"
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect((self.ip, self.port))
#maf = MessageAsciiFactory()
try:
while True:
response = sock.recv(1024)
if not response:
break
try:
decoded = bytearray(cobs.decode(response))
if decoded[0] == 0xAB and decoded[1] == 0xAB:
pass
else:
r = AppMessage(decoded)
self.received.emit(copy(r))
except Exception as ex:
print str(ex)
# records = maf.receive(response)
# for r in records:
# self.received.emit(copy(r))
except Exception as ex:
print "BsClient exception: " + str(ex)
pass
finally:
print "BsClient exiting"
try:
sock.close()
except Exception:
# socket may already be closed, so just ignore
pass
def handle_packet(self, arr):
"""Handle an incoming packet from the serial port. The packetizer has stripped the
line-termination \0 byte from it already.
"""
# write data immediately to the log file, no matter what. For future parsing, we need to
# re-append the line termination symbol.
self.log_file.write(base64.b64encode(arr + b'\0') + b'\n')
# COBS decode the array
if not len(arr):
return
try:
dec = cobs.decode(arr)
except cobs.DecodeError as e:
logging.warning(str(e))
return
packet_type = dec[0]
if packet_type == 0:
self.handle_data_packet(dec)
elif packet_type == 1:
self.handle_command_packet(dec)
elif packet_type == 2:
logging.error(f'Received error packet {dec}')
else:
logging.error(
f'Received unknown packet type: {packet_type} in packet {dec}')
def _run(self):
rxData = []
self._stateMutex.acquire()
while True:
for byte in self.serial.read(self.serial.in_waiting):
if byte == 0:
#print("Rx raw", bytes(rxData))
#print("Rx", cobs.decode(bytes(rxData)))
getattr(self.lib,
"decode_" + self.name)(self._state,
cobs.decode(bytes(rxData)))
rxData.clear()
else:
rxData.append(byte)
txArray = self.ffi.new("uint8_t[]",
getattr(self.lib, "size_tx_" + self.name))
txSize = getattr(self.lib, "encode_" + self.name)(self._state,
txArray)
if txSize:
while txSize:
txData = bytes(txArray)[0:txSize]
#print("Tx", txSize, txData)
#print("Tx raw", cobs.encode(txData) + b'\0')
self.serial.write(cobs.encode(txData) + b'\0')
self._txDone.set()
txSize = getattr(self.lib,
"encode_" + self.name)(self._state,
txArray)
else:
self._stateMutex.release()
select.select([self.serial, self._txReady], [], [])
self._stateMutex.acquire()
self._txReady.clear()
def unpack(frame):
#print("----------------")
frame = ''.join(char for char in frame if char.isalnum())
if (len(frame) != 32):
# # TODO throw an error up on screen
# #print("Malformed frame len " + str(len(frame)) + " encountered - skipping")
return -1
'''frameprint = ''
odd = False
for char in frame:
frameprint += char
if odd:
frameprint += " "
odd = not odd
print("Encoded frame: " + frameprint.upper())'''
try:
decoded = cobs.decode(binascii.unhexlify(frame))
except Exception as e:
#print("Decode failed: " + str(e))
return -1
# Calculate checksum
checksum = fletcher16(decoded[0:13])
cs_calc = chr(checksum >> 8) + " " + chr(checksum & 0xFF)
cs_rcvd = chr(decoded[14]) + " " + chr(decoded[13])
if cs_calc != cs_rcvd:
#print("Decode failed: Checksum mismatch - calc: " + cs_calc + " - rcvd: " + cs_rcvd)
return -1
'''out = "Decoded frame: "
for char in decoded:
out += binascii.hexlify(char).upper() + " "
print(out)'''
return decoded
def unpackData(self, inPacket):
crcCalculator = CRC16()
packetValid = False
try:
correctedPacket = self.rs.decode(inPacket)[0]
packet = cobs.decode(correctedPacket)
crc = crcCalculator.calculate(bytes(packet[:-2]))
# Check CRC validity
if ((crc & 0xFF00) >> 8) == packet[-2] and (crc
& 0xFF) == packet[-1]:
packetValid = True
# Get packet length
packetLen = packet[6] << 8 | packet[7]
# Origin callsign
originCallsign = packet[0:6].decode('utf-8').strip()
# Check for missing packets
if packetValid:
expectedSequenceId = (self.sequenceId + 1) & 0xFFFF
packetSequenceId = packet[8] << 8 | packet[9]
if expectedSequenceId != packetSequenceId:
self.packetLost += 1
self.sequenceId = packetSequenceId
payload = packet[10:-2]
return (originCallsign, packetLen, packetSequenceId, payload)
except Exception as e:
#print('Decode error '+str(e)+'\n'+str(inPacket))
return None
def handle_packet(self, arr):
"""Handle an incoming packet from the serial port. The packetizer has stripped the
line-termination \0 byte from it already.
"""
for fn_raw_callback in self.raw_callbacks:
try:
fn_raw_callback(arr)
except BaseException as e:
logging.critical(e)
# COBS decode the array
if not len(arr):
return
try:
dec = cobs.decode(arr)
except cobs.DecodeError as e:
logging.warning(str(e))
return
packet_type = dec[0]
if packet_type == 0:
self.unpack_data_packet(dec)
elif packet_type == 1:
self.unpack_command_packet(dec)
elif packet_type == 2:
logging.error(f'Received error packet {dec}')
else:
logging.error(f'Received unknown packet type: {packet_type} in packet {dec}')
def from_bytes(cls, raw_pkt):
if not raw_pkt:
raise InvalidLength("Unexpected length: {}".format(len(raw_pkt)))
if raw_pkt[-1] == 0:
raw_pkt = raw_pkt[:-1]
raw = cobs.decode(raw_pkt)
if len(raw) < 8:
raise InvalidLength("Need at least 4 header and 4 footer bytes")
frm_crc32 = uint32_t.unpack(raw[-4:])[0]
computed_crc32 = binascii.crc32(raw[:-4])
if frm_crc32 != computed_crc32:
raise InvalidCRC(
"Frame CRC does not match! given {} != computed {}".format(
frm_crc32, computed_crc32
)
)
frm_type = uint16_t.unpack(raw[0:2])[0]
frm_len = uint16_t.unpack(raw[2:4])[0]
frm_data = raw[4 : (4 + frm_len)]
return cls(type=frm_type, data=frm_data)
def ping_robot(serial):
# ecriture de la commande de ping
ping = _stm32_pack_ping()
serial.write(ping)
serial.flush()
time.sleep(0.5)
# TODO: extraire logique de lecture d'une commande
# lecture de la reponse avec timeout
start_time = time.time()
buf = serial.read(1)
while not b'\0' in buf:
num_bytes_to_read = serial.inWaiting()
if num_bytes_to_read > 0:
buf += serial.read(1)
elif (time.time() - start_time) > SERIAL_TIMEOUT:
raise Exception("Serial timeout (aucun robot detecte probablement)")
# decodage de la reponse
response = ""
if buf == "\0":
response = "\0"
else:
buf = buf[0:-1]
response = cobs.decode(buf)
if len(response) < len(_stm32_generate_header()):
raise Exception("Decodage de cobs a echoue pendant le ping d'init. (reponse invalide)")
if response[3] == STM32_CMD_HEART_BEAT_RESPOND:
print("Le robot a repondu au heartbeat!")
def parse_input(self, input):
input = input[:-1] # Remove last byte (which is delimiter)
packet = cobs.decode(input) # COBS decode packet
message = SimpleMessage() # Create mesage object
message.ParseFromString(packet) # Parse message object
if self.receive_callback:
self.receive_callback(message)
def fnRetieveMessage(self):
"""
Purpose: Decode received COBS byte string to
Passed: None.
Return: Status of message.
"""
if self.protocol == ('TCP'):
data = [] # List containing characters of byte string
c = self.socket.recv(1) # Receive 1 byte of information
# Continue acquiring bytes of data until end point is reached. Combine into byte string.
while c != b'\x00':
if c == b'':
self.socket.close()
return "Disconnected."
data.append(c)
c = self.socket.recv(1)
data = b''.join(data)
elif self.protocol == 'UDP':
data = self.socket.recv(128)
# Try to decode message and returns exception to avoid closing the program
try:
return cobs.decode(data)
except Exception as e:
print("Failed to decode message due to: {}".format(e))
async def read_data(self):
reader, _ = await serial_asyncio.open_serial_connection(
url=self.serial, baudrate=self.baudrate)
while self.run:
buf = await reader.readuntil(b'\x00')
try:
data = list(cobs.decode(buf[:-1])) # Discard separator
except cobs.DecodeError as e:
sys.stderr.write(str(e) + '\n')
continue
else:
data = np.array(data[2:])
# Plot
self.plotline.setData(data)
peaks = np.where(data[150:] > self.threshold)[0]
if len(peaks):
peak = 150 + (peaks[0] - 10)
distance = (self.c * (peak / self.fs)) / 2 - 10
text = ''
if distance > self.ranges['a'][
0] and distance < self.ranges['a'][1]:
text = 'a'
elif distance > self.ranges['b'][
0] and distance < self.ranges['b'][1]:
text = 'b'
elif distance > self.ranges['c'][
0] and distance < self.ranges['c'][1]:
text = 'c'
txt = '<div style="text-align: center"><span style="color: #FFF;font-size:28pt;">%s</span></div>' % (
text)
self.l.setHtml(txt)
else:
self.l.setText('')
def _unittest_frame_compile_service() -> None:
from pyuavcan.transport import Priority, ServiceDataSpecifier, Timestamp
f = SerialFrame(timestamp=Timestamp.now(),
priority=Priority.FAST,
source_node_id=SerialFrame.FRAME_DELIMITER_BYTE,
destination_node_id=None,
data_specifier=ServiceDataSpecifier(123, ServiceDataSpecifier.Role.RESPONSE),
transfer_id=1234567890123456789,
index=1234567,
end_of_transfer=False,
payload=memoryview(b''))
buffer = bytearray(0 for _ in range(50))
mv = f.compile_into(buffer)
assert mv[0] == mv[-1] == SerialFrame.FRAME_DELIMITER_BYTE
segment_cobs = bytes(mv[1:-1])
assert SerialFrame.FRAME_DELIMITER_BYTE not in segment_cobs
segment = cobs.decode(segment_cobs)
# Header validation
assert segment[0] == _VERSION
assert segment[1] == int(Priority.FAST)
assert (segment[2], segment[3]) == (SerialFrame.FRAME_DELIMITER_BYTE, 0)
assert (segment[4], segment[5]) == (0xFF, 0xFF)
assert segment[6:8] == ((1 << 15) | (1 << 14) | 123).to_bytes(2, 'little')
assert segment[8:16] == b'\x00' * 8
assert segment[16:24] == 1234567890123456789 .to_bytes(8, 'little')
assert segment[24:28] == 1234567 .to_bytes(4, 'little')
# Header CRC here
# CRC validation
assert segment[32:] == pyuavcan.transport.commons.crc.CRC32C.new(f.payload).value_as_bytes
def output(self) -> Optional[bytes]:
"""Produce the next COBS-decoded payload.
Returns:
An optional bytestring of the COBS-decoded payload from the next
available COBS-encoded body in the internal buffer. Returns None if
there are no bodies available in the internal buffer.
Raises:
exceptions.ProtocolDataError: the body is obviously incorrect for
COBS decoding; note that it is possible that a body may have
have accumulated byte errors which would silently cause an
incorrect COBS decoding but would not be detected by the decoder
and thus would not raise an exception.
"""
frame = self._buffer.output()
if frame is None:
return None
try:
decoded: bytes = cobs.decode(frame)
except cobs.DecodeError as exc:
raise exceptions.ProtocolDataError(
'Could not COBS-decode body: {!r}'.format(frame)
) from exc
except TypeError as err:
raise exceptions.ProtocolDataError(
'Could not COBS-encode body: required type bytes found type {}'.
format(type(frame))
) from err
self._logger.debug(decoded)
return decoded
def cobs_decode(data):
"""
Decode COBS-encoded DATA.
"""
try:
return bytearray(cobs.decode(data))
except cobs.DecodeError:
return bytearray()
def unpack(frame): try: if len(frame) != 16: return -1 decoded = cobs.decode(frame) return decoded if fletcher16(decoded[0:13]) == (decoded[14] << 8 | decoded[13]) else -1 except Exception: return -1
def establishConnection():
global stateMach
curTime = time.time()
ports = getSerialPorts()
i = 0
while (len(ports) == 0):
b = "Waiting for Serial Devices" + "." * i + " "*(3-i)
print(b, end="\r")
if (i > 2):
i = 0
else:
i += 1
time.sleep(0.5)
ports = getSerialPorts()
print("Found Serial devicess on ports: " + str(ports))
for i in range(len(ports)):
stateMach['devices'][i] = serial.Serial(ports[i], 115200)
stateMach['devices'][i].close()
stateMach['devices'][i].open()
#stateMach['devices'] = serial.Serial(ports[0], 115200)
#stateMach['devices'].close()
#stateMach['devices'].open()
#time.sleep(1)
print("[HOST] Searching for Heavenli devicess over serial...")
connectionEstablished = False
sendOrReceive = True
while not connectionEstablished:
if time.time() - stateMach['t0'] > 0.251:
if (sendOrReceive):
enmass = cobs.encode(b'quack')+b'\x00'
stateMach['devices'].write(enmass)
sendOrReceive = False
else:
bytesToRead = stateMach['devices'].inWaiting()
if (bytesToRead > 0):
try:
print("[HOST] Incoming Bytes: " + str(int(bytesToRead)))
zeroByte = b'\x00'
mess = stateMach['devices'].read_until( zeroByte )[0:-1]
mess = str(cobs.decode( mess ))[2:-1]
print(mess)
#stateMach['devices'].flushInput()
except:
print("Error Decoding Packet")
if (bytesToRead > 128):
#print("flushing buffer")
stateMach['devices'].flushInput()
sendOrReceive = True
#sendOrReceive = not sendOrReceive
stateMach['t0'] = time.time()
time.sleep(0.25)
def read_packet(self) -> comm.TxMicro:
byte = self.ser.read()
result = b''
while byte != b'\x00':
result += byte
byte = self.ser.read()
tx = comm.TxMicro()
tx.ParseFromString(cobs.decode(result))
return tx
def receive_packet(self):
data = self.ser.read_until(b'\0').rstrip(b'\0')
try:
data = cobs.decode(data)
hexdump(print, data, prefix=' <- ')
except Exception as e:
hexdump(print, data, prefix=' FE ')
raise e
return depacketize(data)
def read_cobs_msg(ser, debug=False):
raw = ser.read_until(MSG_DELIM, 512)
xs = bytearray(raw)
xs.pop() # Remove delim.
dec = bytearray(cobs.decode(xs))
if debug:
print(f'DEBUG READ: [RAW] - {raw} | [DECODED] - {dec}')
return dec
def read_names(num_of_images):
names = []
example = "IMG_0519.JPG"
with open(conf_default.SOURCE_FOLDER_NAME + 'names.byte', "rb") as readdata:
data = cobs.decode(readdata.read())
l = len(example)
for i in range(num_of_images):
names.append(data[i * l:(i + 1) * l])
return names
def decode(self, payload):
commands = []
payload_index = 0
while payload_index < len(payload):
self.staging_buffer[self.staging_index] = payload[payload_index]
self.staging_index += 1
payload_index += 1
if self.staging_buffer[self.staging_index - 1] == 0:
try:
buffer = cobs.decode(
self.staging_buffer[:self.staging_index - 1])
except:
print(f'cobs.decode encountered an exception')
self.reset()
return commands, False
logging.debug(
f'EPXProtocol.decode decoded {len(buffer)} bytes')
if len(buffer) > 0:
if buffer[0] == FrameType.HEADERPLUSDATA.value:
offset = HeaderSizes.PRIMARYHEADER.value
device_header = DeviceProtocolHeader._make(
unpack('<BBHB', buffer[:offset]))
self.output_format = device_header.format
self.output_buffer = bytearray(buffer[offset:])
self.output_bytes_remaining = device_header.length - len(
self.output_buffer)
logging.debug(
f'EPXProtocol.decode found HEADER with {len(self.output_buffer)} bytes. Need {self.output_bytes_remaining} bytes more'
)
elif buffer[
0] == FrameType.DATA.value and self.output_buffer != None:
offset = HeaderSizes.DATAHEADER.value
data = buffer[offset:]
self.output_buffer.extend(data)
self.output_bytes_remaining -= len(data)
logging.debug(
f'EPXProtocol.decode found DATA with {len(self.output_buffer)} bytes. Need {self.output_bytes_remaining} bytes more'
)
if self.output_bytes_remaining == 0:
header, data = self._decode_command(self.output_buffer)
commands.append((header, data))
self.output_buffer = None
self.staging_index = 0
self.staging_index = 0
if self.staging_index == COBS_MAXFRAME_SIZE:
self.reset()
send_ack = self.staging_index > 0 or self.output_bytes_remaining > 0
return commands, send_ack
def receive(self):
if self.stop_receive: #If processing a stream - do not process incoming packets
pass
else:
if self.active_port is not None: # Should be redundant - better safe than sorry
temp_buffer = bytearray(self.active_port.readAll().data())
# print("Temp buffer: " + str(temp_buffer))
if self.download_stream_size and self.stream_download_timeout: #If stream is expected and it has timed out, clear the serial buffer before proceeding
if time.time(
) > self.stream_download_timeout: # Check to make sure that stream has not yet timed out
self.showMessage(
"Error: Stream download timed out with " +
str(len(self.serial_buffer)) + " of " +
str(self.download_stream_size) +
" bytes received. Stream aborted.")
self.serial_buffer = []
self.download_stream_size = None # Clear download stream flag
self.stream_download_timeout = None #Clear timeout timer
for i, byte in enumerate(temp_buffer):
if self.download_stream_size: #Check if non-COBS encoded data stream is expected
self.serial_buffer.append(byte)
if self.download_stream_size == len(
self.serial_buffer
): #If streaming is active and full length stream is received, send it to the command queue
self.stop_receive = True
self.command_queue.append(self.serial_buffer)
self.serial_buffer = []
self.serialRouter()
self.stop_receive = False
elif self.serial_buffer:
if self.serial_buffer[
0] == 1: #If a message packet is being received in place of the stream - clear stream flag so message can be processed
self.download_stream_size = None # Clear download stream flag
self.stream_download_timeout = None # Clear timeout timer
elif byte == 0:
try:
self.command_queue.append(
cobs.decode(bytes(self.serial_buffer)))
self.serial_buffer = []
self.serialRouter()
except cobs.DecodeError:
#self.showMessage("Warning: Invalid COBS frame received from driver. Check connection.")
if debug:
print("Invalid COBS packet")
print(temp_buffer[:i])
print(self.serial_buffer)
self.serial_buffer = []
self.dropped_frame_counter += 1
else:
self.serial_buffer.append(byte)
return True
def parse_from_cobs_image(header_payload_crc_image: memoryview,
timestamp: pyuavcan.transport.Timestamp) -> typing.Optional[SerialFrame]:
"""
:returns: Frame or None if the image is invalid.
"""
try:
unescaped_image = cobs.decode(bytearray(header_payload_crc_image))
except cobs.DecodeError:
return None
return SerialFrame.parse_from_unescaped_image(memoryview(unescaped_image), timestamp)
def get(self):
outBuffer = ""
while 1:
read = self.ser.read(1000)
if len(read) == 0:
print "timeout"
break
outBuffer += read
if outBuffer[-1] == '\x00':
break
return cobs.decode(bytes(outBuffer[:-1]))
def proxy_receive(self):
self.rxData = []
encData = []
timeout = 2000
duration = 0
if(self.debug == False):
while(duration < timeout):
if self._logLevel >= 3: print(" Available bytes = {0}".format(self.rxBuffer.available()))
if(self.rxBuffer.available() > 0):
if (self.rxBuffer.available() not in RadioInterface.VALID_PACKET_SIZES):
print(" Unknown packet size detected: {}. Flushing serial buffer...".format(self.rxBuffer.available()))
while(self.rxBuffer.available() != 0):
self.rxBuffer.read()
continue
encData.append(ord(self.rxBuffer.read()))
if(encData[0] == 0): #caught the end of a previous frame
return 0
while(encData[-1] != 0): #get bytes until end of frame
encData.append(ord(self.rxBuffer.read()))
encData = encData[0:-1] #remove trailing 0
tempData = list(cobs.decode(''.join(struct.pack('<B',x) for x in encData)))
self.rxPacket = Protocol.parse_packet(tempData)
self.rxData = list(ord(x) for x in tempData)
if self._logLevel >= 3:
print(" Encoded: {0}".format(["{:02X}".format(x) for x in encData]))
print(" Raw: {0}".format(["{:02X}".format(x) for x in self.rxData]))
if(self.rxPacket.data[0] == ProtocolDefs.CMD_NACK):
print(" Radio proxy node returned an error: {}".format(["{:02X}".format(x) for x in self.rxPacket.data]))
return 0
return 1
else:
duration += 100
time.sleep(0.1)
if(duration >= timeout):
print("RadioInterface.proxy_receive: Timeout")
self.rxPacket = ProtocolDefs.rawPacket()
self.rxData = []
return -1
else:
tempData = Protocol.form_packet(type=1, srcID=6, dstID=self._nodeID, shSrcID=6, shDstID=self._nodeID, cmd=ProtocolDefs.CMD_ACK, addr=1, data=2, enc='bytes')
self.rxPacket = Protocol.parse_packet(tempData)
self.rxData = list(ord(x) for x in tempData)
return 1
def decode_format4(data_str,device_info):
"""
Converts raw data of the format 4, which is popped from the deque
given as argument
The data is sends in binary packages using the the consistent overhead
byte stuffing (`COBS
<https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing>`_)
algorithm.
After decoding cobs the binary data has the following content
==== ====
Byte Usage
==== ====
0 0xAE (Packet type)
1 FLAG LTC (see comment)
2 LTC COMMAND 0 (as send to the AD Converter)
3 LTC COMMAND 1
4 LTC COMMAND 2
5 packet counter msb
...
9 packet counter lsb
10 clock 10 khz msb
...
14 clock 10 khz lsb
15 LTC2442 0 msb
16 LTC2442 1
17 LTC2442 2 lsb
. 3 bytes per activated LTC2442
==== ====
Maximum packet size: 17 + 8 * 3 = 41 Byte
FLAG LTC: Every bit is dedicated to one of the eight physically
available LTC2442 and is set to one if activated
Args:
deque:
stream:
dt: Time interval for polling [s]
Returns:
[]: List of data
"""
funcname = '.decode_format4()'
data_packets = []
ind_ltcs = [0,0,0,0,0,0,0,0]
nstreams = (max(device_info['channel_seq']) + 1)
#data_stream = [[] for _ in range(nstreams) ]
data_split = data_str.split(b'\x00') # Packets are separated by 0x00
err_packet = 0
cobs_err_packet = 0
ind_bad = []
ind_bad0 = 0
good_packet = 0
if(len(data_split) > 1):
#print('data_str',data_str)
#print('data_split',data_split)
if(len(data_split[-1]) == 0): # The last byte was a 0x00
data_str = b''
else:
data_str = data_split[-1]
data_split.pop() # remove last element
for data_cobs in data_split:
ind_bad0 += len(data_cobs)
# Timing
ta = []
#print('Cobs data:')
#print(data_cobs)
try:
#logger.debug(funcname + ': ' + data_decobs.encode('hex_codec'))
if(len(data_cobs) > 3):
data_decobs = cobs.decode(data_cobs)
#print('decobs data:')
#print(data_decobs)
#print(data_decobs[0],type(data_decobs[0]))
packet_ident = data_decobs[0]
#logger.debug(funcname + ': packet_ident ' + str(packet_ident))
# LTC2442 packet
if(packet_ident == 0xae):
#print('JA')
#packet_flag_ltc = ord(data_decobs[1]) # python2
packet_flag_ltc = data_decobs[1]
num_ltcs = bin(packet_flag_ltc).count("1")
# Convert ltc flat bits into indices
# If this is slow, this is a hot cython candidate
for i in range(8):
ind_ltcs[i] = (packet_flag_ltc >> i) & 1
ind_ltc = ind_ltcs
packet_size = 5 + 5 + 5 + num_ltcs * 3
packet_com_ltc0 = data_decobs[2]
packet_com_ltc1 = data_decobs[3]
packet_com_ltc2 = data_decobs[4]
# Decode the command
#speed,channel = ltc2442.interprete_ltc2442_command([packet_com_ltc0,packet_com_ltc1,packet_com_ltc2],channel_naming=1)
speed,channel = ltc2442.interprete_ltc2442_command_test([packet_com_ltc0,packet_com_ltc1,packet_com_ltc2],channel_naming=1)
ind = 5
#logger.debug(funcname + ': ltc flag ' + str(packet_flag_ltc))
#logger.debug(funcname + ': Num ltcs ' + str(num_ltcs))
#logger.debug(funcname + ': Ind ltc ' + str(ind_ltc))
#logger.debug(funcname + ': channel ' + str(channel))
#logger.debug(funcname + ': packet_size ' + str(packet_size))
#logger.debug(funcname + ': len(data_cobs) ' + str(len(data_cobs)))
if(len(data_decobs) == packet_size):
packet_num_bin = data_decobs[ind:ind+5]
packet_num = int(packet_num_bin.hex(), 16) # python3
ind += 5
packet_cnt10k_bin = data_decobs[ind:ind+5]
packet_cnt10ks = int(packet_cnt10k_bin.hex(), 16)/device_info['counterfreq']
data_list = [packet_num,packet_cnt10ks]
data_packet = {'num':packet_num,'cnt10ks':packet_cnt10ks}
data_packet['type'] = 'L'
data_packet['spd'] = speed
data_packet['ch'] = channel
data_packet['ind'] = ind_ltcs
data_packet['V'] = [9999.99] * num_ltcs
ind += 5
#logger.debug(funcname + ': Packet number: ' + packet_num_bin.hex())
#logger.debug(funcname + ': Packet 10khz time ' + packet_cnt10k_bin.hex())
for n,i in enumerate(range(0,num_ltcs*3,3)):
data_ltc = data_decobs[ind+i:ind+i+3]
data_ltc += 0x88.to_bytes(1,'big') # python3
if(len(data_ltc) == 4):
#conv = ltc2442.convert_binary(data_ltc,ref_voltage=4.096,Voff = 2.048)
conv = ltc2442.convert_binary_fast(data_ltc,ref_voltage=4.096,Voff = 2.048)
#print(conv)
data_packet['V'][n] = conv
# This could make trouble if the list is too short ...
data_list.append(conv)
#else:
# data_packet.append(9999.99)
#data_stream[channel].append(data_list)
data_packets.append(data_packet)
good_packet += 1
else:
# Peter temporary
logger.debug(funcname + ': Wrong packet size, is:' + str(len(data_cobs)) + ' should: ' + str(packet_size) )
err_packet += 1
#input('fds')
ind_bad.append(ind_bad0)
#print('data_cobs:',data_cobs)
#print('data_decobs:',data_decobs)
elif(packet_ident == 0x53): # Status string packet 'S'
data_utf8 = data_decobs.decode(encoding='utf-8')
data_split = data_utf8.split(';')
data_packet = {'type':'Stat'}
tstr = data_split[1]
td_tmp = datetime.datetime.strptime(tstr, '%Y.%m.%d %H:%M:%S')
#https://stackoverflow.com/questions/7065164/how-to-make-an-unaware-datetime-timezone-aware-in-python
data_packet['date'] = td_tmp.replace(tzinfo = device_info['timezone'])
data_packet['timestamp']= datetime.datetime.timestamp(data_packet['date'])
data_packet['date_str'] = tstr
data_packet['format'] = int(data_split[2].split(':')[1])
data_packet['cnt10ks'] = float(data_split[3])/device_info['counterfreq']
data_packet['cnt32k'] = int(data_split[4])
data_packet['start'] = int(data_split[5].split(':')[1])
data_packet['show'] = int(data_split[6].split(':')[1])
data_packet['log'] = int(data_split[7].split(':')[1])
data_packet['sd'] = int(data_split[8].split(':')[1])
if(len(data_split[9]) > 1):
data_packet['filename'] = data_split[9]
else:
data_packet['filename'] = None
#print('Status!',data_packet)
data_packets.append(data_packet)
elif(packet_ident == 0xac): # ACC IMU packet
if(len(data_decobs) > 12):
ind = 1
packet_num_bin = data_decobs[ind:ind+5]
packet_num = int(packet_num_bin.hex(), 16) # python3, its unsigned, TODO check how to do that
ind += 5
packet_cnt10k_bin = data_decobs[ind:ind+5]
packet_cnt10ks = int(packet_cnt10k_bin.hex(), 16)/device_info['counterfreq']
ind += 5
accx = int.from_bytes(data_decobs[ind:ind+2],byteorder='big',signed=True)/16384.
ind += 2
accy = int.from_bytes(data_decobs[ind:ind+2],byteorder='big',signed=True)/16384.
ind += 2
accz = int.from_bytes(data_decobs[ind:ind+2],byteorder='big',signed=True)/16384.
ind += 2
T = int(data_decobs[ind:ind+2].hex(),16)
T = T / 333.87 + 21.0
ind += 2
gyrox = int.from_bytes(data_decobs[ind:ind+2],byteorder='big',signed=True)
ind += 2
gyroy = int.from_bytes(data_decobs[ind:ind+2],byteorder='big',signed=True)
ind += 2
gyroz = int.from_bytes(data_decobs[ind:ind+2],byteorder='big',signed=True)
ind += 2
magx = int.from_bytes(data_decobs[ind:ind+2],byteorder='big',signed=True)
ind += 2
magy = int.from_bytes(data_decobs[ind:ind+2],byteorder='big',signed=True)
ind += 2
magz = int.from_bytes(data_decobs[ind:ind+2],byteorder='big',signed=True)
data_packet = {'num':packet_num,'cnt10ks':packet_cnt10ks}
data_packet['type'] = 'A'
data_packet['T'] = T
data_packet['acc'] = [accx,accy,accz]
data_packet['gyro'] = [gyrox,gyroy,gyroz]
data_packet['mag'] = [magx,magy,magz]
data_packets.append(data_packet)
elif(packet_ident == 0xf0): # Pyroscience firesting
#\xf0\x00\x00\x00\x01Y\x00\x00\x08\x8f\x03RMR1 3 0 13 2 153908 -867323 -634011 -305757 -300000 -300000 482 16785 -1 -1 0 -62587\r
if(len(data_decobs) > 12):
ind = 1
packet_num_bin = data_decobs[ind:ind+5]
packet_num = int(packet_num_bin.hex(), 16) # python3
ind += 5
packet_cnt10k_bin = data_decobs[ind:ind+5]
packet_cnt10ks = int(packet_cnt10k_bin.hex(), 16)/device_info['counterfreq']
data_list_O = [packet_num,packet_cnt10ks]
data_utf8 = data_decobs[11:].decode(encoding='utf-8')
if("RMR1 3 0" in data_utf8):
data_pyro = data_utf8.split(' ')
if(len(data_pyro) > 4):
data_stat = float(data_pyro[4])
data_dphi = float(data_pyro[5])
data_umol = float(data_pyro[6])
data_mbar = float(data_pyro[7])
data_asat = float(data_pyro[8])
data_ext_temp = float(data_pyro[9])
data_int_temp = float(data_pyro[10])
data_sgn_int = float(data_pyro[11])
data_amb_light = float(data_pyro[12])
data_press = float(data_pyro[13])
data_hum = float(data_pyro[14])
data_packet = {'num':packet_num,'cnt10ks':packet_cnt10ks}
data_packet['type'] = 'O'
data_packet['phi'] = data_dphi
data_packet['umol'] = data_umol
data_packet['mbar'] = data_mbar
data_packet['asat'] = data_asat
data_packet['ext_temp'] = data_ext_temp
data_packet['int_temp'] = data_int_temp
data_packet['sgn_int'] = data_sgn_int
data_packet['sgn_amb_light'] = data_amb_light
data_packet['sgn_press'] = data_press
data_packet['sgn_hum'] = data_hum
data_packets.append(data_packet)
#except cobs.DecodeError:
# logger.debug(funcname + ': COBS DecodeError')
except Exception as e:
cobs_err_packet += 1
# Peter temporary
logger.debug(funcname + ': Error:' + str(e))
#print('data_cobs:',data_cobs)
#input('Fds')
pass
packet_err = {'type':'format4_log','num_err':err_packet,'num_cobs_err':cobs_err_packet,'num_good':good_packet,'ind_bad':ind_bad}
data_packets.append(packet_err)
return [data_packets,data_str]
print train_data2[0][16]
for i in range(l):
byte_data.extend(train_data2[i].flatten())
return byte_data, names, labels, is_hard
for SCALE in range(1):
f = general.list_images()
images = []
byte_data = []
labels = []
is_hard = []
names = []
for (img, filename, dir_path) in f:
images.append(img)
labels.append(helper.get_label_index(dir_path, conf.ALL_DATA_TYPES))
is_hard.append(isHard(dir_path))
names.append(filename)
byte_data.extend(img.flatten())
byte_data, names, labels, is_hard = enlargeDataset(images, byte_data, names, labels, is_hard)
saveToFile(byte_data, names, labels, is_hard, general.DATASET_FOLDER)
with open(general.DATASET_FOLDER + 'data.byte', "rb") as readdata:
data = cobs.decode(readdata.read())
result = array.array('B', data)
num_of_images = len(result) / (WIDTH * HEIGHT * 3)
result = np.asarray(result, dtype=np.uint8)
result = result.reshape(num_of_images, HEIGHT, WIDTH, 3)
print (images[0])
print ('test')
print (result[0])
def read_basic(path):
with open(path, "rb") as read_data:
data = cobs.decode(read_data.read())
data = array.array('B', data)
data = np.asarray(data, dtype=np.uint8)
return data
print('could not open port')
sys.exit(1)
pass
with open(datetime.datetime.now().strftime("%y-%m-%d_%H-%M-%S") + '.txt', 'w+') as f:
csv_f = csv.writer(f)
csv_f.writerow(csv_caption)
line = ""
try:
while True: # truncate first incomplete packet
if ftread() == '\0':
break
while True:
c = ftread()
if c == '\0': # got packet separator
try:
pkg = unpack_pkg(cobs.decode(line))
print('%8.3f %+8.3f %+8.3f ' % (pkg[1], pkg[2], pkg[3]), end='')
print('%+7.3f %+7.3f %+7.3f ' % (pkg[4], pkg[5], pkg[6]), end='')
print('%4d %4d %4d ' % (pkg[7], pkg[8], pkg[9]), end='')
print('%d %d %d ' % (pkg[10], pkg[11], pkg[12]), end='')
print('%12s' % (pkg[0]))
csv_f.writerow(pkg)
except (cobs.DecodeError, struct.error):
pass
line = ""
else:
line += c
except KeyboardInterrupt:
pass
f.close()
ser.close()
