def packet_received(self, pkt): """Called when a valid packet has been received.""" if pkt.dev_id != self.dev_id() and pkt.dev_id != packet.Id.BROADCAST: # Not a packet for us return if self.show & Bus.SHOW_COMMANDS: log('Rcvd packet for ID: {} Cmd: {}'.format( pkt.dev_id, packet.Command(pkt.cmd))) if self.show & Bus.SHOW_PACKETS: dump_mem(pkt.pkt_bytes, prefix=' R', show_ascii=False, log=log) if pkt.cmd == packet.Command.PING: self.command_ping(pkt) elif pkt.cmd == packet.Command.READ: self.command_read(pkt) elif pkt.cmd == packet.Command.WRITE: self.command_write(pkt) elif pkt.cmd == packet.Command.REG_WRITE: self.command_reg_write(pkt) elif pkt.cmd == packet.Command.ACTION: self.command_action(pkt) elif pkt.cmd == packet.Command.RESET: self.command_reset(pkt) elif pkt.cmd == packet.Command.SYNC_WRITE: self.command_sync_write(pkt) else: self.command_unknown(pkt)
def packet_received(self, pkt): """Called when a valid packet has been received.""" if pkt.dev_id != self.dev_id() and pkt.dev_id != packet.Id.BROADCAST: # Not a packet for us return if self.show & Bus.SHOW_COMMANDS: log('Rcvd packet for ID: {} Cmd: {}'.format(pkt.dev_id, packet.Command(pkt.cmd))) if self.show & Bus.SHOW_PACKETS: dump_mem(pkt.pkt_bytes, prefix=' R', show_ascii=False, log=log) if pkt.cmd == packet.Command.PING: self.command_ping(pkt) elif pkt.cmd == packet.Command.READ: self.command_read(pkt) elif pkt.cmd == packet.Command.WRITE: self.command_write(pkt) elif pkt.cmd == packet.Command.REG_WRITE: self.command_reg_write(pkt) elif pkt.cmd == packet.Command.ACTION: self.command_action(pkt) elif pkt.cmd == packet.Command.RESET: self.command_reset(pkt) elif pkt.cmd == packet.Command.SYNC_WRITE: self.command_sync_write(pkt) else: self.command_unknown(pkt)
def read_status_packet(self): """Reads a status packet and returns it. Rasises a bioloid.bus.BusError if an error occurs. """ pkt = packet.Packet() while True: start = pyb.micros() byte = self.serial_port.read_byte() if byte is None: raise BusError(packet.ErrorCode.TIMEOUT) err = pkt.process_byte(byte) if err != packet.ErrorCode.NOT_DONE: break if err != packet.ErrorCode.NONE: raise BusError(err) if self.show & Bus.SHOW_COMMANDS: log('Rcvd Status: {}'.format(packet.ErrorCode(err))) if self.show & Bus.SHOW_PACKETS: dump_mem(pkt.pkt_bytes, prefix=' R', show_ascii=True, log=log) err = pkt.error_code() if err != packet.ErrorCode.NONE: raise BusError(err) return pkt
def send(self, obj): """Converts a python object into its json representation and then sends it using the 'serial_port' passed in the constructor. """ j_str = json.dumps(obj).encode('ascii') j_len = len(j_str) j_lrc = lrc(j_str) hdr = bytearray((SOH, j_len & 0xff, j_len >> 8, STX)) ftr = bytearray((ETX, j_lrc, EOT)) if self.show_packets: data = hdr + j_str + ftr dump_mem(data, 'Send') self.serial_port.write(hdr) self.serial_port.write(j_str) self.serial_port.write(ftr)
def write_status_packet(self, status, params=None): """Allocates and fills a packet. param should be a bytearray of data to include in the packet, or None if no data should be included. """ packet_len = 6 if not params is None: packet_len += len(params) pkt_bytes = bytearray(packet_len) pkt_bytes[0] = 0xff pkt_bytes[1] = 0xff pkt_bytes[2] = self.dev_id() pkt_bytes[3] = 2 # for len and status pkt_bytes[4] = self.status | status if not params is None: pkt_bytes[3] += len(params) pkt_bytes[5:packet_len - 1] = params pkt_bytes[-1] = ~sum(pkt_bytes[2:-1]) & 0xff if self.show & Bus.SHOW_PACKETS: dump_mem(pkt_bytes, prefix=' W', show_ascii=False, log=log) self.dev_port.write_packet(pkt_bytes)
def fill_and_write_packet(self, dev_id, cmd, data=None): """Allocates and fills a packet. data should be a bytearray of data to include in the packet, or None if no data should be included. """ packet_len = 6 if not data is None: packet_len += len(data) pkt_bytes = bytearray(packet_len) pkt_bytes[0] = 0xff pkt_bytes[1] = 0xff pkt_bytes[2] = dev_id pkt_bytes[3] = 2 # for len and cmd pkt_bytes[4] = cmd if not data is None: pkt_bytes[3] += len(data) pkt_bytes[5:packet_len - 1] = data pkt_bytes[-1] = ~sum(pkt_bytes[2:-1]) & 0xff if self.show & Bus.SHOW_PACKETS: dump_mem(pkt_bytes, prefix=' W', show_ascii=True, log=log) self.serial_port.write_packet(pkt_bytes)
def send(self, obj): """Convert a Python object into its string representation and then send it using the 'serial_port' passed in the constructor. Parameters ---------- obj : list, string or int object that is send via 'serial_port' """ data_type = type(obj) payload_str = None payload_len = None payload_lrc = None if data_type is list: payload_str = tsv.dumps(obj).encode('ascii') payload_len = len(payload_str) payload_lrc = lrc(payload_str) #longitudinal redundancy check hdr = bytearray( (SOH, payload_len & 0xff, payload_len >> 8, SEQ, STX) ) #header & 0xff masks the lower eight bits(FF is 255) >>8 means shift to the right by 8 bits elif data_type is str: payload_str = obj.encode('ascii') payload_len = len(payload_str) payload_lrc = lrc(payload_str) hdr = bytearray( (SOH, payload_len & 0xff, payload_len >> 8, MSG, STX)) elif data_type is int: payload_str = str(obj).encode('ascii') payload_len = len(payload_str) payload_lrc = lrc(payload_str) hdr = bytearray( (SOH, payload_len & 0xff, payload_len >> 8, INS, STX)) else: raise TypeError("Type cannot be send using Packet.") ftr = bytearray((ETX, payload_lrc, EOT)) #footer if self.show_packets: data = hdr + payload_str + ftr dump_mem(data, 'Send') self.serial_port.write(hdr) self.serial_port.write(payload_str) self.serial_port.write(ftr)
log('Control-C - Quit') log('s - Scan Bus') log('d - Debug - Show Packets') show = Bus.SHOW_NONE while True: heartbeat.process() activity.process() byte = host_uart.read_byte() if byte is not None: rc = pkt.process_byte(byte) if rc != packet.ErrorCode.NOT_DONE: activity.kick() device_uart.write_packet(pkt.pkt_bytes) if show & Bus.SHOW_PACKETS: dump_mem(pkt.pkt_bytes, prefix=' H->D', show_ascii=False, log=log) if device_uart.any(): byte = device_uart.read_byte() rc = rsp.process_byte(byte) if rc != packet.ErrorCode.NOT_DONE: activity.kick() host_uart.write_packet(rsp.pkt_bytes) if show & Bus.SHOW_PACKETS: dump_mem(rsp.pkt_bytes, prefix=' D->H', show_ascii=False, log=log) if repl_uart.any():
heartbeat = HeartBeat(RED_LED) activity = Activity(GREEN_LED) while True: heartbeat.process() activity.process() byte = host_uart.read_byte() if byte is not None: if dev.process_byte(byte) == packet.ErrorCode.NONE: # We've received a packet. See if it should be forwarded along pkt = dev.pkt if pkt.dev_id != dev.dev_id() or pkt.cmd == packet.Command.SYNC_WRITE: activity.kick() device_uart.write_packet(pkt.pkt_bytes) if show & Bus.SHOW_PACKETS: dump_mem(pkt.pkt_bytes, prefix=' H->D', show_ascii=False, log=log) if device_uart.any(): byte = device_uart.read_byte() if rsp.process_byte(byte) == packet.ErrorCode.NONE: activity.kick() host_uart.write_packet(rsp.pkt_bytes) if show & Bus.SHOW_PACKETS: dump_mem(rsp.pkt_bytes, prefix=' D->H', show_ascii=False, log=log) if repl_uart.any(): byte = repl_uart.readchar() if byte == 3: # Control-C log('Control-C') raise KeyboardInterrupt if byte == ord('d'):
#!/usr/bin/python3 from dump_mem import dump_mem PREFIX = ' Prefix' print('Empty Buffer') dump_mem(b'', prefix=PREFIX) print print('Less than line') dump_mem(b'0123', prefix=PREFIX) print print('Exactly one line') dump_mem(b'0123456789ABCDEF', prefix=PREFIX) print print('A bit more than a line') dump_mem(b'0123456789ABCDEFGHI', prefix=PREFIX) print print('Set a prefix') dump_mem(b'0123', prefix=' Something') print print('Set an address and a line_width') dump_mem(b'0123456789ABCDEFGHI', address=0x2000, line_width=8, prefix=PREFIX) DATA = bytearray([0x30, 0x31, 0x32, 0x33, 0, 0x80, 0xFF]) print
#!/usr/bin/python3 from dump_mem import dump_mem PREFIX = ' Prefix' print('Empty Buffer') dump_mem(b'', prefix=PREFIX) print print('Less than line') dump_mem(b'0123', prefix=PREFIX) print print('Exactly one line') dump_mem(b'0123456789ABCDEF', prefix=PREFIX) print print('A bit more than a line') dump_mem(b'0123456789ABCDEFGHI', prefix=PREFIX) print print('Set a prefix') dump_mem(b'0123', prefix=' Something') print print('Set an address and a line_width') dump_mem(b'0123456789ABCDEFGHI', address=0x2000, line_width=8, prefix=PREFIX) DATA = bytearray([0x30, 0x31, 0x32, 0x33, 0, 0x80, 0xFF]) print print('Check out some non-printable characters')