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 requestNumBulbs(self, lamp):
# Avoid spamming micro controller
if (time.time() - self.requestTimer > 0.5):
enmess = cobs.encode(b'LID:')+bytes(lamp.getID())
enmess += cobs.encode(b'NB?')+b'\x01'+b'\x00'
self.serialDevice.write(enmess)
pass
self.requestTimer = time.time()
return
async def receive(self) -> bytes:
"""Receive data from the serial connection."""
if not self._open:
raise BrokenPipeError('Cannot read on a broken serial connection!')
await trio.sleep(0.5)
read: bytes = (cobs.encode(b'new serial rx!') + b'\0' +
cobs.encode(b'another new serial rx!') + b'\0')
if read:
self._logger.debug('Read: %s', read)
return read
def write_msg(ser, msg_type, data=None, debug=False):
enc_msg = ''
if data:
data_mut = bytearray(data)
data_mut.insert(0, msg_type)
enc_msg = bytearray(cobs.encode(data_mut))
else:
msg_t_bytes = bytearray([msg_type])
enc_msg = bytearray(cobs.encode(msg_t_bytes))
enc_msg.append(0)
if debug: print(f'Writing encoded message:\n{enc_msg}')
return ser.write(enc_msg)
def ws_msg_rcv(self, client, server, message):
# TODO: Match pinout, which pins are input, which output
if message.startswith('digital'):
pin = int(message.split('digital')[1]) - 16
print(f'toggling pin {pin}')
cmd_p = struct.pack(CommandPacketStruct, *[1, 1, 7 - pin])
enc = cobs.encode(cmd_p)
self.ser.write(enc + b'\0')
else:
delta = int(float(message) * 127)
if abs(delta) > 30:
cmd_p = struct.pack(CommandPacketStruct, *[1, 1, delta])
enc = cobs.encode(cmd_p)
self.ser.write(enc + b'\0')
def random_packet(self):
packet_type = 0
packet_size = struct.calcsize(DataPacketStruct)
packet_crc = 777
packet_id = self.n_packet
packet_us_start = self.n_packet * self.interval * 1000 % (2 ** 32 - 1)
packet_us_end = self.n_packet * self.interval * 1000 % (2 ** 32 - 1)
packet_analog = self.analog()
packet_states = self.states()
packet_digitalIn = int(self.n_packet / 10 % 2 ** 16)
packet_digitalOut = int(self.n_packet / 10 % 2 ** 8)
# print(packet_digitalOut)
packet = []
packet.extend([packet_type, packet_size, packet_crc, packet_id,
packet_us_start, packet_us_end, *packet_analog, *packet_states,
packet_digitalIn, packet_digitalOut])
try:
ps = struct.pack(DataPacketStruct, *packet)
except struct.error as e:
logging.error(e)
print(packet)
return
enc = cobs.encode(ps)
return enc + b'\0'
def proxy_send(self, destination, command, address, payload, singleHopDest=None, cbytes=None):
if(singleHopDest is None): singleHopDest = self._nodeID
if(destination not in range(0, 2**16)): raise Exception("The destination, {0}, must be in the range [0,65535]".format(destination))
if(singleHopDest not in range(0, 2**16)): raise Exception("The single-hop destination, {0}, must be in the range [0,65535]".format(singleHopDest))
self.txData = Protocol.form_packet(type=ProtocolDefs.RAW_PACKET, srcID=self._nodeID, dstID=destination, shSrcID=self._nodeID, shDstID=singleHopDest, cmd=command, addr=address, data=payload, bytes=cbytes, enc='bytes')
if self._logLevel >= 2:
txPacket = Protocol.parse_packet(self.txData)
print("About to send: [header: [{0}], size = {1}, data = {2}]".format(ProtocolDefs.print_structure(txPacket.hdr), txPacket.size, list(i for i in txPacket.data)))
encData = cobs.encode(''.join(self.txData))
encData = list(ord(x) for x in encData)
if self._logLevel >= 3:
print(" Raw: {0}".format(["{:02X}".format(int(x.encode("HEX"), 16)) for x in self.txData]))
print(" Encoded: {0}".format(["{:02X}".format(x) for x in encData]))
if(self.debug == False):
if(self._connected == True):
#write out everything in encData
for b in encData:
self.cmdBuffer.write(b)
self.cmdBuffer.write(0) #needed to mark end of frame
else:
print("Error - need to call connect_sketch first")
def sendPacket(self, packet, silent=False):
if self.service.silenceDict[packet.desc.name]:
silent = True
if packet.desc.name == "Ping":
packet.setField('icd', str(self.service.protocol.crc))
if not silent:
if (packet.token & 0x7FFF) != (self.lastToken & 0x7FFF):
self.print("")
self.print(" --> " + packet.printJSON(self.service.showMeta))
raw = packet.pack()
if self.service.showBytes and not silent:
self.print(" PACK HDR: " + ''.join(' {:02x}'.format(x)
for x in raw[:8]))
self.print(" PACK DATA: " + ''.join(' {:02x}'.format(x)
for x in raw[8:]))
encoded = cobs.encode(bytearray(raw))
encoded += bytes([0])
if hasattr(self, 'coms'):
self.coms.send(encoded)
self.lastToken = packet.token
return encoded
def output(self) -> Optional[bytes]:
"""Produce the next COBS-encoded frame body.
Returns:
An optional bytestring of the COBS-encoded body from the next
available payload in the internal buffer. Returns None if there are
no payloads available in the internal buffer.
Raises:
exceptions.ProtocolDataError: the payload is too long for COBS
encoding.
"""
payload = self._buffer.output()
if payload is None:
return None
if len(payload) > FRAME_PAYLOAD_MAX_SIZE:
raise exceptions.ProtocolDataError(
'Frame payload for COBS encoding is too long ({} bytes)!'
.format(len(payload))
)
try:
encoded: bytes = cobs.encode(payload)
except TypeError as err:
raise exceptions.ProtocolDataError(
'Frames payload for COBS encoding should be bytes not {}'
.format(type(payload))
) from err
self._logger.debug(encoded)
return encoded
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 createPacket(self, data=b''):
packet = bytearray()
# Header (1 byte - added after padding)
# Callsign (6 bytes)
packet += self.callsign
# Length (2 bytes)
dataLen = len(data)
packet += bytes([(dataLen & 0xFF00) >> 8, dataLen & 0xFF])
# Sequence ID (2 bytes)
packet += bytes([(self.sequenceId & 0xFF00) >> 8,
self.sequenceId & 0xFF])
self.sequenceId = (self.sequenceId + 1) & 0xFFFF
# payload (unknown length)
packet += data
# CRC and footer (CRC MSB, CRC LSB) (2 bytes)
crcCalculator = CRC16()
crc = crcCalculator.calculate(bytes(packet))
packet += bytes([(crc & 0xFF00) >> 8, crc & 0xFF])
cobs_packet = cobs.encode(packet)
encoded_packet = self.rs.encode(cobs_packet)
return bytes([0]) + encoded_packet + bytes([0])
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 write_to_file(path, bytes_content):
folder = retrieve_folder(path)
if not os.path.isdir(folder):
os.makedirs(folder)
with open(path, "wb") as compdata:
bytes_content = bytearray(cobs.encode(bytes_content))
compdata.write(bytes_content)
compdata.close()
def encode_packet(device_id: int, packet_id: int, data: Union[bytes,
bytearray]):
""" Encode the packet using the bpl protocol."""
tx_packet = bytes(data)
tx_packet += bytes([packet_id, device_id, len(tx_packet) + 4])
tx_packet += bytes([BPLProtocol.CRC8_FUNC(tx_packet)])
packet: bytes = cobs.encode(tx_packet) + b'\x00'
return packet
def cobs_overhead_measure(num_bytes):
# TODO: review value
NUM_TESTS = 10000
overhead = 0
for _i in xrange(NUM_TESTS):
output = cobs.encode(np.random.bytes(num_bytes))
overhead += (len(output) - num_bytes)
return overhead / float(NUM_TESTS)
def set_status(self, positions, speeds):
msg = b'\x01'
msg += len(positions).to_bytes(1, byteorder='little', signed=True)
for ix in range(len(positions)):
msg += positions[ix].to_bytes(4, byteorder='little', signed=True)
msg += speeds[ix].to_bytes(2, byteorder='little', signed=True)
encoded = cobs.encode(msg) + b'\x00'
self.read = encoded
def setUp(self):
self.testdata = struct.pack('<bbbIffB', 0x1B, 0x1B, 0x02, 1, 1.5, 2.5,
155)
self.crc = crc32(self.testdata)
self.testdata = struct.pack('<bbbIffBbI', 0x1B, 0x1B, 0x02, 1, 1.5, 2.5,
155, 0x03, self.crc)
self.bufferobj = rgslib.MsgBuffer(io.BytesIO())
self.da = b'\x00' + cobs.encode(self.testdata)
def send_packet(self, pkt_type, data):
if self.debug:
print(f'\033[93mSending {len(data)} bytes, packet type {pkt_type.name} ({pkt_type.value})\033[0m')
hexdump(print, data, self.width)
data = bytes([pkt_type.value]) + data
encoded = cobs.encode(data) + b'\0'
self.ser.write(encoded)
self.ser.flushOutput()
def test_Start(self):
self.bufferobj._clearBuffer()
self.bufferobj._source = io.BytesIO(b'\x00' +
cobs.encode(self.testdata) + b'\x00')
factorymock = rgslib.TestMsg()
factorymock.construct = mock.MagicMock()
obj = rgslib.ReceiveLoop(factorymock, self.bufferobj)
self.assertTrue(obj._start_functionality() == self.testdata)
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 test_non_zeros(self):
for length in xrange(1, 1000):
test_string = non_zero_bytes(length)
encoded = cobs.encode(test_string)
expected_encoded = self.simple_encode_non_zeros_only(test_string)
self.assertEqual(
encoded, expected_encoded,
"encoded != expected_encoded for length %d\nencoded: %s\nexpected_encoded: %s"
% (length, repr(encoded), repr(expected_encoded)))
def test_zeros(self):
for length in xrange(520):
test_string = '\x00' * length
encoded = cobs.encode(test_string)
expected_encoded = '\x01' * (length + 1)
self.assertEqual(encoded, expected_encoded,
"encoding zeros failed for length %d" % length)
decoded = cobs.decode(encoded)
self.assertEqual(decoded, test_string,
"decoding zeros failed for length %d" % length)
def _write_packet(self,
packet_type,
packet_format='',
*packet_data,
payload=b''):
packet = struct.pack('<B' + packet_format, packet_type.value, *
packet_data) + payload
#print('> %s' % packet.hex())
packet = cobs.encode(packet) + b'\x00'
self._serial.write(packet)
def requestAlias(self, lamp):
# Avoid spamming micro controller
if (time.time() - self.requestTimer > 0.5):
tmcid = [(self.clientID[0]), (self.clientID[1])]
tmlid = [(lamp.lid[0]), (lamp.lid[1])]
enmess = cobs.encode(b'CID:'+bytearray(tmcid)+b'LID:'+bytearray(tmlid)+b'KN?')+b'\x01'+b'\x00'
self.serialDevice.write(enmess)
pass
self.requestTimer = time.time()
return
def requestNumLamps(self):
# Avoid spamming micro controller
if (time.time() - self.requestTimer > 0.5):
print("Requesting number of lamps on client:" + str(self.clientID))
tms = [(self.clientID[0]), (self.clientID[1])]
enmess = cobs.encode(b'CID:'+bytearray(tms)+b'CNL?')+b'\x01'+b'\x00'
self.serialDevice.write(enmess)
pass
self.requestTimer = time.time()
return
def make_link_send_data_unit(self, data_event):
"""Override DataUnitLink.make_link_send_data_unit."""
frame_payload = data_event.data
if len(frame_payload) > self.PAYLOAD_SIZE_LIMIT:
self.sender_counter['error_payload_length'] += 1
raise ValueError(
'Cannot send an excessively long payload: {} bytes given, '
'{} bytes allowed'.format(len(frame_payload),
self.PAYLOAD_SIZE_LIMIT))
return cobs.encode(frame_payload)
def requestAllParameters(self, lamp):
# Avoid spamming micro controller
if (time.time() - self.requestTimer > 0.5):
tms = [(self.clientID[0]), (self.clientID[1])]
tml = [(lamp.lid[0]), (lamp.lid[1])]
enmess = cobs.encode(b'CID:'+bytes(tms)+b'LID:'+bytes(tml)+b'PAR?')+b'\x01'+b'\x00'
self.serialDevice.write(enmess)
pass
self.requestTimer = time.time()
return
def ilda_transfer(port, file):
# Read file and encode to COBS
ilda_data = file.read()
cobs_ilda_data = cobs.encode(bytearray(ilda_data)) + b'\x00'
print('Encoded lenght: {}\n'.format(len(cobs_ilda_data)))
s = serial.Serial(port, 115200, timeout=5) # Timeout 1 second for reads
s.write(cobs.encode(bytearray('RTS')) +
b'\x00') # write request to send to STM
ready = cobs.decode(s.read_until(expected='\x00'))
if ready == b'CTS':
print('Transfering ILDA file...\n')
s.write(cobs_ilda_data)
print('Transfer complete!\n')
def to_bytes(self):
frm_type = uint16_t.pack(self.type)
frm_len = uint16_t.pack(len(self.data))
raw = frm_type + frm_len + bytes(self.data)
raw += uint32_t.pack(binascii.crc32(raw))
pkt = cobs.encode(raw) + b"\0"
return pkt
def _stm32_pack_cmd(payload, cmd=STM32_CMD_MOVEMENT_COMMAND, robot_idx=STM32_ADDR_BROADCAST):
header = _stm32_generate_header(cmd, robot_idx)
packet = header
if payload:
packet += payload
checksum = bytes([sum(packet) & 0xff])
packet = packet[:4] + checksum + packet[5:]
return cobs.encode(bytes(packet)) + b'\0'
def make_recipe(recipe):
print("Send make request")
machine = "http://192.168.178.111"
raw_bytes = recipe.SerializeToString()
raw_bytes = cobs.encode(raw_bytes)
print(raw_bytes)
answer = requests.post(machine + "/make",
data={'proto': raw_bytes + b'\x00'})
print(answer)
print(answer.content)
return answer
def encode_packet(device_id, packet_id, data):
"""
Encode the packet using the bpl protocol.
:param device_id: Device ID
:param packet_id: Packet ID
:param data: Data in bytes
:return: bytes of the encoded packet.
"""
tx_packet = bytearray(data)
tx_packet += bytearray([packet_id, device_id, len(tx_packet)+4])
tx_packet += bytearray([CRC8_FUNC(bytes(tx_packet))])
packet = bytearray(cobs.encode(bytes(tx_packet))) + bytearray([0])
return packet
def encode_packet(device_id: int, packet_id: int, data: Union[bytes, bytearray]):
"""
Encode the packet using the bpl protocol.
:param device_id: Device ID
:param packet_id: Packet ID
:param data: Data in bytes
:return: bytes of the encoded packet.
"""
tx_packet = bytes(data)
tx_packet += bytes([packet_id, device_id, len(tx_packet)+4])
tx_packet += bytes([BPLProtocol.CRC8_FUNC(tx_packet)])
packet: bytes = cobs.encode(tx_packet) + b'\x00'
return packet
def send(self, message=None, cobs_encode=True):
self.heartbeat_timer = timer() # Reset heartbeat timer
if self.active_port is None: #If driver is disconnected, then don't try to send packet
return
if cobs_encode:
packet = bytearray(self.prefix_dict[inspect.stack(
)[2].function].to_bytes(
1,
"big")) #Add routing prefix based on name of calling function
if message:
if isinstance(message, str):
message = bytearray(message.encode())
elif isinstance(message, int):
message = message.to_bytes(1, "big")
packet.extend(bytearray(message))
self.gui.splashText("Func: " + str(inspect.stack()[2].function) +
", Tx: " + str(packet))
if debug:
print("Func: " + str(inspect.stack()[2].function) + ", Tx: " +
str(packet))
self.active_port.write(cobs.encode(bytes(packet)))
self.active_port.write(bytes(1)) # Send NULL framing byte
else:
self.gui.splashText("Func: " + str(inspect.stack()[2].function) +
", Tx: " + str(message))
if debug:
print("Func: " + str(inspect.stack()[2].function) + ", Tx: " +
str(message[:100]))
if len(message) > 100:
print("↑ Total tx packet length: " + str(len(message)))
bytes_written = self.active_port.write(message)
if bytes_written != len(message):
self.showMessage(
"Error: Only " + str(bytes_written) + " of " +
str(len(message)) +
" were sent to LED driver. Please check connection.")
wait_time = 200
if message:
wait_time += round(
len(message) / 10
) #adjust the wait time according to the size of the packet to be transmitted
if self.active_port.waitForBytesWritten(
wait_time): #Wait for data to be sent
pass
else:
if not self.initializing_connection:
self.showMessage(
"Error: Message buffer failed to be sent to driver, please check driver connection."
)
try:
ser = serial.Serial(opt.serial, 57600, timeout=0)
except Exception, e:
raise ScriptError(str(e))
else:
logger.info("using no serial, just printing stuff")
recorder = Recorder(opt.source, sample_rate, opt.list_sources)
t0 = int(time())
n = 0
for chunk in recorder:
matrix = calculate_levels(chunk, chunk_size, sample_rate)
if not opt.no_serial:
matrix = (10 * matrix ** 2).clip(max=255).astype('uint8')
packet = cobs.encode(matrix.tostring()) + '\0'
ser.write(packet)
else:
print "matrix:", matrix
t1 = int(time())
if t1 > t0:
logger.debug("chunks recorded: %s", n)
n = 0
t0 = t1
n += 1
if not opt.no_serial:
read_from_serial(ser)
def send(self, data): self.init() enc = cobs.encode(bytes(data)) + chr(0) return self.ser.write(enc)
def write_packet(packet):
data = cobs.encode(bytearray(packet))
ser.write(data + b"\0")
def encode_colours(cols):
return cobs.encode(b''.join(bytes(c) for c in cols)) + b'\0'
