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'