def send(message):
ser = Serial("COM3", baudrate=57600)
ser.flush()
ser.write(message.encode())
sleep(0.1)
if ser.inWaiting():
print(ser.readline().decode().rstrip('\r\n').split(",")[0])
class SerialPortBackEnd(BackEnd): def __init__(self, port = None, baudrate = 115200, read_timeout = 1.0, write_timeout = 0.1): """ Provides a serial-port back-end for the protocol. Timeouts are in seconds. """ BackEnd.__init__(self) self.name = "Serial" # Start the emulator as a child-process self.serial = Serial(port, baudrate = baudrate, timeout = read_timeout, writeTimeout = write_timeout) def read(self, length): return self.serial.read(length) def write(self, data): self.serial.write(data) def flush(self): self.serial.flush() def close(self): self.serial.close()
class SerialServer(Thread): def __init__(self, port="/dev/ttyACM0", baudrate=9600, timetoread=2): Thread.__init__(self) self.server = Serial(port=port, baudrate=baudrate) self.dataInQueeud = False self.running = True self.timetoread = timetoread #periodicite de lecture du port self.datas = "" #les donnees lus self.dataToSend = "" #les donneees à envoyer def run(self): """lecture continue du port série. Lorsqu'il y'a quelque chose à lire, il lie puis envoie les donnees""" while self.running == True: while self.server.inWaiting() > 0: self.datas += self.server.readline() sleep(self.timetoread) #pause #envoie s'il y'a quelques a envoyer if self.dataToSend != "": self.server.write(self.dataToSend) self.server.flush() def recvdatas(self): res = self.datas self.datas = "" return res def senddatas(self, data = ""): self.dataToSend = data def toStop(self): self.server.close() self.running = False
class Arduino:
def __init__(self, serialport):
"""Takes the string of the serial port
and connects to that port, binding the
Arduino object to it.
"""
print('Instanciado Arduino dispositivo %s' % serialport)
try:
self.serialport = Serial(serialport, 9600)
self.error = False
self.id = None
except:
self.error = True
self.id = -666
raise
def read_byte(self, block=False):
"""Lee un byte.
Si block=True lee con espera activa hasta
que exista algún byte que leer, porque no
funciona sin espera activa.
"""
if block == False and self.serialport.inWaiting() < 1:
return None
return self.serialport.read(1);
def read_until(self, until):
buffer = []
while buffer[-1] != until:
if self.serialport.inWaiting() < 1:
return buffer
buffer += self.serialport.read(1);
return buffer
def write(self, str):
return self.serialport.write(str)
def write_byte(self, byte):
return self.serialport.write(chr(byte))
def get_id(self):
if self.error:
return self.id
if self.id != None:
return self.id
# Consume all bytes for this query
self.serialport.flushInput()
print 'Lendo o ID..'
while self.id == None:
self.write_byte(QUERY_IDENT)
self.id = self.serialport.read(1)
self.id = ord(self.id)
self.serialport.flush()
self.serialport.flushInput()
return self.id
class SrfStickCommunication:
def __init__(self, usb_port_number):
port = '/dev/ttyACM{}'.format(usb_port_number)
baudrate = 115200
timeout = 1
print("opening connection...")
self.connection = Serial(port=port, baudrate=baudrate, timeout=timeout)
self.flush_input_buffer()
self.connection.flush()
def __enter__(self):
return self
def __exit__(self, type, value, traceback):
self.close()
def close(self):
print("closing connection...")
self.connection.close()
def flush_input_buffer(self):
timeout = False
while not timeout:
response = self.connection.read(1)
if response == '':
timeout = True
def set_frequency(self, frequency):
if not self.send_srf_commands(["+++", "ATCN{}\r".format(frequency), "ATAC\r", "ATDN\r"]):
sys.stderr.write("ERROR: Failed to set frequency.\n")
self.close()
def send_srf_commands(self, commands):
for command in commands:
print(command)
if not self.send_srf_command(command):
return False
return True
def send_srf_command(self, command):
self.connection.write(command)
self.connection.flush()
response = self.connection.read(2)
if not response:
sleep(3)
response = self.connection.read(2)
self.connection.reset_input_buffer()
if response == "OK":
return True
else:
if command[-1] == '\r':
command = command[:-1]
sys.stderr.write("ERROR: command '{}' returned '{}'.\n".format(command, response))
return False
class LightStrip(object):
def __init__(self, port='/dev/ttyUSB0'):
self.ser = Serial(port=port, baudrate=115200, timeout=1)
time.sleep(0) # wait for a arduino reset to pass
self.ser.flush()
def send(self, rgbs):
assert len(rgbs) == 15, rgbs
packet = "\x60" + ''.join(chr(int(r))+chr(int(g))+chr(int(b)) for r,g,b in rgbs)
self.ser.write(packet)
class EEPROM():
def __init__(self, rom_size=8192):
self.RECSIZE = 16
self.port = None
self.rom_size = rom_size
def open_port(self, tty_port="/dev/tty.usbserial-1420"):
if isinstance(tty_port, str):
try:
self.port = Serial(tty_port, timeout=0.1, dsrdtr=True)
except SerialException as err:
raise EEPROMException(err)
else:
self.port = tty_port
def __del__(self):
self.close()
def close(self):
if self.port:
self.port.close()
def read(self, addr):
cmd = str.encode("R" + address_field(addr) + chr(10))
self.send_cmd(cmd)
response = self.port.readline().upper()
self.wait_okay()
return response
def write(self, addr, data):
cmd = str.encode("W" + address_field(addr) + ":" + data_field(data) +
chr(10))
self.send_cmd(cmd)
self.wait_okay()
return cmd
def wait_okay(self):
retries = 0
resp = self.port.readline()
while resp != OK:
print("RESP:", resp)
retries += 1
if retries > 5:
self.port.close()
sys.exit("Didn't receive OK back from programmer.\n")
resp = self.port.readline()
def version(self):
cmd = str.encode("V" + chr(10))
self.send_cmd(cmd)
response = self.port.readline().upper()
return response.decode('UTF-8')
def send_cmd(self, cmd):
self.port.write(cmd)
self.port.flush()
class Iridium(Device):
PORT = '/dev/serial0'
BAUDRATE = 19200
def __init__(self):
super().__init__('Iridium')
self.serial: Serial = None
def flush(self):
"""
Clears the serial buffer
:return: (None)
"""
self.serial.flushInput()
self.serial.flushOutput()
def functional(self) -> bool:
"""
Checks the state of the serial port (initializing it if needed)
:return: (bool) serial connection is working
"""
if self.serial is None:
try:
self.serial = Serial(port=self.PORT, baudrate=self.BAUDRATE, timeout=1)
self.serial.flush()
return True
except:
return False
if self.serial.is_open:
return True
try:
self.serial.open()
self.serial.flush()
return True
except:
return False
def write(self, command: str) -> bool:
"""
Write a command to the serial port.
:param command: (str) Command to write
:return: (bool) if the serial write worked
"""
if not self.functional():
return False
command = command + "\r\n"
try:
self.serial.write(command.encode("UTF-8"))
except:
return False
return True
class LightStrip(object):
def __init__(self, port='/dev/ttyUSB0'):
self.ser = Serial(port=port, baudrate=115200, timeout=1)
time.sleep(0) # wait for a arduino reset to pass
self.ser.flush()
def send(self, rgbs):
assert len(rgbs) == 15, rgbs
packet = "\x60" + ''.join(
chr(int(r)) + chr(int(g)) + chr(int(b)) for r, g, b in rgbs)
self.ser.write(packet)
def sendParam(serialConnection: serial.Serial, ba):
"""
send params to arduino
"""
# TODO try
serialConnection.write(ba)
serialConnection.flush()
# TODO timeout
line = serialConnection.readline()
return decodeStepperLine(line)
def handle(self, *args, **options):
conf = options['config']
config = Configurator().set_watch(False).read(conf)
serial_com = config.get_kv("serial/device")
serial_speed = config.get_kv("serial/speed", 9600)
ser = Serial(serial_com, serial_speed)
ser.write(bytes('1/11/1/', 'utf-8'))
ser.flush()
time.sleep(1)
ser.close()
class ServoController(object):
def __init__(self):
rospy.init_node('servo_controller')
try:
self.__serial = Serial('/dev/ttyACM0', 9600)
rospy.sleep(1)
#for i in range(32):
# self.__move(i, 90)
#rospy.sleep(1)
except SerialException as e:
rospy.logfatal("Could not open serial port.")
exit(1)
rospy.Subscriber('direct', ServoCommand, self.move_callback)
rospy.spin()
################################################################################################
def move_callback(self, data):
if data.angle < 0 or data.angle > 180:
rospy.logerr('Tried to set servo #' + str(data.index) +
' out of bounds (' + str(data.angle) + ' degrees).')
return
self.__move(data.index, data.angle, data.duration)
################################################################################################
def __move(self, index, angle, duration=0.1):
move_string = ServoController.__move_string(index, angle, duration)
self.__serial.write(move_string)
self.__serial.flush()
rospy.sleep(0.003)
################################################################################################
@staticmethod
def __move_string(index, angle, duration):
if duration < 0.1:
duration = 0.1
pulse_duration = int(((angle / 180.0) * (2500 - 500))) + 500
move_time = int(duration * 1000)
return '#' + str(index + 1) + 'P' + str(pulse_duration) + 'T' + str(
move_time) + '\r\n'
class SerialDriver(Driver):
"""
An implementation of a serial ANT+ device driver
"""
def __init__(self,
device: str,
baudRate: int = 115200,
logger: Logger = None):
super().__init__(logger=logger)
self._device = device
self._baudRate = baudRate
self._serial = None
def __str__(self):
if self.isOpen():
return self._device + " @ " + str(self._baudRate)
return None
def _isOpen(self) -> bool:
return self._serial is not None
def _open(self) -> None:
try:
self._serial = Serial(port=self._device, baudrate=self._baudRate)
except SerialException as e:
raise DriverException(str(e))
if not self._serial.isOpen():
raise DriverException("Could not open specified device")
def _close(self) -> None:
self._serial.close()
self._serial = None
def _read(self, count: int, timeout=None) -> bytes:
return self._serial.read(count)
def _write(self, data: bytes) -> None:
try:
self._serial.write(data)
self._serial.flush()
except SerialTimeoutException as e:
raise DriverException(str(e))
def _abort(self) -> None:
if self._serial is not None:
self._serial.cancel_read()
self._serial.cancel_write()
self._serial.reset_input_buffer()
self._serial.reset_output_buffer()
class ServoController(object):
def __init__(self):
rospy.init_node('servo_controller')
try:
self.__serial = Serial('/dev/ttyACM0', 9600)
rospy.sleep(1)
#for i in range(32):
# self.__move(i, 90)
#rospy.sleep(1)
except SerialException as e:
rospy.logfatal("Could not open serial port.")
exit(1)
rospy.Subscriber('direct', ServoCommand, self.move_callback)
rospy.spin()
################################################################################################
def move_callback(self, data):
if data.angle < 0 or data.angle > 180:
rospy.logerr('Tried to set servo #' + str(data.index) + ' out of bounds (' +
str(data.angle) + ' degrees).')
return
self.__move(data.index, data.angle, data.duration)
################################################################################################
def __move(self, index, angle, duration = 0.1):
move_string = ServoController.__move_string(index, angle, duration)
self.__serial.write(move_string)
self.__serial.flush()
rospy.sleep(0.003)
################################################################################################
@staticmethod
def __move_string(index, angle, duration):
if duration < 0.1:
duration = 0.1
pulse_duration = int(((angle / 180.0) * (2500 - 500))) + 500
move_time = int(duration * 1000)
return '#' + str(index + 1) + 'P' + str(pulse_duration) + 'T' + str(move_time) + '\r\n'
class MeerstetterTEC_USB(MeerstetterTEC):
"""
Discrete implementation of the MeerstetterTEC class on an USB based interface
"""
def __init__(self,
port,
timeout=1,
baudrate=57600,
tec_address=0,
check_crc=True):
super().__init__(tec_address=tec_address, check_crc=check_crc)
self.port = port
self.timeout = timeout
self.baudrate = baudrate
self.ser = Serial(
port=self.port,
timeout=self.timeout,
baudrate=self.baudrate,
)
def __exit__(self, exc_type, exc_val, exc_tb):
self.ser.__exit__(exc_type, exc_val, exc_tb)
def __enter__(self):
return self
def stop(self):
self.ser.flush()
self.ser.close()
def _sendAndReceive(self, frame):
byte_arr = frame + b'\r'
#clear all remaining buffers
self.ser.reset_output_buffer()
self.ser.reset_input_buffer()
#send and flush
self.ser.write(byte_arr)
self.ser.flush()
#read all in
answer_arr = self.ser.read_until(terminator=b'\r')
return answer_arr[0:-1]
class SerialTransport(Transport):
"""
This transports the messages over a Serial Port.
"""
def __init__(self):
Transport.__init__(self)
self.port = None
self.baudrate = None
def _open(self):
self.serial = Serial(**self.settings)
def read(self):
return self.serial.read(size=1)
def flush(self):
self.serial.flush()
class TraceTerminal(object):
serial = None # Serial() object
port = None
baudrate = None
echo = None
def __init__(self, port, baudrate=9600, echo=True, timeout=10):
self.port = port
self.baudrate = int(baudrate)
self.timeout = int(timeout)
self.echo = False if str(echo).lower() == 'off' else True
try:
from serial import Serial, SerialException
self.serial = Serial(self.port,
baudrate=self.baudrate,
timeout=self.timeout)
self.serial.flush()
self.serial.reset_input_buffer()
except (IOError, ImportError, OSError, Exception):
self.serial = None
return
def terminal(self):
try:
import serial.tools.miniterm as miniterm
except (IOError, ImportError, OSError) as e:
print(repr(e))
return False
term = miniterm.Miniterm(self.serial, echo=self.echo)
term.exit_character = '\x03'
term.menu_character = '\x14'
term.set_rx_encoding('UTF-8')
term.set_tx_encoding('UTF-8')
term.rx_transformations += [CalliopeDebugTransform()]
term.start()
try:
term.join(True)
except KeyboardInterrupt:
pass
term.join()
term.close()
def open_upy(port, baud):
log.debug("Entering")
log.info("Opening port and resetting ESP into run mode.")
upy = Serial(port, baud)
upy.reset_input_buffer()
upy.rts=1
upy.dtr=0
upy.rts=0
log.info("Sending ^c after 500ms.")
sleep(0.5)
upy.write(b'\x03')
upy.flush()
log.debug(upy.read_until(b'>>> ').decode(errors='ignore'))
sleep(0.01)
if upy.in_waiting:
log.debug(upy.read_until(b'>>> ').decode(errors='ignore'))
return upy
class SerialTransport(Transport):
"""
This transports the messages over a Serial Port.
"""
def __init__(self):
Transport.__init__(self)
self.port = None
self.baudrate = None
def _open(self):
self.serial = Serial(**self.settings)
def read(self):
return self.serial.read(size=1)
def flush(self):
self.serial.flush()
class TagReader:
def __init__(self, port):
self.serial_port = Serial(port, baudrate=9600, timeout=0.01)
self.serial_port.close()
self.serial_port.open()
self.serial_port.flush() # Flush any old data
self.should_do_checksum = True
def getBufferSize(self):
return self.serial_port.inWaiting()
"""
RFID Tag is 16 characters: STX(02h) DATA (10 ASCII) CHECK SUM (2 ASCII) CR LF ETX(03h)
1 char of junk, 10 of hexadecimal data, 2 of hexadecimal check sum, 3 of junk
XOR-ing 5 1-byte hex values will give the 1-byte check sum - if requested
"""
def readTag(self):
# run the while loop since it may read a tag twice.
# May not be necessary
while (self.getBufferSize() >= 16):
junk = self.serial_port.read(1) # Junk byte 1
tag = self.serial_port.read(10) # tag bytes 10
check_sum = self.serial_port.read(2) # check sum 2 bytes
junk = self.serial_port.read(3) # Last 3 bytes are junk
if (self.should_do_checksum):
self.doCheckSum(tag, check_sum)
return int(tag, base=16) # Convert the tag to an integer.
def doCheckSum(self, tag, check_sum):
try:
checked_val = 0
for i in range(0, 5):
checked_val = checked_val ^ int(tag[(2 * i):(2 * (i + 1))], 16)
if checked_val != int(check_sum, 16):
print("Tag reader checksum error. Tag was not read fully...")
except ValueError:
print("Error reading the tag properly.")
print("Tag: " + tag)
def close(self):
self.serial_port.close()
class MockControll:
def openSerial(self, delay=0.5):
i = 0
while True:
try:
self.serial = Serial('/dev/ttyACM' + str(i), 512000)
print("Opened port /dev/ttyACM" + str(i) + " successfully!")
break
except:
self.serial = None
print("Couldn't open port /dev/ttyACM" + str(i) + "!")
time.sleep(delay)
i += 1
if (i >= 10): i = 0
def waitSign(self):
print("Esperando sinal...")
self.serial.flush()
c = b''
while c != b'\x40':
c = self.serial.read()
print(c)
print("Got sign...")
def sendGoal(self):
print("Sending goal...\n")
data = [
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19,
20
]
cs = reduce(
lambda x, y: x ^ y,
struct.unpack('>42b', struct.pack('>2b20h', *([3, 45] + data))))
header = [-3599, 3, 45, cs]
msg = struct.pack('>h3b20h', *(header + data))
self.serial.write(msg)
def readResponse(self):
print("Waiting for response...")
c = self.serial.read()
while c != b'\x24':
stdout.write(c.decode())
c = self.serial.read()
stdout.write('\n\n')
class PlasmaSerial(Plasma):
def __init__(self, light_count, port='/dev/ttyAMA0', baudrate=115200):
self._serial_port = port
self._serial = Serial(port, baudrate=baudrate)
Plasma.__init__(self, light_count)
def show(self):
"""Display current buffer on LEDs."""
sof = b"LEDS"
eof = b"\x00\x00\x00\xff"
pixels = []
for pixel in self._pixels:
for channel in pixel:
pixels += [channel]
self._serial.write(sof + bytearray(pixels) + eof)
self._serial.flush()
def __init__(self):
self.serial = None
for port in comports():
try:
serial = Serial(port[0], baudrate=BAUD_RATE, timeout=2)
serial.write("V\n")
result = serial.readline()
if "SERVOTOR" in result:
log.info("Connected on port: %s" % port[0])
serial.flush()
self.serial = serial
break
except SerialException:
log.warning("Could not connect on port: %s" % port[0])
if not self.serial:
raise ConnectionException
def initialize_com(baud):
active_ports = comports()
# if there is only one com port, use that one
if (len(active_ports) == 1):
port_id = active_ports[0].name
# if there are multiple com ports, ask user which to use
else:
print('Active ports: ')
for port in active_ports:
print(' - ' + port.name)
port_id = input('What port would you like to use? ')
print('Using port: ' + port_id)
ser = Serial(port_id, baud, timeout=1)
ser.flush()
return ser
class ArduinoCommunication:
def __init__(self, device, baud):
self.device = device
self.baud_rate = baud
self.serial_comm = Serial(device, baud)
def send(self, payload):
bytes_payload = []
for fl in payload:
bytes_payload += struct.pack('f', fl)
print(bytes_payload)
print(payload)
self.serial_comm.write(bytes_payload)
self.serial_comm.flush()
def receive(self):
return self.serial_comm.read()
def main():
parser = argparse.ArgumentParser(description='Send a byte over UART')
parser.add_argument('-r',
'--rx',
dest='rx',
action='store_true',
default=False,
help='Wait for RX data and print it back')
parser.add_argument('val',
type=str,
help='Byte to send, in hexadecimal format')
args = parser.parse_args()
rx = args.rx
val = int(args.val, 16)
if not 0 <= val <= 255:
raise ValueError('Value is out of range: min 0x00 max 0xFF')
data = bytes([val])
ser = Serial('/dev/cu.usbserial-FT94JP1H', baudrate=57600, timeout=1)
print(ser.write(data))
ser.flush()
if rx is not True:
ser.close()
return
data = []
while True:
try:
data.append(ord(ser.read(size=1)))
except TypeError:
break
print(data)
ser.close()
return
class SRFThread:
def __init__(self, port):
# Serial connection to Arduino
self.port = port
print("Connecting to arduino...")
self.arduinoSerial = Serial(port=self.port,
baudrate=ARDU_BAUD,
timeout=1)
sleep(3)
print("Arduino connected")
self.prevDistance = 0.01
# SRF Threading
self.qSRF = Queue()
tSRF = Thread(target=self._distance)
tSRF.daemon = True
tSRF.start()
def _distance(self):
while True:
self.arduinoSerial.flush()
self.arduinoSerial.write(b'a')
srf_distance = self.arduinoSerial.readline().decode(
'ascii').strip()
try:
distance = int(srf_distance) / 100.0
if not self.qSRF.empty():
try:
self.qSRF.get_nowait()
except:
pass
self.qSRF.put(distance)
except:
pass
def readSRF(self):
distance = self.qSRF.get()
if distance is None or distance is 0:
return self.prevDistance
else:
self.prevDistance = distance
return distance
def readThread(self):
ser = Serial('COM3')
ser.flush()
i = 0
rgb = 'rgb'
color = [Qt.red, Qt.green, Qt.blue]
while True:
data = ser.read(2).decode('utf-8')
print(data)
if data[0] == '1' and self.me.left() - 10 >= 0:
self.me.moveTo(self.me.left() - 10, self.me.top())
elif data[0] == '2' and self.me.left() + 10 <= 400:
self.me.moveTo(self.me.left() + 10, self.me.top())
self.bul.moveTo(self.bul.left(), self.bul.top() - 30)
if data[1] == '1' and self.bul.top() < 0:
self.bul.setRect(self.me.left() + 50,self.me.top(),10,10)
i += 1
self.color = color[i % 3]
ser.write(rgb[i % 3].encode())
for ship, _ in self.ships:
ship.moveTo(ship.left(), ship.top() + 10)
if ship.contains(self.bul):
self.ships.remove((ship, _))
self.bul.moveTo(-100, -100)
if (ship.contains(self.me.center())):
self.msg = 'You Died!!'
self.update()
return
if (ship.top() > 1000):
ship.moveTo(ship.left(), -randrange(50, 300))
if len(self.ships) == 0:
self.msg = 'You Win!!'
self.update()
return
self.update()
class SerialWorker(QtCore.QObject):
"""
Serial Worker that reads lines from the serial port
"""
readline = QtCore.pyqtSignal(str)
def __init__(self):
super(SerialWorker, self).__init__()
self.active = True
self.serial = None
self.sendNewLine = True
def openSerial(self, port, baudRate, timeout, sendNewLine=True):
""" Opens the serial port """
if self.serial is not None:
raise Exception("Cannot read multiple serial")
self.serial = Serial(port = port, baudrate = baudRate, timeout = timeout)
self.sendNewLine = sendNewLine
def closeSerial(self):
""" Closes the serial port """
if self.serial is not None:
self.serial.close()
self.serial = None
def workerStart(self):
""" Runs in a loop reading from the serial port """
while(self.active):
if self.serial is not None:
try:
if self.sendNewLine:
self.serial.write(b"\n")
self.serial.flush()
line = self.serial.readline()
except Exception as e:
print("Error Reading Serial Port")
self.readline[str].emit(line)
else:
QtCore.QThread.msleep(500)
def workerStop(self):
self.closeSerial()
class Link:
def __init__(self, port="/dev/ttyACM0", baudrate=115200):
self.ser = Serial(port, baudrate, timeout=0.5)
self.ser.flush()
self.data = ""
self.new_data = False
self.start_marker = "<"
self.end_marker = ">"
self.incoming_in_process = False
self.incoming_data = ""
def read(self):
try:
while self.ser.inWaiting() > 0:
data = self.ser.read().decode()
if (data == self.start_marker):
self.incoming_in_process = True
#self.new_data = False
self.incoming_data = ""
elif (data == self.end_marker):
self.incoming_in_process = False
self.data = self.incoming_data
self.incoming_data = ""
self.new_data = True
elif (self.incoming_in_process == True):
self.incoming_data += data
except Exception as e:
print(e)
def get_name(self):
self.ser.write(b"#")
def write(self, msg):
self.ser.write(b"" + self.start_marker.encode())
self.ser.write(b"" + msg.encode())
self.ser.write(b"" + self.end_marker.encode())
def get_data(self):
if self.new_data:
self.new_data = False
return self.data
return ""
def get_port(self):
test_port = None
current_ports = Ports.comports()
for port in current_ports:
if 'USB' in port.device:
try:
test_port = Serial(port.device, 9600)
except Exception as e:
print('opening port failed: {}'.format(e))
continue
test_port.write(b'\r\n\r\n')
test_port.flush()
while test_port.in_waiting > 0:
line = test_port.readline()
if b'Vin: ' in line:
test_port.flush()
return test_port
test_port.close()
raise HydraNotConnectedError(
'Hydra not found. Please connect Hydra and try again.')
def apply(self):
# Init Uart
_LOGGER.info("Execute: %s", self.cmd)
uart = Serial(self.port, self.baudrate, timeout=self.timeout)
# Execute Command
time_start = time.time()
time_escape = time.time() - time_start
uart.write(self.cmd + self.eol)
uart.flush()
rawdata = ""
while time_escape <= self.cmd_timeout:
rawdata += uart.read(self.read_size)
if self.recv != None and len(rawdata) >= self.recv:
break
find_endmark = False
for endmark in self.endmarks:
sret = re.search(endmark, rawdata)
if sret != None:
find_endmark = True
break
if find_endmark == True:
break
time_escape = time.time() - time_start
uart.close()
# Print rawdata
lines = rawdata.split("\n")
for line in lines:
_LOGGER.info("%s", line)
## Check Error
find_error = False
for error_filter in self.error_filters:
try:
error_filter.determine(rawdata)
except ValueError, err:
_LOGGER.error("%s", err)
find_error = True
if find_error == True:
# Raise this error again so that SESCommandItem can catch it.
_LOGGER.info("ERROR: read value with error!")
break
def serial_open(port, baudrate, read_timeout=None, write_timeout=None):
'''Try to open a Serial Port.'''
ser = None
print_log(LOG.INFO,
"Opening Serial port {} at {}bps...".format(port, str(baudrate)))
try:
ser = Serial(port=None)
ser.port = port
ser.baudrate = baudrate
ser.timeout = read_timeout
ser.write_timeout = write_timeout
if not ser.isOpen():
ser.open()
ser.flush()
print_log(LOG.INFO, "Port successfully open.")
except Exception as e:
print_log(LOG.ERROR, str(e))
if (ser is None) or (not ser.isOpen()):
ser = None
print_log(LOG.ERROR, "Can't open serial port.")
return ser
class UartDevice(IODevice):
def __init__(self, port: str = "/dev/ttyUSB0", baudrate: int = 9600):
self.uart = Serial(port, baudrate, xonxoff=False)
def ReadByte(self):
return self.uart.read_all()
def ReadAll(self):
return self.uart.read_all()
def Write(self, arr: bytes):
assert (type(arr) is bytes)
self.uart.write(arr)
def Flush(self):
self.uart.flush()
def SetBaudrate(self, baudrate: int) -> None:
self.uart.baudrate = baudrate
def GetBaudrate(self):
return self.uart.baudrate
class Device():
def __init__(self): #linkage is probably screwing something up
port = find_port()
if port is None:
raise Exception('cannot find device')
self.serial = Serial(port, BAUD_RATE, timeout=TIMEOUT)
# self.flush()
self.pos = 0
# he made his own flush? why? the below just fills the serial buffer with zeroes... maybe
# def flush(self):
# for i in range(10):
# self.move(0, 0, 0)
def readline(self):
return self.serial.readline().decode('utf-8').strip(
) # He doesn't care what it looks for but this function will lock up the program until it reads soemthing (probably not certain)
# called by move - step 3 THIS IS THE MOST IMPORTANT PART
def send(self, cmd):
print(cmd.strip()
) # gets rid of spaces (which don't exist) but good practice
self.serial.write(
cmd.encode('utf-8')
) # converts to something for serial with python AND SENDS AS WELL THIS IS THE SEND DAVE
self.serial.flush() # clears buffer (library)
print(
self.readline()
) # this is waiting for Drew's "OK" called above - doesn't do anything with this but print
# called by step_to - step 2
def move(self, pos):
cmd = 'STEP %d\n' % (
pos
) # cmd is a variable that's a string! It's what's sent over serial
# cmd = 'JOG %f %f %d\n' % (da1, da2, ms) #this says make a float, a float, and a decimal, a linebreak and the stuff in parenthesis is what gets filled in
self.send(cmd)
class ArduinoController():
def __init__(self, port: str, steppers: dict, **kwargs):
self.port = port
self.steppers = steppers
self.baudrate = kwargs.get('baudrate', 9600)
self.timeout = kwargs.get('timeout', 1)
self.serial = Serial(self.port, self.baudrate, timeout=self.timeout)
def confirmContact(self):
byte_received = False
while not byte_received:
byte = self.serial.read()
if byte == b'A':
print('first contact attempt received')
byte_received = True
self.serial.flush()
self.serial.write(b'A')
self.serial.write(b'Mooie test gek')
while True:
self.serial.flush()
print(self.serial.readline())
class Rangefinder(object):
def __init__(self):
#self.serialDevice = "/dev/ttyAMA0" # default for RaspberryPi
self.serialDevice = "/dev/serial0"
self.connection = Serial(self.serialDevice)
#self.node = rospy.init_node('rangeFinder', anonymous=True)
self.pub = rospy.Publisher('range', Int16, queue_size=10)
def getRange(self):
b = self.connection.read(1)
while b != b'R':
b = self.connection.read(1)
b = self.connection.read(1)
num = b''
while b != b'\r':
num += b
b = self.connection.read(1)
if self.connection.in_waiting > 10:
self.connection.flush()
val = int(num.decode()) // 10
print('MASSIVE FARTS!!!' + str(val))
return val
def pubRange(self):
b = self.connection.read(1)
while b != b'R':
b = self.connection.read(1)
b = self.connection.read(1)
num = b''
while b != b'\r':
num += b
b = self.connection.read(1)
if self.connection.in_waiting > 10:
self.connection.flush()
val = int(num.decode()) // 10
#print(val)
self.pub.publish(val)
class EchoApplication(WebSocketApplication):
def on_open(self):
print "Connection opened"
self.serialPort = Serial(serialPortFile, 9600, timeout = 1)
def on_message(self, message):
if message == None:
return
print "Got message: ", message
if message == "LED On":
self.serialPort.write('l')
self.serialPort.flush()
if message == "LED Off":
self.serialPort.write('k')
self.serialPort.flush()
self.ws.send(message)
def on_close(self, reason):
self.serialPort.close()
print "Closing", reason
def connect():
#Initiate a connection
print("initializing..")
arduino = Serial('/dev/ttyACM0', 115200)
#Read confirmation from Arduino
print("Done initializing...")
arduino.flushInput()
arduino.flushOutput()
arduino.flush()
count = 0
try:
while True:
if count == 0:
print('about to write..')
arduino.write("H".encode('utf-8'))
print('wrote..')
count = count + 1
if arduino.inWaiting() > 0:
#read the message from the Arduino
print('about to read.')
time.sleep(0.2)
raw_message = str(arduino.read(1))
print('reading..')
#remove EOL characters from string
message = raw_message.rstrip()
print(message)
count = count + 1
if count >= 100:
count = 0
print ("count " + str(count))
except:
arduino.flushInput()
arduino.flushOutput()
arduino.flush()
arduino.close()
print('closed')
class Arduino:
def __init__(self, device):
super().__init__()
self.logger = Log.instance('Arduino')
try:
self.port = Serial(device, baudrate=9600, timeout=1.0)
except Exception as e:
self.logger.error(e)
def freight_ready(self):
if self.port.in_waiting > 0:
c = self.port.read()
self.logger.info('read "{}"'.format(c))
return c.decode() == 'L'
else:
return False
def write(self, cmd):
self.port.write(cmd.encode())
self.port.flush()
self.logger.info('sent command: "{}"'.format(cmd))
def move_off(self):
self.write('G')
def pick_up(self, height):
cmd = 'S{};'.format(height)
self.write(cmd)
def decelerate(self):
self.write('D')
def drop_freight(self, height):
cmd = 'S{};'.format(height)
self.write(cmd)
def halt(self):
self.write('H')
class SRFNoThread:
def __init__(self, port):
# Serial connection to Arduino
self.port = port
print("Connecting to arduino...")
self.arduinoSerial = Serial(port=self.port,
baudrate=ARDU_BAUD,
timeout=1)
sleep(3)
print("Arduino connected")
def distance(self):
while True:
self.arduinoSerial.flush()
self.arduinoSerial.write(b'a')
srf_distance = self.arduinoSerial.readline().decode(
'ascii').strip()
try:
distance = int(srf_distance) / 100.0
return distance
except:
continue
class SAMBA(object):
def __init__(self, port_name, progress=None, application_name='Brick Viewer'):
self.r_command_bug = False
self.sam_series = None
self.current_mode = None
self.progress = progress
try:
self.port = Serial(port_name, 115200, timeout=5)
except SerialException as e:
if '[Errno 13]' in str(e):
if sys.platform.startswith('linux'):
raise SAMBAException("No permission to open serial port, try starting {0} as root".format(application_name))
else:
raise SAMBAException("No permission to open serial port")
else:
raise e
try:
self.change_mode('T')
self.change_mode('N')
except:
raise SAMBAException('No Brick in Bootloader found')
chipid_cidr = self.read_uint32(CHIPID_CIDR)
chipid_exid = self.read_uint32(CHIPID_EXID)
# SAM3
if chipid_cidr == CHIPID_CIDR_ATSAM3S2B_A:
self.sam_series = 3
self.flash_base = 0x400000
self.flash_page_count = 512
self.flash_page_size = 256
self.flash_lockbit_count = 8
elif chipid_cidr == CHIPID_CIDR_ATSAM3S4C_A:
self.sam_series = 3
self.flash_base = 0x400000
self.flash_page_count = 1024
self.flash_page_size = 256
self.flash_lockbit_count = 16
# SAM4S
elif chipid_cidr in [CHIPID_CIDR_ATSAM4S2B_A, CHIPID_CIDR_ATSAM4S2B_B]:
self.sam_series = 4
self.flash_base = 0x400000
self.flash_page_count = 256
self.flash_page_size = 512
self.flash_lockbit_count = 16
elif chipid_cidr in [CHIPID_CIDR_ATSAM4S4C_A, CHIPID_CIDR_ATSAM4S4C_B]:
self.sam_series = 4
self.flash_base = 0x400000
self.flash_page_count = 512
self.flash_page_size = 512
self.flash_lockbit_count = 32
# SAM4E
elif chipid_cidr in [CHIPID_CIDR_ATSAM4E8C_A, CHIPID_CIDR_ATSAM4E8C_B] and \
chipid_exid in [CHIPID_EXID_ATSAM4E8C_A, CHIPID_EXID_ATSAM4E8C_B]:
self.sam_series = 4
self.flash_base = 0x400000
self.flash_page_count = 1024
self.flash_page_size = 512
self.flash_lockbit_count = 64
elif chipid_cidr in [CHIPID_CIDR_ATSAM4E16E_A, CHIPID_CIDR_ATSAM4E16E_B] and \
chipid_exid in [CHIPID_EXID_ATSAM4E16E_A, CHIPID_EXID_ATSAM4E16E_B]:
self.sam_series = 4
self.flash_base = 0x400000
self.flash_page_count = 2048
self.flash_page_size = 512
self.flash_lockbit_count = 128
# unknown
else:
raise SAMBAException('Brick has unknown CHIPID: 0x%08X / 0x%08X' % (chipid_cidr, chipid_exid))
self.flash_size = self.flash_page_count * self.flash_page_size
self.flash_lockregion_size = self.flash_size // self.flash_lockbit_count
self.flash_pages_per_lockregion = self.flash_lockregion_size // self.flash_page_size
def change_mode(self, mode):
if self.current_mode == mode:
return
try:
self.port.write((mode + '#').encode('ascii'))
except:
raise SAMBAException('Write error during mode change')
if mode == 'T':
while True:
try:
response = self.port.read(1)
except:
raise SAMBAException('Read error during mode change')
if len(response) == 0:
raise SAMBAException('Read timeout during mode change')
if response == b'>':
break
else:
try:
response = self.port.read(2)
except:
raise SAMBAException('Read error during mode change')
if len(response) == 0:
raise SAMBAException('Read timeout during mode change')
if response != b'\n\r':
raise SAMBAException('Protocol error during mode change')
self.current_mode = mode
def read_uid64(self):
self.write_flash_command(EEFC_FCR_FCMD_STUI, 0)
self.wait_for_flash_ready('while reading UID', ready=False)
uid1 = self.read_uint32(self.flash_base + 8)
uid2 = self.read_uint32(self.flash_base + 12)
self.write_flash_command(EEFC_FCR_FCMD_SPUI, 0)
self.wait_for_flash_ready('after reading UID')
return uid2 << 32 | uid1
def flash(self, firmware, imu_calibration, lock_imu_calibration_pages):
# Split firmware into pages
firmware_pages = []
offset = 0
while offset < len(firmware):
page = firmware[offset:offset + self.flash_page_size]
if len(page) < self.flash_page_size:
page += b'\xff' * (self.flash_page_size - len(page))
firmware_pages.append(page)
offset += self.flash_page_size
if self.sam_series == 3:
# SAM3S flash programming erata: FWS must be 6
self.write_uint32(EEFC_FMR, 0x06 << 8)
# Unlock
self.reset_progress('Unlocking flash pages', 0)
self.wait_for_flash_ready('before unlocking flash pages')
for region in range(self.flash_lockbit_count):
page_num = (region * self.flash_page_count) // self.flash_lockbit_count
self.write_flash_command(EEFC_FCR_FCMD_CLB, page_num)
self.wait_for_flash_ready('while unlocking flash pages')
# Erase All
self.reset_progress('Erasing flash pages', 0)
self.write_flash_command(EEFC_FCR_FCMD_EA, 0)
self.wait_for_flash_ready('while erasing flash pages', timeout=10000, update_progress=True)
# Write firmware
self.write_pages(firmware_pages, 0, 'Writing firmware')
# Write IMU calibration
if imu_calibration is not None:
self.reset_progress('Writing IMU calibration', 0)
ic_relative_address = self.flash_size - 0x1000 * 2 - 12 - 0x400
ic_prefix_length = ic_relative_address % self.flash_page_size
ic_prefix_address = self.flash_base + ic_relative_address - ic_prefix_length
ic_prefix = ''
offset = 0
while len(ic_prefix) < ic_prefix_length:
ic_prefix += self.read_word(ic_prefix_address + offset)
offset += 4
prefixed_imu_calibration = ic_prefix + imu_calibration
# Split IMU calibration into pages
imu_calibration_pages = []
offset = 0
while offset < len(prefixed_imu_calibration):
page = prefixed_imu_calibration[offset:offset + self.flash_page_size]
if len(page) < self.flash_page_size:
page += b'\xff' * (self.flash_page_size - len(page))
imu_calibration_pages.append(page)
offset += self.flash_page_size
# Write IMU calibration
page_num_offset = (ic_relative_address - ic_prefix_length) // self.flash_page_size
self.write_pages(imu_calibration_pages, page_num_offset, 'Writing IMU calibration')
# Lock firmware
self.reset_progress('Locking flash pages', 0)
self.lock_pages(0, len(firmware_pages))
# Lock IMU calibration
if imu_calibration is not None and lock_imu_calibration_pages:
first_page_num = (ic_relative_address - ic_prefix_length) // self.flash_page_size
self.lock_pages(first_page_num, len(imu_calibration_pages))
# Verify firmware
self.verify_pages(firmware_pages, 0, 'firmware', imu_calibration is not None)
# Verify IMU calibration
if imu_calibration is not None:
page_num_offset = (ic_relative_address - ic_prefix_length) // self.flash_page_size
self.verify_pages(imu_calibration_pages, page_num_offset, 'IMU calibration', True)
# Set Boot-from-Flash flag
self.reset_progress('Setting Boot-from-Flash flag', 0)
self.wait_for_flash_ready('before setting Boot-from-Flash flag')
self.write_flash_command(EEFC_FCR_FCMD_SGPB, 1)
self.wait_for_flash_ready('after setting Boot-from-Flash flag')
# Boot
try:
self.reset()
except SAMBAException as e:
raise SAMBARebootError(str(e))
def reset_progress(self, title, length):
if self.progress != None:
self.progress.reset(title, length)
def update_progress(self, value):
if self.progress != None:
self.progress.update(value)
def write_pages(self, pages, page_num_offset, title):
self.reset_progress(title, len(pages))
page_num = 0
for page in pages:
# FIXME: the S command used by the write_bytes function doesn't
# write the data correctly. instead use the write_word function
if False:
address = self.flash_base + (page_num_offset + page_num) * self.flash_page_size
self.write_bytes(address, page)
else:
offset = 0
while offset < len(page):
address = self.flash_base + (page_num_offset + page_num) * self.flash_page_size + offset
self.write_word(address, page[offset:offset + 4])
offset += 4
self.wait_for_flash_ready('while writing flash pages')
self.write_flash_command(EEFC_FCR_FCMD_WP, page_num_offset + page_num)
self.wait_for_flash_ready('while writing flash pages')
page_num += 1
self.update_progress(page_num)
def verify_pages(self, pages, page_num_offset, title, title_in_error):
self.reset_progress('Verifying written ' + title, len(pages))
offset = page_num_offset * self.flash_page_size
page_num = 0
for page in pages:
read_page = self.read_bytes(self.flash_base + offset, len(page))
offset += len(page)
if read_page != page:
if title_in_error:
raise SAMBAException('Verification error ({0})'.format(title))
else:
raise SAMBAException('Verification error')
page_num += 1
self.update_progress(page_num)
def lock_pages(self, page_num, page_count):
start_page_num = page_num - (page_num % self.flash_pages_per_lockregion)
end_page_num = page_num + page_count
if (end_page_num % self.flash_pages_per_lockregion) != 0:
end_page_num += self.flash_pages_per_lockregion - (end_page_num % self.flash_pages_per_lockregion)
self.wait_for_flash_ready('before locking flash pages')
for region in range(start_page_num // self.flash_pages_per_lockregion,
end_page_num // self.flash_pages_per_lockregion):
page_num = (region * self.flash_page_count) // self.flash_lockbit_count
self.write_flash_command(EEFC_FCR_FCMD_SLB, page_num)
self.wait_for_flash_ready('while locking flash pages')
def read_word(self, address): # 4 bytes
self.change_mode('N')
try:
self.port.write(('w%X,4#' % address).encode('ascii'))
except:
raise SAMBAException('Write error while reading from address 0x%08X' % address)
try:
response = self.port.read(4)
except:
raise SAMBAException('Read error while reading from address 0x%08X' % address)
if len(response) == 0:
raise SAMBAException('Timeout while reading from address 0x%08X' % address)
if len(response) != 4:
raise SAMBAException('Length error while reading from address 0x%08X' % address)
return response
def write_word(self, address, value): # 4 bytes
self.write_uint32(address, struct.unpack('<I', value)[0])
def read_uint32(self, address):
return struct.unpack('<I', self.read_word(address))[0]
def write_uint32(self, address, value):
self.change_mode('N')
try:
self.port.write(('W%X,%X#' % (address, value)).encode('ascii'))
except:
raise SAMBAException('Write error while writing to address 0x%08X' % address)
def read_bytes(self, address, length):
# according to the BOSSA flash program, SAM-BA can have a bug regarding
# reading more than 32 bytes at a time if the amount to be read is a
# power of 2. to work around this split the read operation in two steps
prefix = b''
if self.r_command_bug and length > 32 and length & (length - 1) == 0:
prefix = self.read_word(address)
address += 4
length -= 4
self.change_mode('T')
try:
self.port.write(('R%X,%X#' % (address, length)).encode('ascii'))
except:
raise SAMBAException('Write error while reading from address 0x%08X' % address)
try:
response = self.port.read(length + 3)
except:
raise SAMBAException('Read error while reading from address 0x%08X' % address)
if len(response) == 0:
raise SAMBAException('Timeout while reading from address 0x%08X' % address)
if len(response) != length + 3:
raise SAMBAException('Length error while reading from address 0x%08X' % address)
if not response.startswith(b'\n\r') or not response.endswith(b'>'):
raise SAMBAException('Protocol error while reading from address 0x%08X' % address)
return prefix + response[2:-1]
def write_bytes(self, address, bytes_):
self.change_mode('T')
# FIXME: writes '33337777BBBBFFFF' instead of '0123456789ABCDEF'
try:
# according to the BOSSA flash program, SAM-BA can get confused if
# the command and the data to be written is received in the same USB
# packet. to work around this, flush the serial port in between the
# command and the data
self.port.write(('S%X,%X#' % (address, len(bytes_))).encode('ascii'))
self.port.flush()
self.port.write(bytes_)
except:
raise SAMBAException('Write error while writing to address 0x%08X' % address)
try:
response = self.port.read(3)
except:
raise SAMBAException('Read error while writing to address 0x%08X' % address)
if len(response) == 0:
raise SAMBAException('Timeout while writing to address 0x%08X' % address)
if response != b'\n\r>':
raise SAMBAException('Protocol error while writing to address 0x%08X' % address)
def reset(self):
self.reset_progress('Triggering Brick reset', 0)
try:
self.write_uint32(RSTC_MR, (RSTC_MR_KEY << RSTC_MR_KEY_OFFSET) | (10 << RSTC_MR_ERSTL_OFFSET) | RSTC_MR_URSTEN)
self.write_uint32(RSTC_CR, (RSTC_CR_KEY << RSTC_CR_KEY_OFFSET) | RSTC_CR_EXTRST | RSTC_CR_PERRST | RSTC_CR_PROCRST)
except:
raise SAMBAException('Write error while triggering reset')
def go(self, address):
self.change_mode('N')
try:
# according to the BOSSA flash program, SAM-BA can get confused if
# another command is received in the same USB packet as the G
# command. to work around this, flush the serial port afterwards
self.port.write(('G%X#' % address).encode('ascii'))
self.port.flush()
except:
raise SAMBAException('Write error while executing code at address 0x%08X' % address)
def wait_for_flash_ready(self, message, timeout=2000, ready=True, update_progress=False):
for i in range(timeout):
fsr = self.read_uint32(EEFC_FSR)
if (fsr & EEFC_FSR_FLOCKE) != 0:
raise SAMBAException('Flash locking error ' + message)
if (fsr & EEFC_FSR_FCMDE) != 0:
raise SAMBAException('Flash command error ' + message)
if self.sam_series == 4 and (fsr & EEFC_FSR_FLERR) != 0:
raise SAMBAException('Flash memory error ' + message)
if ready:
if (fsr & EEFC_FSR_FRDY) != 0:
break
else:
if (fsr & EEFC_FSR_FRDY) == 0:
break
time.sleep(0.001)
if update_progress:
self.update_progress(0)
else:
raise SAMBAException('Flash timeout ' + message)
def write_flash_command(self, command, argument):
self.write_uint32(EEFC_FCR, (EEFC_FCR_FKEY << 24) | (argument << 8) | command)
print ports
command=command+"\n\r"
print command
com = Serial(
port=ports,
baudrate=9600,
bytesize=8,
parity='N',
stopbits=1,
timeout=1,#None,
xonxoff=0,
rtscts=0,
writeTimeout=1,#None,
dsrdtr=None)
#print com.portstr
#com.write("%01#RDD0000000015**\r")
com.flush()
i=0
com.write(command)
#res=com.read
com.close()
#print len(res)
#print res
#with open('res.txt', 'w+b') as file:
# file.write(res)
class FieldforceTCM:
Component = namedtuple('Component', [
'name', 'struct'
])
ModInfo = namedtuple('ModInfo', [
'Type', 'Revision'
])
CalScores = namedtuple('CalScores', [
'MagCalScore', 'CalParam2', 'AccelCalScore', 'DistError',
'TiltError', 'TiltRange'
])
AcqParams = namedtuple('AcqParams', [
'PollingMode', 'FlushFilter', 'SensorAcqTime', 'IntervalRespTime'
])
Datum = namedtuple('Datum', [
'Heading', 'Temperature', 'Distortion', 'CalStatus',
'PAligned', 'RAligned', 'IZAligned',
'PAngle', 'RAngle', 'XAligned', 'YAligned', 'ZAligned'
])
good_cal_score = CalScores('< 1', 'ignore (pni reserved)', '< 1',
'< 1', '< 1', 'angle of tilt' )
components = {
5: Component('Heading', _struct_float32),
7: Component('Temperature', _struct_float32),
8: Component('Distortion', _struct_boolean),
9: Component('CalStatus', _struct_boolean),
21: Component('PAligned', _struct_float32),
22: Component('RAligned', _struct_float32),
23: Component('IZAligned', _struct_float32),
24: Component('PAngle', _struct_float32),
25: Component('RAngle', _struct_float32),
27: Component('XAligned', _struct_float32),
28: Component('YAligned', _struct_float32),
29: Component('ZAligned', _struct_float32)
}
config = {
1: Component('Declination', _struct_float32),
2: Component('TrueNorth', _struct_boolean),
6: Component('BigEndian', _struct_boolean),
10: Component('MountingRef', _struct_uint8),
12: Component('UserCalNumPoints', _struct_uint32),
13: Component('UserCalAutoSampling', _struct_boolean),
14: Component('BaudRate', _struct_uint8),
15: Component('MilOutput', _struct_boolean),
16: Component('DataCal', _struct_boolean),
18: Component('CoeffCopySet', _struct_uint32),
19: Component('AccelCoeffCopySet', _struct_uint32)
}
fir_defaults = {
0: [ ],
4: [ 4.6708657655334e-2, 4.5329134234467e-1,
4.5329134234467e-1, 4.6708657655334e-2 ],
8: [ 1.9875512449729e-2, 6.4500864832660e-2,
1.6637325898141e-1, 2.4925036373620e-1,
2.4925036373620e-1, 1.6637325898141e-1,
6.4500864832660e-2, 1.9875512449729e-2 ],
16: [ 7.9724971069144e-3, 1.2710056429342e-2,
2.5971390034516e-2, 4.6451949792704e-2,
7.1024151197772e-2, 9.5354386848804e-2,
1.1484431942626e-1, 1.2567124916369e-1,
1.2567124916369e-1, 1.1484431942626e-1,
9.5354386848804e-2, 7.1024151197772e-2,
4.6451949792704e-2, 2.5971390034516e-2,
1.2710056429342e-2, 7.9724971069144e-3 ],
32: [ 1.4823725958818e-3, 2.0737124095482e-3,
3.2757326624196e-3, 5.3097803863757e-3,
8.3414139286254e-3, 1.2456836057785e-2,
1.7646051430536e-2, 2.3794805168613e-2,
3.0686505921968e-2, 3.8014333463472e-2,
4.5402682509802e-2, 5.2436112653103e-2,
5.8693165018301e-2, 6.3781858267530e-2,
6.7373451424187e-2, 6.9231186101853e-2,
6.9231186101853e-2, 6.7373451424187e-2,
6.3781858267530e-2, 5.8693165018301e-2,
5.2436112653103e-2, 4.5402682509802e-2,
3.8014333463472e-2, 3.0686505921968e-2,
2.3794805168613e-2, 1.7646051430536e-2,
1.2456836057785e-2, 8.3414139286254e-3,
5.3097803863757e-3, 3.2757326624196e-3,
2.0737124095482e-3, 1.4823725958818e-3 ]
}
def reader(self):
"""
factory which returns a callable object suitable (for example)
to pass to threading.Thread(target=reader()).
"""
def do_it():
while True:
self._wait_and_read_all()
rdy_pkts = self._decode()
if rdy_pkts:
#for pkt in rdy_pkts:
# fid = ord(pkt[0])
# print 'new pkt: {0} {1}'.format(FrameID.invert[fid], fid)
self._notify_listeners(rdy_pkts)
return do_it
def _wait_and_read_all(self):
s = self.fp
b = self.recv_buf
b.append(s.read())
wait_ct = s.inWaiting()
if wait_ct > 0:
b.extend(s.read(wait_ct))
def _notify_listeners(self, rdy_pkts):
cbs = self.recv_cbs
lock = self.cb_lock
lock.acquire()
x = []
for i in range(0, len(cbs)):
if cbs[i](rdy_pkts):
# A callback returning true is removed.
# XXX: Can't remove while iterating over it.
x.append(i)
for it in x:
del cbs[it]
lock.release()
def _decode(self):
"""
Given self.recv_buf and self.crc, attempts to decode a valid packet,
advancing by a single byte on each decoding failure.
"""
b = self.recv_buf
crc_fn = self.crc
decode_pos = 0
discard_amt = 0
good_pos = 0 # discard before this
decode_len = len(b)
rdy_pkts = []
# min packet = 2 (byte_count) + 1 (frame id) + 2 (crc) = 5
#attempt_ct = 0
#print decode_len
while decode_len >= 5:
#attempt_ct += 1
#print '--decode attempt {0}'.format(attempt_ct)
(byte_count, ) = struct.unpack('>H', bytes(b[decode_pos:decode_pos+2]))
frame_size = byte_count - 4
# max frame = 4092, min frame = 1
if frame_size < 1 or frame_size > 4092:
#print '-- fail 1Â {0}'.format(frame_size)
decode_pos += 1
decode_len -= 1
continue
# not enough in buffer for this decoding
if decode_len < byte_count:
#print '-- fail 2'
decode_pos += 1
decode_len -= 1
continue
frame_pos = decode_pos + 2
frame_id = b[frame_pos]
# invalid frame id
if frame_id not in FrameID.__dict__.itervalues():
#print '-- fail 3'
decode_pos += 1
decode_len -= 1
continue
crc_pos = frame_pos + frame_size
crc = b[crc_pos:crc_pos + 2]
entire_pkt = b[decode_pos:frame_pos + frame_size + 2]
# CRC failure
crc_check = crc_fn(bytes(entire_pkt))
if crc_check != 0:
#print '-- fail 4'
decode_pos += 1
decode_len -= 1
continue
# valid packet? wow.
rdy_pkts.append(b[frame_pos:frame_pos + frame_size])
# number of invalid bytes discarded to make this work.
discard_amt += decode_pos - good_pos
# advance to right after the decoded packet.
decode_pos += byte_count
decode_len -= byte_count
# the decode position that will be started from next time
good_pos = decode_pos
# discard this packet from buffer. also discard everything prior.
del b[0:good_pos]
self.discard_stat += discard_amt
return rdy_pkts
def remove_listener(self, r):
c = self.recv_cbs
l = self.cb_lock
l.acquire()
try:
# FIXME: linear search.
c.remove(r)
except Exception:
pass
l.release()
def add_listener(self, cb):
c = self.recv_cbs
l = self.cb_lock
l.acquire()
c.append(cb)
l.release()
return cb
def _recv_msg_prep(self, *expected_frame_id):
s = _one_msg_stall(*expected_frame_id)
t = self.add_listener(s.cb())
return (s, t)
def _recv_msg_wait(self, w, timeout = _DEFAULT_TIMEOUT):
s, t = w
r = s.wait(timeout)
self.remove_listener(t)
if (r == None):
raise TimeoutException('Did not recv frame_id {0} within {1} seconds.'.format(s.frame_ids, timeout), timeout)
else:
return (ord(r[0]), r[1:])
def _recvSpecificMessage(self, *expected_frame_id, **only_timeout):
w = self._recv_msg_prep(self, *expected_frame_id)
return self._recv_msg_wait(w, **only_timeout)
def _send_msg_w_resp(self, send_frame_id, payload, recv_frame_id, timeout=_DEFAULT_TIMEOUT):
""" Send a full message (with payload) and wait for a responce """
w = self._recv_msg_prep(self, recv_frame_id)
self._sendMessage(send_frame_id, payload)
return self._recv_msg_wait(w, timeout=timeout)
def _send_s_msg_w_resp(self, send_frame_id, recv_frame_id, timeout=_DEFAULT_TIMEOUT):
""" Send a simple message (only frame id) and wait for a responce. """
w = self._recv_msg_prep(self, recv_frame_id)
self._sendMessage(send_frame_id)
return self._recv_msg_wait(w, timeout=timeout)
def __init__(self, path, baud):
self.fp = Serial(
port = path,
baudrate = baud,
bytesize = serial.EIGHTBITS,
parity = serial.PARITY_NONE,
stopbits = serial.STOPBITS_ONE
)
# CRC-16 with generator polynomial X^16 + X^12 + X^5 + 1.
self.crc = crcmod.mkCrcFun(0b10001000000100001, 0, False)
self.recv_cbs = []
self.cb_lock = threading.Lock()
self.decode_pos = 0
self.discard_stat = 0
self.recv_buf = bytearray()
self.read_th = rt = threading.Thread(target=self.reader())
rt.daemon = True
rt.start()
def close(self):
self.fp.flush()
self.fp.close()
def _send(self, fmt):
self.fp.write(fmt)
def _sendMessage(self, frame_id, payload=b''):
count = len(payload) + 5
head = struct.pack('>HB{0}s'.format(len(payload)), count, frame_id, payload)
tail = struct.pack('>H', self.crc(head))
self._send(head + tail)
def _createDatum(self, data):
for component in self.Datum._fields:
if component not in data.keys():
data[component] = None
return self.Datum(**data)
def getModelInfo(self):
"""
Query the module's type and firmware revision number.
"""
(_, payload) = self._send_s_msg_w_resp(FrameID.kGetModInfo, FrameID.kGetModInfo)
return self.ModInfo(*struct.unpack('>4s4s', payload))
def readData(self, timeout=None):
"""
Read a single DataResp frame
"""
(_, payload) = self._recvSpecificMessage(FrameID.kDataResp, timeout=timeout)
(comp_count, ) = struct.unpack('>B', payload[0])
comp_index = 0
offset = 1
data = dict()
while comp_index < comp_count:
(component_id, ) = struct.unpack('>B', payload[offset])
component = self.components[component_id]
datum = payload[(offset + 1):(offset + component.struct.size + 1)]
(value, ) = component.struct.unpack(datum)
data[component.name] = value
offset += 1 + component.struct.size
comp_index += 1
return self._createDatum(data)
def getData(self, timeout=None):
"""
Query a single packet of data that containing the components specified
by setDataComponents(). All other components are set to zero.
"""
# FIXME: race condition: data may already have passed us by before we are listening.
self._sendMessage(FrameID.kGetData)
return self.readData(timeout)
def setConfig(self, config_id, value):
"""
Sets a single configuration value based on the config_id, which must be
one of the values in Configuration. Acceptable values deend upon the
configuration option being set.
"""
payload_id = _struct_uint8.pack(config_id)
payload_value = self.config[config_id].struct.pack(value)
self._send_msg_w_resp(FrameID.kSetConfig, payload_id + payload_value, FrameID.kSetConfigDone)
def setOrientation(self, orientation):
"""
Set the orientation of the compass
"""
self.setConfig(Configuration.kMountingRef, orientation)
def takeUserCalSample(self):
"""
Commands the module to take a sample during user calibration.
"""
self._sendMessage(FrameID.kTakeUserCalSample)
def getConfig(self, config_id):
"""
Get the value of a single configuration value based on config_id.
"""
payload_id = self._struct_uint8.pack(config_id)
(_, response) = self._send_msg_w_resp(FrameID.kGetConfig, payload_id, FrameID.kConfigResp)
(response_id, ) = self._struct_uint8.unpack(response[0])
if response_id == config_id:
(value, ) = self.config[config_id].struct.unpack(response[1:])
return value
else:
raise IOError('Response has unexpected configuration id: {0}.'
.format(response_id))
def setFilter(self, count, values=None):
"""
Configure the number of taps and weights of the on-board finite impulse
response (FIR) filter. The number of taps must be zero (i.e. disabled),
4, 8, 16, or 32. If values is omitted or is set to None, the weights
default to PNI's recommended values. See the Fieldforce TCM User Manual
for details.
"""
assert count in [ 0, 4, 8, 16, 32 ]
if values == None:
values = self.fir_defaults[count]
else:
assert len(values) == count
payload = struct.pack('>BBB{0}d'.format(count), 3, 1, count, *values)
self._send_msg_w_resp(FrameID.kSetParam, payload, FrameID.kSetParamDone)
def getFilter(self):
"""
Gets the current finite impulse response (FIR) filter weights. See
setFilter() for more information.
"""
payload_request = struct.pack('>BB', 3, 1)
(_, payload_response) = self._send_msg_w_resp(FrameID.kGetParam, payload_request, FrameID.kParamResp)
param_id, axis_id, count = struct.unpack('>BBB', payload_response[0:3])
if param_id != 3:
raise IOError('Expected param ID of 3, got {0}'.format(param_id))
elif axis_id != 1:
raise IOError('Expected axis ID of 1, got {0}'.format(axis_id))
fir = struct.unpack('>{0}d'.format(count), payload_response[3:])
return list(fir)
def setDataComponents(self, components):
"""
Specify which data components, specified as a list of component IDs,
will be returned with each sample. An arbitrary number of IDs is
supported.
"""
count = len(components)
payload_counts = struct.pack('>B', count)
payload_content = struct.pack('>{0}B'.format(count), *components)
payload = payload_counts + payload_content
self._sendMessage(FrameID.kSetDataComponents, payload)
def setAcquisitionParams(self, mode, flush_filter, acq_time, resp_time):
"""
Set the acquisition mode, including:
- PollingMode: poll if true, push if false
- FlushFilter: flush the FIR filter registers after each sample
- SensorAcqTime: time between sample in seconds
- IntervalRespTime: time delay between sending subsequent samples
Even if polling is enabled here, it must be explicitly started using
startStreaming().
"""
payload = struct.pack('>BBff', mode, flush_filter, acq_time, resp_time)
self._send_msg_w_resp(FrameID.kSetAcqParams, payload, FrameID.kAcqParamsDone)
def getAcquisitionParams(self):
"""
Gets the current acquisition mode. See setAcquisitionParams() for more
information.
"""
(_, payload) = self._send_s_msg_w_resp(FrameID.kGetAcqParams, kAcqParamsResp)
response = struct.unpack('>BBff', payload)
return self.AcqParams(*response)
def startStreaming(self):
"""
Start streaming data. See setAcquisitionParams() for more information
and use stopStreaming() when done. Streaming must be stopped before any
other commands can be used.
"""
self._sendMessage(FrameID.kStartIntervalMode)
def stopAll(self):
"""
Stop all modes which result in periodic messages
"""
self.stopStreaming()
self.stopCalibration()
def stopStreaming(self):
"""
Stops streaming data; companion of startStreaming(). Streaming must be
stopped before any other commands can be used.
"""
self._sendMessage(FrameID.kStopIntervalMode)
def stopCalibration(self):
"""
Stops calibration;
"""
self._sendMessage(FrameID.kStopCal)
def powerUp(self):
"""
Power up the sensor after a powerDown(). This has undefined results if
the sensor is already powered on.
"""
w = self._recv_msg_prep(FrameID.kPowerUp)
self._send(b'\xFF')
self._recv_msg_wait(w)
def powerDown(self):
"""
Power down the sensor down.
"""
self._send_s_msg_w_resp(FrameID.kPowerDown, FrameID.kPowerDownDone)
def save(self):
"""
Write the current configuration to non-volatile memory. Note that this
is the only command that writes to non-volatile memory, so it should be
paired with any configuration options (e.g. calibration) that are
intended to be persistant.
"""
(_, response) = self._send_s_msg_w_resp(FrameID.kSave, FrameID.kSaveDone)
(code, ) = _struct_uint16.unpack(response)
if code != 0:
raise IOError('Save failed with error code {0}.'.format(code))
def startCalibration(self, mode=None):
"""
Starts calibration. See the FieldForce TCM User Manual for details
about the necessary setup for each calibration mode.
When mode = None, the last calibration mode used is repeated.
"""
if mode == None:
self._sendMessage(FrameID.kStartCal)
else:
payload_mode = _struct_uint32.pack(mode)
self._sendMessage(FrameID.kStartCal, payload_mode)
def getCalibrationStatus(self, timeout=15):
"""
Blocks waiting for a calibration update from the sensor. This returns a
tuple where the first elements is a boolean that indicates whether the
calibration is complete.
"""
frame_id, message = self._recvSpecificMessage(FrameID.kUserCalSampCount, FrameID.kUserCalScore, timeout=timeout)
# One UserCalSampCount message is generated for each recorded
# sample. This continues until the calibration has converged or the
# maximum number of points have been collected.
if frame_id == FrameID.kUserCalSampCount:
(sample_num, ) = _struct_uint32.unpack(message)
return (False, sample_num)
# Calibration accuracy is reported in a single UserCalScore message
# once calibration is complete.
elif frame_id == FrameID.kUserCalScore:
scores_raw = struct.unpack('>6f', message)
scores = self.CalScores(*scores_raw)
return (True, scores)
else:
raise IOError('Unexpected frame id: {0}.'.format(frame_id))
def resetMagCalibration(self):
self._send_s_msg_w_resp(FrameID.kFactoryUserCal, FrameID.kFactoryUserCalDone)
def resetAccelCalibration(self):
self._send_s_msg_w_resp(FrameID.kFactoryInclCal, FrameID.kFactoryInclCalDone)
def handle_send_break_cmd(self, port, disk, reset_type=None, baudrate=115200, timeout=1, verbose=False):
"""! Resets platforms and prints serial port output
@detail Mix with switch -r RESET_TYPE and -p PORT for versatility
"""
from serial import Serial
if not reset_type:
reset_type = 'default'
port_config = port.split(':')
if len(port_config) == 2:
# -p COM4:115200
port = port_config[0]
baudrate = int(port_config[1])
elif len(port_config) == 3:
# -p COM4:115200:0.5
port = port_config[0]
baudrate = int(port_config[1])
timeout = float(port_config[2])
if verbose:
print "mbedhtrun: serial port configuration: %s:%s:%s"% (port, str(baudrate), str(timeout))
try:
serial_port = Serial(port, baudrate=baudrate, timeout=timeout)
except Exception as e:
print "mbedhtrun: %s" % (str(e))
print json.dumps({
"port" : port,
"disk" : disk,
"baudrate" : baudrate,
"timeout" : timeout,
"reset_type" : reset_type,
}, indent=4)
return False
serial_port.flush()
# Reset using one of the plugins
result = host_tests_plugins.call_plugin('ResetMethod', reset_type, serial=serial_port, disk=disk)
if not result:
print "mbedhtrun: reset plugin failed"
print json.dumps({
"port" : port,
"disk" : disk,
"baudrate" : baudrate,
"timeout" : timeout,
"reset_type" : reset_type
}, indent=4)
return False
print "mbedhtrun: serial dump started (use ctrl+c to break)"
try:
while True:
test_output = serial_port.read(512)
if test_output:
sys.stdout.write('%s'% test_output)
if "{end}" in test_output:
if verbose:
print
print "mbedhtrun: stopped (found '{end}' terminator)"
break
except KeyboardInterrupt:
print "ctrl+c break"
serial_port.close()
return True
class CommunicationGroup10:
def __init__(self, usb_port_number):
"""communication interface for sending commands and receiving sensor info to/from the Arduino
:param usb_port_number: the USB port number for the SRF stick - normally either 1 or 0, found by finding X in
the /dev/ttyACMX corresponding to the SRF stick (numbered by the order of USB devices connected)
"""
self.closed = True
self.connection = None
self.sender = None
self.port = '/dev/ttyACM{}'.format(usb_port_number)
self.baudrate = 115200
self.has_ball = False
self.timeout = 5
# start the router thread
self.router_thread = None
self.router_thread_alive = False
self.list = []
def __enter__(self):
return self
def __exit__(self, type, value, traceback):
self.close()
def open(self, handle_message):
self.handle_message = handle_message
print("[COMMS GROUP 10] opening connection...")
self.router_thread = Thread(target=self.router, name="Communication Group 10 Thread")
self.connection = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout)
self.sender = Sender(self.connection)
self.connection.flush()
self.router_thread_alive = True
self.router_thread.start()
self.closed = False
# shortcut functions for commands that can be sent
def configure_command(self, bearing, heading, grabbing=0, kicking=0):
self.list.append("{}{}{}{}".format(self.bearing_to_byte_string(bearing), self.bearing_to_byte_string(heading),
chr(grabbing+33), chr(kicking+33)))
@staticmethod
def bearing_to_byte_string(angle):
# range 0 to 180
angle_one = int(angle/2)
angle_two = angle - angle_one
return "{}{}".format(chr(int(angle_one+33)), chr(angle_two+33))
def router(self):
"""separate thread for routing received bytes to sender/receiver, and handling them appropriately"""
while self.router_thread_alive:
try:
tm.sleep(0.1)
d = None
if len(self.list) > 0:
self.connection.write(self.list[-1])
self.list = self.list[:-1]
d = self.connection.read(1)
if d == 't':
self.handle_message("B")
print "[COMMS GROUP 10] ball acquired"
else:
self.handle_message("G")
except Exception as e:
self.close()
raise e
def close(self):
"""must be called to kill the router thread and exit cleanly"""
if not self.closed:
self.closed = True
print("[COMMS GROUP 10] closing connection")
self.router_thread_alive = False # kill the router thread
self.connection.close() # kill the serial connection
def restart(self):
self.close()
self.open(self.handle_message)
from serial import Serial
import pynmea2
GPS = Serial(port = '/dev/ttyUSB0', baudrate = 4800)
GPS.flush()
for i in range(10):
inp_data = GPS.readline()
msg = pynmea2.parse(inp_data)
print msg
try:
print msg.timestamp
print "Longitude: {}".format(msg.longitude)
print "Latitude: {}".format(msg.latitude)
print "Speed: {}".format(msg.spd_over_grnd)
print msg
except:
pass
class Teensy:
def __init__(self, device):
self.errorCount = 0
self.initSerial(device)
def __del__(self):
if self.serial is not None:
self.serial.close()
#logging.debug("Teensy close")
def initSerial(self, device):
self.serial = Serial(device, 115200, timeout=1)
self.serial.write("l 0")
self.serial.flushInput()
self.initDevice()
def initDevice(self):
response = self.sendCommand("i", seperator="|")
self.name = None
try:
name = response[0]
if name:
self.name = name
except:
print "InitDevice Error"
pass
def write(self, data):
self.serial.write(data)
def read(self, numBytes):
return self.serial.read(numBytes)
def readWaiting(self):
if self.serial.inWaiting() > 0:
return self.read(self.serial.inWaiting())
return ""
def sendCommand(self, strCommand, seperator=None):
# sende Kommando und liefere Ergebnis, besserer Name
try:
#self.serial.flush()
self.serial.write(strCommand)
#self.serial.flush()
response = self.serial.readline().strip()
if seperator is not None:
response = response.split(seperator)
return response
except SerialException as e:
print "Error sendCommand", type(e), e.message
self.reCover()
raise
# device reports readiness to read but returned no data (device disconnected or multiple access on port?)
# Attempting to use a port that is not open
# write failed: [Errno 6] Device not configured
def reCover(self):
self.errorCount += 1
if self.errorCount > 10:
self.serial.close()
print "Reinit Serial"
self.initSerial(self.serial.name)
self.serial.flush()
self.errorCount = 0
time.sleep(1)
def sendQuick(self, strCommand):
# sende Kommando, besserer Name
try:
self.serial.flush()
self.serial.write(strCommand)
#self.serial.flush()
except:
raise
class Ostrich2(object):
def __init__(self, device):
self.channel = Serial(device, 921600, timeout = 5, writeTimeout = 5)
self.channel.flushInput()
def disconnect(self):
'''
'''
self.channel.close()
@staticmethod
def default_device():
if platform.system() == 'Windows':
return 'COM3'
elif platform.system() == 'Darwin':
serial_devices = glob.glob('/dev/tty.usbserial-*')
return serial_devices[0] if len(serial_devices) else '?'
else:
return '/dev/ttyUSB0'
###########################################################################
##
## IO operations
##
###########################################################################
def write(self, data):
'''
Write a sequence of bytes
'''
if isinstance(data, int):
data = bytes((data,))
result = self.channel.write(data)
if result != len(data):
raise Exception('write timeout')
self.channel.flush()
def write_with_checksum(self, data):
self.write(data)
self.write(make_checksum(data))
def read_with_checksum(self, size):
data = self.read(size)
checksum = self.read(1)
if checksum != make_checksum(data):
pass # TODO raise Exception('Read checksum does not match')
return data
def read(self, size = 1):
'''
Read a sequence of bytes
'''
if size == 0:
return
result = self.channel.read(size)
if len(result) != size:
raise Exception('I/O error')
return result
def read_byte(self):
'''
Read a single byte
'''
return int(self.read()[0])
##
## Protocol
##
def version(self):
self.write(b'VV')
device_type = self.read_byte()
version = self.read_byte()
device_id = chr(self.read_byte())
return (device_type, version, device_id)
def serial_number(self):
'''
returns (vendor ID, serial number)
'''
self.write_with_checksum(b'NS')
result = self.read_with_checksum(9)
vendor_id = int(result[0])
serial_number = ''.join(hex(b)[2:] for b in result[1:])
return (vendor_id, serial_number)
def write_memory(self, data, start_address, chunk_size = 0x1000):
if (len(data) & 0xff) or (start_address & 0xff):
raise Exception('Can only write 0x0100 byte blocks')
for address in range(start_address, start_address + len(data), chunk_size):
offset = address - start_address
block = data[offset:offset + chunk_size]
block_count = len(block) >> 8
mmsb = (address >> 16) & 0x07
msb = (address >> 8) & 0xff
message = bytearray(b'ZW')
message += bytes((block_count,))
message += bytes((mmsb,))
message += bytes((msb,))
message += block
self.write_with_checksum(message)
self.expect_ok()
yield len(block)
def read_memory(self, start, end, chunk_size = 0x1000):
# Can only read full 0x100 byte pages
read_start = start & 0x7ff00
read_end = (end & 0x7ff00) + 0x0100
block_count = chunk_size >> 8
for address in range(read_start, read_end, chunk_size):
this_chunk_size = chunk_size
trim_start = start - address
if trim_start > 0:
this_chunk_size -= trim_start & 0x7ff00
trim_start &= 0xff
trim_end = (address + this_chunk_size) - end - 1
if trim_end > 0:
this_chunk_size -= trim_end & 0x7ff00
trim_end &= 0xff
mmsb = (address >> 16) & 0x07
msb = (address >> 8) & 0xff
message = bytearray(b'ZR')
message += bytes((block_count,))
message += bytes((mmsb,))
message += bytes((msb,))
self.write_with_checksum(message)
data = self.read_with_checksum(this_chunk_size)
if trim_start > 0:
data = data[trim_start:]
if trim_end > 0:
data = data[:-trim_end]
yield data
def expect_ok(self):
response = self.read()
if response != b'O':
raise Exception('Write error')
def set_io_bank(self, bank):
message = bytearray(b'BR')
message += bytes((bank,))
self.write_with_checksum(message)
self.expect_ok()
def get_io_bank(self):
self.write_with_checksum(b'BRR')
return self.read_byte()
def set_emulation_bank(self, bank, persistent = False):
if persistent:
message = bytearray(b'BS')
else:
message = bytearray(b'BE')
message += bytes((bank,))
self.write_with_checksum(message)
self.expect_ok()
def get_emulation_bank(self, persistent = False):
if persistent:
message = b'BES'
else:
message = b'BER'
self.write_with_checksum(message)
return self.read_byte()
class Stk500v2(ispBase.IspBase):
def __init__(self):
self.serial = None
self.seq = 1
self.lastAddr = -1
self.progressCallback = None
def connect(self, port = 'COM22', speed = 115200):
if self.serial != None:
self.close()
try:
self.serial = Serial(port, speed, timeout=1, writeTimeout=10000)
except SerialException as e:
raise ispBase.IspError("Failed to open serial port")
except:
raise ispBase.IspError("Unexpected error while connecting to serial port:" + port + ":" + str(sys.exc_info()[0]))
self.seq = 1
#Reset the controller
self.serial.setDTR(1)
time.sleep(0.1)
self.serial.setDTR(0)
time.sleep(0.2)
self.sendMessage([1])
if self.sendMessage([0x10, 0xc8, 0x64, 0x19, 0x20, 0x00, 0x53, 0x03, 0xac, 0x53, 0x00, 0x00]) != [0x10, 0x00]:
self.close()
raise ispBase.IspError("Failed to enter programming mode")
def close(self):
if self.serial != None:
self.serial.close()
self.serial = None
#Leave ISP does not reset the serial port, only resets the device, and returns the serial port after disconnecting it from the programming interface.
# This allows you to use the serial port without opening it again.
def leaveISP(self):
if self.serial != None:
if self.sendMessage([0x11]) != [0x11, 0x00]:
raise ispBase.IspError("Failed to leave programming mode")
ret = self.serial
self.serial = None
return ret
return None
def isConnected(self):
return self.serial != None
def sendISP(self, data):
recv = self.sendMessage([0x1D, 4, 4, 0, data[0], data[1], data[2], data[3]])
return recv[2:6]
def writeFlash(self, flashData):
#Set load addr to 0, in case we have more then 64k flash we need to enable the address extension
pageSize = self.chip['pageSize'] * 2
flashSize = pageSize * self.chip['pageCount']
if flashSize > 0xFFFF:
self.sendMessage([0x06, 0x80, 0x00, 0x00, 0x00])
else:
self.sendMessage([0x06, 0x00, 0x00, 0x00, 0x00])
loadCount = (len(flashData) + pageSize - 1) / pageSize
for i in xrange(0, loadCount):
recv = self.sendMessage([0x13, pageSize >> 8, pageSize & 0xFF, 0xc1, 0x0a, 0x40, 0x4c, 0x20, 0x00, 0x00] + flashData[(i * pageSize):(i * pageSize + pageSize)])
if self.progressCallback != None:
self.progressCallback(i + 1, loadCount*2)
def verifyFlash(self, flashData):
#Set load addr to 0, in case we have more then 64k flash we need to enable the address extension
flashSize = self.chip['pageSize'] * 2 * self.chip['pageCount']
if flashSize > 0xFFFF:
self.sendMessage([0x06, 0x80, 0x00, 0x00, 0x00])
else:
self.sendMessage([0x06, 0x00, 0x00, 0x00, 0x00])
loadCount = (len(flashData) + 0xFF) / 0x100
for i in xrange(0, loadCount):
recv = self.sendMessage([0x14, 0x01, 0x00, 0x20])[2:0x102]
if self.progressCallback != None:
self.progressCallback(loadCount + i + 1, loadCount*2)
for j in xrange(0, 0x100):
if i * 0x100 + j < len(flashData) and flashData[i * 0x100 + j] != recv[j]:
raise ispBase.IspError('Verify error at: 0x%x' % (i * 0x100 + j))
def sendMessage(self, data):
message = struct.pack(">BBHB", 0x1B, self.seq, len(data), 0x0E)
for c in data:
message += struct.pack(">B", c)
checksum = 0
for c in message:
checksum ^= ord(c)
message += struct.pack(">B", checksum)
try:
self.serial.write(message)
self.serial.flush()
except SerialTimeoutException:
raise ispBase.IspError('Serial send timeout')
self.seq = (self.seq + 1) & 0xFF
return self.recvMessage()
def recvMessage(self):
state = 'Start'
checksum = 0
while True:
s = self.serial.read()
if len(s) < 1:
raise ispBase.IspError("Timeout")
b = struct.unpack(">B", s)[0]
checksum ^= b
#print(hex(b))
if state == 'Start':
if b == 0x1B:
state = 'GetSeq'
checksum = 0x1B
elif state == 'GetSeq':
state = 'MsgSize1'
elif state == 'MsgSize1':
msgSize = b << 8
state = 'MsgSize2'
elif state == 'MsgSize2':
msgSize |= b
state = 'Token'
elif state == 'Token':
if b != 0x0E:
state = 'Start'
else:
state = 'Data'
data = []
elif state == 'Data':
data.append(b)
if len(data) == msgSize:
state = 'Checksum'
elif state == 'Checksum':
if checksum != 0:
state = 'Start'
else:
return data
class Stk500v2(ispBase.IspBase):
def __init__(self):
self.serial = None
self.seq = 1
self.last_addr = -1
self.progress_callback = None
def connect(self, port = "COM22", speed = 115200):
if self.serial is not None:
self.close()
try:
self.serial = Serial(str(port), speed, timeout=1, writeTimeout=10000)
except SerialException:
raise ispBase.IspError("Failed to open serial port")
except:
raise ispBase.IspError("Unexpected error while connecting to serial port:" + port + ":" + str(sys.exc_info()[0]))
self.seq = 1
#Reset the controller
for n in range(0, 2):
self.serial.setDTR(True)
time.sleep(0.1)
self.serial.setDTR(False)
time.sleep(0.1)
time.sleep(0.2)
self.serial.flushInput()
self.serial.flushOutput()
if self.sendMessage([0x10, 0xc8, 0x64, 0x19, 0x20, 0x00, 0x53, 0x03, 0xac, 0x53, 0x00, 0x00]) != [0x10, 0x00]:
self.close()
raise ispBase.IspError("Failed to enter programming mode")
self.sendMessage([0x06, 0x80, 0x00, 0x00, 0x00])
if self.sendMessage([0xEE])[1] == 0x00:
self._has_checksum = True
else:
self._has_checksum = False
self.serial.timeout = 5
def close(self):
if self.serial is not None:
self.serial.close()
self.serial = None
#Leave ISP does not reset the serial port, only resets the device, and returns the serial port after disconnecting it from the programming interface.
# This allows you to use the serial port without opening it again.
def leaveISP(self):
if self.serial is not None:
if self.sendMessage([0x11]) != [0x11, 0x00]:
raise ispBase.IspError("Failed to leave programming mode")
ret = self.serial
self.serial = None
return ret
return None
def isConnected(self):
return self.serial is not None
def hasChecksumFunction(self):
return self._has_checksum
def sendISP(self, data):
recv = self.sendMessage([0x1D, 4, 4, 0, data[0], data[1], data[2], data[3]])
return recv[2:6]
def writeFlash(self, flash_data):
#Set load addr to 0, in case we have more then 64k flash we need to enable the address extension
page_size = self.chip["pageSize"] * 2
flash_size = page_size * self.chip["pageCount"]
Logger.log("d", "Writing flash")
if flash_size > 0xFFFF:
self.sendMessage([0x06, 0x80, 0x00, 0x00, 0x00])
else:
self.sendMessage([0x06, 0x00, 0x00, 0x00, 0x00])
load_count = (len(flash_data) + page_size - 1) / page_size
for i in range(0, int(load_count)):
self.sendMessage([0x13, page_size >> 8, page_size & 0xFF, 0xc1, 0x0a, 0x40, 0x4c, 0x20, 0x00, 0x00] + flash_data[(i * page_size):(i * page_size + page_size)])
if self.progress_callback is not None:
if self._has_checksum:
self.progress_callback(i + 1, load_count)
else:
self.progress_callback(i + 1, load_count * 2)
def verifyFlash(self, flash_data):
if self._has_checksum:
self.sendMessage([0x06, 0x00, (len(flash_data) >> 17) & 0xFF, (len(flash_data) >> 9) & 0xFF, (len(flash_data) >> 1) & 0xFF])
res = self.sendMessage([0xEE])
checksum_recv = res[2] | (res[3] << 8)
checksum = 0
for d in flash_data:
checksum += d
checksum &= 0xFFFF
if hex(checksum) != hex(checksum_recv):
raise ispBase.IspError("Verify checksum mismatch: 0x%x != 0x%x" % (checksum & 0xFFFF, checksum_recv))
else:
#Set load addr to 0, in case we have more then 64k flash we need to enable the address extension
flash_size = self.chip["pageSize"] * 2 * self.chip["pageCount"]
if flash_size > 0xFFFF:
self.sendMessage([0x06, 0x80, 0x00, 0x00, 0x00])
else:
self.sendMessage([0x06, 0x00, 0x00, 0x00, 0x00])
load_count = (len(flash_data) + 0xFF) / 0x100
for i in range(0, int(load_count)):
recv = self.sendMessage([0x14, 0x01, 0x00, 0x20])[2:0x102]
if self.progress_callback is not None:
self.progress_callback(load_count + i + 1, load_count * 2)
for j in range(0, 0x100):
if i * 0x100 + j < len(flash_data) and flash_data[i * 0x100 + j] != recv[j]:
raise ispBase.IspError("Verify error at: 0x%x" % (i * 0x100 + j))
def sendMessage(self, data):
message = struct.pack(">BBHB", 0x1B, self.seq, len(data), 0x0E)
for c in data:
message += struct.pack(">B", c)
checksum = 0
for c in message:
checksum ^= c
message += struct.pack(">B", checksum)
try:
self.serial.write(message)
self.serial.flush()
except SerialTimeoutException:
raise ispBase.IspError("Serial send timeout")
self.seq = (self.seq + 1) & 0xFF
return self.recvMessage()
def recvMessage(self):
state = "Start"
checksum = 0
while True:
s = self.serial.read()
if len(s) < 1:
raise ispBase.IspError("Timeout")
b = struct.unpack(">B", s)[0]
checksum ^= b
if state == "Start":
if b == 0x1B:
state = "GetSeq"
checksum = 0x1B
elif state == "GetSeq":
state = "MsgSize1"
elif state == "MsgSize1":
msg_size = b << 8
state = "MsgSize2"
elif state == "MsgSize2":
msg_size |= b
state = "Token"
elif state == "Token":
if b != 0x0E:
state = "Start"
else:
state = "Data"
data = []
elif state == "Data":
data.append(b)
if len(data) == msg_size:
state = "Checksum"
elif state == "Checksum":
if checksum != 0:
state = "Start"
else:
return data
Continuously read the serial port and process IO data received from a remote XBee.
"""
# This version is for the pi
import time
from xbee import XBee
from serial import Serial
ser = Serial('/dev/tty.usbserial-A7025WZ6', 57600)
#ser = Serial('/dev/xbee', 57600)
xbee = XBee(ser)
data = 1
# Continuously send
while data == 1:
try:
signal = ['up', 'down', 'right', 'left']
#print signal
#send = xbee.send("str(signal")
xbee.send("at", frame="A", command='MY', parameter=None)
ser.flush()
#send = ser.writelines("testing API\n")
time.sleep(.5)
data += 1
except KeyboardInterrupt:
break
ser.close()
class SuperSimon:
def __init__(self, configuration):
self.__conf = configuration
self._init_port()
self.__readDumpTimeout = 250 # ms
self.__magicTimeout = 250 # ms
self.players = []
self.__queue = EventQueue()
def _init_port(self):
self.__port = Serial(self.__conf.device, baudrate=9600)
def check_joins(self):
self.__queue.enqueue(self.__protocol_join_state)
def discover_clients(self):
self.__queue.enqueue(self.__protocol_discover)
def check_game_info(self, address):
self.__queue.enqueue(self.__protocol_game_info_request, [address])
def send_sequence(self, address, sequence):
self.__queue.enqueue(self.__protocol_send_sequence, [address, sequence])
def start_game(self, address):
self.__queue.enqueue(self.__protocol_start_game, [address])
def exit(self):
print("Stopping communication...")
self.__queue.stop()
self.__port.close()
def _find_or_create_player(self, address):
for player in self.players:
if player.address == address:
return player
player = Player(address)
self.players.append(player)
return player
@staticmethod
def _format_byte(b):
if b is None:
return '<None>'
return b.encode('hex')
# ==========================================================================
# BELOW HERE ARE THE PROTOCOL HELPERS / THREAD TARGETS
# ==========================================================================
# noinspection PyMethodMayBeStatic
def __protocol_request_turn(self):
return
def __protocol_end_turn(self):
self.__port.flush()
sleep(0.01) # Sleep for 10ms to allow things to calm down
return
def __protocol_discover(self):
self.__protocol_request_turn()
maximum_discover = 255
if self.__conf.discoverMaximum > -1:
maximum_discover = min(self.__conf.discoverMaximum, 255)
for addr in range(0, maximum_discover):
print("Sending discover to address " + str(addr) + "...")
discovered = self.__protocol_send_discover(addr)
player = self._find_or_create_player(addr)
player.online = discovered
# print("Address " + str(addr) + " discovered = " + str(discovered))
self.__protocol_end_turn()
def __protocol_join_state(self):
self.__protocol_request_turn()
for player in self.players:
if player.online and not player.joined:
print("Sending join state request to address " + str(player.address) + "...")
joined = self.__protocol_request_join_state(player.address)
if joined is None:
player.online = False
player.reset()
print("Address " + str(player.address) + " has been considered as offline")
else:
player.joined = joined
# print("Address " + str(player.address) + " joined = " + str(joined))
self.__protocol_end_turn()
def __protocol_game_info_request(self, address):
player = self._find_or_create_player(address)
if player.checkingGameInfo:
return
player.checkingGameInfo = True # Before protocol lockout to avoid duplicate locks
self.__protocol_request_turn()
print("Sending game info request to address " + str(address) + "...")
game_info = None
try:
game_info = self.__protocol_request_game_info(address)
except ValueError as e:
print(str(e))
player.online = False
player.reset()
print("Address " + str(address) + " has been considered as offline")
if game_info is not None:
player.lastGameInfo = game_info
player.roundCompleted = True
self.__protocol_end_turn()
player.checkingGameInfo = False
def __protocol_send_sequence(self, address, sequence):
self.__protocol_request_turn()
print("Sending sequence to address " + str(address) + "...")
player = self._find_or_create_player(address)
try:
self.__protocol_send_game_info(address, sequence)
except ValueError as e:
print(str(e))
player.online = False
player.reset()
print("Address " + str(address) + " has been considered offline")
self.__protocol_end_turn()
def __protocol_start_game(self, address):
self.__protocol_request_turn()
print("Sending start game...")
self.__protocol_send_start_game(address)
self.__protocol_end_turn()
# ==========================================================================
# BELOW HERE ARE THE PROTOCOL IMPLEMENTATIONS
# ==========================================================================
def __protocol_read(self, throw_ex=False):
v = self.__port.read()
if v == '':
if throw_ex:
raise ValueError("Failed to read from serial port: Timeout?")
return None
return v
def __protocol_send_magic(self):
sequence = '\xDE\xAD\xBE\xEF'
self.__port.write(sequence)
def __protocol_read_magic(self):
sequence = ['\xCA', '\xFE', '\xBA', '\xBE']
curr_index = 0
last_read = millis()
while curr_index < len(sequence):
b = self.__protocol_read()
if b is None:
raise ValueError('Could not read magic value: Timeout')
now = millis()
if (now - last_read) > self.__readDumpTimeout:
curr_index = 0
if b == sequence[curr_index]:
curr_index += 1
def __protocol_read_int(self):
byte_str = ""
byte_str += self.__protocol_read(True)
byte_str += self.__protocol_read(True)
byte_str += self.__protocol_read(True)
byte_str += self.__protocol_read(True)
return struct.unpack(">L", byte_str)[0]
def __protocol_read_short(self):
byte_str = ""
byte_str += self.__protocol_read(True)
byte_str += self.__protocol_read(True)
return struct.unpack(">H", byte_str)[0]
def __protocol_write_int(self, i):
self.__port.write(struct.pack(">I", i))
def __protocol_read_ack(self):
self.__protocol_read_magic()
b = self.__protocol_read()
if b != '\x00':
raise ValueError("Failed to read acknowledge: Invalid byte received (got " + self._format_byte(b) + ")")
def __protocol_read_join_response(self):
self.__protocol_read_magic()
b = self.__protocol_read()
if b == '\x07':
return False
elif b == '\x08':
return True
else:
raise ValueError("Failed to read join response: Invalid byte received (got " + self._format_byte(b) + ")")
def __protocol_read_game_info_request(self):
self.__protocol_read_magic()
b = self.__protocol_read()
if b == '\x04':
return None # No game info
elif b == '\x05':
# Has game information
# noinspection PyUnusedLocal
address = self.__protocol_read(True) # Ignored address - not important
length = self.__protocol_read_int()
i = 0
game_info = []
expecting_button = True
last_button = None
while i < length:
if expecting_button:
last_button = ord(self.__protocol_read(True))
expecting_button = False
i += 1 # Button is 1 byte
else:
ms = self.__protocol_read_short()
expecting_button = True
i += 2 # Shorts are 2 bytes
game_info.append(PressedButton(last_button, ms))
return game_info
else:
raise ValueError(
"Failed to read game information response: Invalid byte received (got " + self._format_byte(b) + ")")
def __protocol_send_discover(self, address):
self.__protocol_send_magic()
self.__port.write('\x09')
self.__port.write(chr(address))
prev_timeout = self.__port.timeout
self.__port.timeout = self.__magicTimeout / 1000.0
received = True
try:
self.__protocol_read_ack()
except ValueError as e:
print(str(e))
received = False
self.__port.timeout = prev_timeout
return received
def __protocol_request_join_state(self, address):
self.__protocol_send_magic()
self.__port.write('\x06')
self.__port.write(chr(address))
prev_timeout = self.__port.timeout
self.__port.timeout = self.__magicTimeout / 1000.0
try:
joining = self.__protocol_read_join_response()
except ValueError as e:
print(str(e))
joining = None
self.__port.timeout = prev_timeout
return joining
def __protocol_request_game_info(self, address):
self.__protocol_send_magic()
self.__port.write('\x03')
self.__port.write(chr(address))
prev_timeout = self.__port.timeout
self.__port.timeout = self.__magicTimeout / 1000.0
val = None
err = None
try:
val = self.__protocol_read_game_info_request()
except ValueError as e:
err = e
self.__port.timeout = prev_timeout
if err:
raise err
return val
def __protocol_send_game_info(self, address, sequence):
self.__protocol_send_magic()
self.__port.write('\x01')
self.__port.write(chr(address))
self.__protocol_write_int(len(sequence))
for i in sequence:
self.__port.write(chr(i))
prev_timeout = self.__port.timeout
self.__port.timeout = self.__magicTimeout / 1000.0
err = None
try:
self.__protocol_read_ack()
except ValueError as e:
err = e
self.__port.timeout = prev_timeout
if err is not None:
raise err
def __protocol_send_start_game(self, address):
self.__protocol_send_magic()
self.__port.write('\x02')
self.__port.write(chr(address))
class printcore():
def __init__(self, port = None, baud = None):
"""Initializes a printcore instance. Pass the port and baud rate to
connect immediately"""
self.baud = None
self.port = None
self.analyzer = GCodeAnalyzer()
self.printer = None # Serial instance connected to the printer,
# should be None when disconnected
self.clear = 0 # clear to send, enabled after responses
self.online = False # The printer has responded to the initial command
# and is active
self.printing = False # is a print currently running, true if printing
# , false if paused
self.mainqueue = None
self.priqueue = Queue(0)
self.queueindex = 0
self.lineno = 0
self.resendfrom = -1
self.paused = False
self.sentlines = {}
self.log = deque(maxlen = 10000)
self.sent = []
self.writefailures = 0
self.tempcb = None # impl (wholeline)
self.recvcb = None # impl (wholeline)
self.sendcb = None # impl (wholeline)
self.preprintsendcb = None # impl (wholeline)
self.printsendcb = None # impl (wholeline)
self.layerchangecb = None # impl (wholeline)
self.errorcb = None # impl (wholeline)
self.startcb = None # impl ()
self.endcb = None # impl ()
self.onlinecb = None # impl ()
self.loud = False # emit sent and received lines to terminal
self.greetings = ['start', 'Grbl ']
self.wait = 0 # default wait period for send(), send_now()
self.read_thread = None
self.stop_read_thread = False
self.send_thread = None
self.stop_send_thread = False
self.print_thread = None
if port is not None and baud is not None:
self.connect(port, baud)
self.xy_feedrate = None
self.z_feedrate = None
self.pronterface = None
@locked
def disconnect(self):
"""Disconnects from printer and pauses the print
"""
if self.printer:
if self.read_thread:
self.stop_read_thread = True
self.read_thread.join()
self.read_thread = None
if self.print_thread:
self.printing = False
self.print_thread.join()
self._stop_sender()
try:
self.printer.close()
except socket.error:
pass
except OSError:
pass
self.printer = None
self.online = False
self.printing = False
@locked
def connect(self, port = None, baud = None):
"""Set port and baudrate if given, then connect to printer
"""
if self.printer:
self.disconnect()
if port is not None:
self.port = port
if baud is not None:
self.baud = baud
if self.port is not None and self.baud is not None:
# Connect to socket if "port" is an IP, device if not
host_regexp = re.compile("^(([0-9]|[1-9][0-9]|1[0-9]{2}|2[0-4][0-9]|25[0-5])\.){3}([0-9]|[1-9][0-9]|1[0-9]{2}|2[0-4][0-9]|25[0-5])$|^(([a-zA-Z0-9]|[a-zA-Z0-9][a-zA-Z0-9\-]*[a-zA-Z0-9])\.)*([A-Za-z0-9]|[A-Za-z0-9][A-Za-z0-9\-]*[A-Za-z0-9])$")
is_serial = True
if ":" in port:
bits = port.split(":")
if len(bits) == 2:
hostname = bits[0]
try:
port = int(bits[1])
if host_regexp.match(hostname) and 1 <= port <= 65535:
is_serial = False
except:
pass
self.writefailures = 0
if not is_serial:
self.printer_tcp = socket.socket(socket.AF_INET,
socket.SOCK_STREAM)
self.timeout = 0.25
self.printer_tcp.settimeout(1.0)
try:
self.printer_tcp.connect((hostname, port))
self.printer_tcp.settimeout(self.timeout)
self.printer = self.printer_tcp.makefile()
except socket.error as e:
print _("Could not connect to %s:%s:") % (hostname, port)
self.printer = None
self.printer_tcp = None
print _("Socket error %s:") % e.errno,
print e.strerror
return
else:
disable_hup(self.port)
self.printer_tcp = None
try:
self.printer = Serial(port = self.port,
baudrate = self.baud,
timeout = 0.25)
except SerialException as e:
print _("Could not connect to %s at baudrate %s:") % (self.port, self.baud)
self.printer = None
print _("Serial error: %s") % e
return
self.stop_read_thread = False
self.read_thread = Thread(target = self._listen)
self.read_thread.start()
self._start_sender()
def reset(self):
"""Reset the printer
"""
if self.printer and not self.printer_tcp:
self.printer.setDTR(1)
time.sleep(0.2)
self.printer.setDTR(0)
def _readline(self):
try:
try:
line = self.printer.readline()
if self.printer_tcp and not line:
raise OSError(-1, "Read EOF from socket")
except socket.timeout:
return ""
if len(line) > 1:
self.log.append(line)
if self.recvcb:
try: self.recvcb(line)
except: pass
if self.loud: print "RECV:", line.rstrip()
return line
except SelectError as e:
if 'Bad file descriptor' in e.args[1]:
print _(u"Can't read from printer (disconnected?) (SelectError {0}): {1}").format(e.errno, decode_utf8(e.strerror))
return None
else:
print _(u"SelectError ({0}): {1}").format(e.errno, decode_utf8(e.strerror))
raise
except SerialException as e:
print _(u"Can't read from printer (disconnected?) (SerialException): {0}").format(decode_utf8(str(e)))
return None
except socket.error as e:
print _(u"Can't read from printer (disconnected?) (Socket error {0}): {1}").format(e.errno, decode_utf8(e.strerror))
return None
except OSError as e:
if e.errno == errno.EAGAIN: # Not a real error, no data was available
return ""
print _(u"Can't read from printer (disconnected?) (OS Error {0}): {1}").format(e.errno, e.strerror)
return None
def _listen_can_continue(self):
if self.printer_tcp:
return not self.stop_read_thread and self.printer
return (not self.stop_read_thread
and self.printer
and self.printer.isOpen())
def _listen_until_online(self):
while not self.online and self._listen_can_continue():
self._send("M105")
if self.writefailures >= 4:
print _("Aborting connection attempt after 4 failed writes.")
return
empty_lines = 0
while self._listen_can_continue():
line = self._readline()
if line is None: break # connection problem
# workaround cases where M105 was sent before printer Serial
# was online an empty line means read timeout was reached,
# meaning no data was received thus we count those empty lines,
# and once we have seen 15 in a row, we just break and send a
# new M105
# 15 was chosen based on the fact that it gives enough time for
# Gen7 bootloader to time out, and that the non received M105
# issues should be quite rare so we can wait for a long time
# before resending
if not line:
empty_lines += 1
if empty_lines == 15: break
else: empty_lines = 0
if line.startswith(tuple(self.greetings)) \
or line.startswith('ok') or "T:" in line:
if self.onlinecb:
try: self.onlinecb()
except: pass
self.online = True
return
def _listen(self):
"""This function acts on messages from the firmware
"""
self.clear = True
if not self.printing:
self._listen_until_online()
while self._listen_can_continue():
line = self._readline()
if line is None:
break
if line.startswith('DEBUG_'):
continue
if line.startswith(tuple(self.greetings)) or line.startswith('ok'):
self.clear = True
if line.startswith('ok') and "T:" in line and self.tempcb:
#callback for temp, status, whatever
try: self.tempcb(line)
except: pass
elif line.startswith('Error'):
if self.errorcb:
#callback for errors
try: self.errorcb(line)
except: pass
# Teststrings for resend parsing # Firmware exp. result
# line="rs N2 Expected checksum 67" # Teacup 2
if line.lower().startswith("resend") or line.startswith("rs"):
for haystack in ["N:", "N", ":"]:
line = line.replace(haystack, " ")
linewords = line.split()
while len(linewords) != 0:
try:
toresend = int(linewords.pop(0))
self.resendfrom = toresend
#print str(toresend)
break
except:
pass
self.clear = True
self.clear = True
def _start_sender(self):
self.stop_send_thread = False
self.send_thread = Thread(target = self._sender)
self.send_thread.start()
def _stop_sender(self):
if self.send_thread:
self.stop_send_thread = True
self.send_thread.join()
self.send_thread = None
def _sender(self):
while not self.stop_send_thread:
try:
command = self.priqueue.get(True, 0.1)
except QueueEmpty:
continue
while self.printer and self.printing and not self.clear:
time.sleep(0.001)
self._send(command)
while self.printer and self.printing and not self.clear:
time.sleep(0.001)
def _checksum(self, command):
return reduce(lambda x, y: x ^ y, map(ord, command))
def startprint(self, gcode, startindex = 0):
"""Start a print, gcode is an array of gcode commands.
returns True on success, False if already printing.
The print queue will be replaced with the contents of the data array,
the next line will be set to 0 and the firmware notified. Printing
will then start in a parallel thread.
"""
if self.printing or not self.online or not self.printer:
return False
self.printing = True
self.mainqueue = gcode
self.lineno = 0
self.queueindex = startindex
self.resendfrom = -1
self._send("M110", -1, True)
if not gcode.lines:
return True
self.clear = False
resuming = (startindex != 0)
self.print_thread = Thread(target = self._print,
kwargs = {"resuming": resuming})
self.print_thread.start()
return True
# run a simple script if it exists, no multithreading
def runSmallScript(self, filename):
if filename is None: return
f = None
try:
with open(filename) as f:
for i in f:
l = i.replace("\n", "")
l = l[:l.find(";")] # remove comments
self.send_now(l)
except:
pass
def pause(self):
"""Pauses the print, saving the current position.
"""
if not self.printing: return False
self.paused = True
self.printing = False
# try joining the print thread: enclose it in try/except because we
# might be calling it from the thread itself
try:
self.print_thread.join()
except:
pass
self.print_thread = None
# saves the status
self.pauseX = self.analyzer.x - self.analyzer.xOffset
self.pauseY = self.analyzer.y - self.analyzer.yOffset
self.pauseZ = self.analyzer.z - self.analyzer.zOffset
self.pauseE = self.analyzer.e - self.analyzer.eOffset
self.pauseF = self.analyzer.f
self.pauseRelative = self.analyzer.relative
def resume(self):
"""Resumes a paused print.
"""
if not self.paused: return False
if self.paused:
# restores the status
self.send_now("G90") # go to absolute coordinates
xyFeedString = ""
zFeedString = ""
if self.xy_feedrate is not None:
xyFeedString = " F" + str(self.xy_feedrate)
if self.z_feedrate is not None:
zFeedString = " F" + str(self.z_feedrate)
self.send_now("G1 X%s Y%s%s" % (self.pauseX, self.pauseY,
xyFeedString))
self.send_now("G1 Z" + str(self.pauseZ) + zFeedString)
self.send_now("G92 E" + str(self.pauseE))
# go back to relative if needed
if self.pauseRelative: self.send_now("G91")
# reset old feed rate
self.send_now("G1 F" + str(self.pauseF))
self.paused = False
self.printing = True
self.print_thread = Thread(target = self._print,
kwargs = {"resuming": True})
self.print_thread.start()
def send(self, command, wait = 0):
"""Adds a command to the checksummed main command queue if printing, or
sends the command immediately if not printing"""
if self.online:
if self.printing:
self.mainqueue.append(command)
else:
self.priqueue.put_nowait(command)
else:
print "Not connected to printer."
def send_now(self, command, wait = 0):
"""Sends a command to the printer ahead of the command queue, without a
checksum"""
if self.online:
self.priqueue.put_nowait(command)
else:
print "Not connected to printer."
def _print(self, resuming = False):
self._stop_sender()
try:
if self.startcb:
#callback for printing started
try: self.startcb(resuming)
except:
print "Print start callback failed with:"
traceback.print_exc(file = sys.stdout)
while self.printing and self.printer and self.online:
self._sendnext()
self.sentlines = {}
self.log.clear()
self.sent = []
if self.endcb:
#callback for printing done
try: self.endcb()
except:
print "Print end callback failed with:"
traceback.print_exc(file = sys.stdout)
except:
print "Print thread died due to the following error:"
traceback.print_exc(file = sys.stdout)
finally:
self.print_thread = None
self._start_sender()
#now only "pause" is implemented as host command
def processHostCommand(self, command):
command = command.lstrip()
if command.startswith(";@pause"):
if self.pronterface is not None:
self.pronterface.pause(None)
else:
self.pause()
def _sendnext(self):
if not self.printer:
return
while self.printer and self.printing and not self.clear:
time.sleep(0.001)
self.clear = False
if not (self.printing and self.printer and self.online):
self.clear = True
return
if self.resendfrom < self.lineno and self.resendfrom > -1:
self._send(self.sentlines[self.resendfrom], self.resendfrom, False)
self.resendfrom += 1
return
self.resendfrom = -1
if not self.priqueue.empty():
self._send(self.priqueue.get_nowait())
self.priqueue.task_done()
return
if self.printing and self.queueindex < len(self.mainqueue):
(layer, line) = self.mainqueue.idxs(self.queueindex)
gline = self.mainqueue.all_layers[layer][line]
if self.layerchangecb and self.queueindex > 0:
(prev_layer, prev_line) = self.mainqueue.idxs(self.queueindex - 1)
if prev_layer != layer:
try: self.layerchangecb(layer)
except: traceback.print_exc()
if self.preprintsendcb:
if self.queueindex + 1 < len(self.mainqueue):
(next_layer, next_line) = self.mainqueue.idxs(self.queueindex + 1)
next_gline = self.mainqueue.all_layers[next_layer][next_line]
else:
next_gline = None
gline = self.preprintsendcb(gline, next_gline)
if gline is None:
self.queueindex += 1
self.clear = True
return
tline = gline.raw
if tline.lstrip().startswith(";@"): # check for host command
self.processHostCommand(tline)
self.queueindex += 1
self.clear = True
return
tline = tline.split(";")[0]
if len(tline) > 0:
self._send(tline, self.lineno, True)
self.lineno += 1
if self.printsendcb:
try: self.printsendcb(gline)
except: traceback.print_exc()
else:
self.clear = True
self.queueindex += 1
else:
self.printing = False
self.clear = True
if not self.paused:
self.queueindex = 0
self.lineno = 0
self._send("M110", -1, True)
def _send(self, command, lineno = 0, calcchecksum = False):
if calcchecksum:
prefix = "N" + str(lineno) + " " + command
command = prefix + "*" + str(self._checksum(prefix))
if "M110" not in command:
self.sentlines[lineno] = command
if self.printer:
self.sent.append(command)
# run the command through the analyzer
try: self.analyzer.Analyze(command)
except:
print "Warning: could not analyze command %s:" % command
traceback.print_exc(file = sys.stdout)
if self.loud:
print "SENT:", command
if self.sendcb:
try: self.sendcb(command)
except: pass
try:
self.printer.write(str(command + "\n"))
if self.printer_tcp: self.printer.flush()
self.writefailures = 0
except socket.error as e:
print _(u"Can't write to printer (disconnected?) (Socket error {0}): {1}").format(e.errno, decode_utf8(e.strerror))
self.writefailures += 1
except SerialException as e:
print _(u"Can't write to printer (disconnected?) (SerialException): {0}").format(decode_utf8(str(e)))
self.writefailures += 1
except RuntimeError as e:
print _(u"Socket connection broken, disconnected. ({0}): {1}").format(e.errno, decode_utf8(e.strerror))
self.writefailures += 1
class CopyNES(object):
'''
Interface to a CopyNES device
'''
###########################################################################
##
## Status operations
##
###########################################################################
def __init__(self, data_device, control_device):
'''
Connects to a USB CopyNES
'''
self.data_channel = Serial(data_device, 115200, timeout = 5)
self.control_channel = Serial(control_device, 115200, timeout = 5)
# Empty the read buffer
time.sleep(0.1)
self.data_channel.flushInput()
def disconnect(self):
'''
Disconnects from the CopyNES device and frees up all resources
'''
self.data_channel.close()
self.control_channel.close()
def power(self):
'''
Returns True if the CopyNES is powered on
'''
return not self.control_channel.getCD()
def reset(self):
'''
Resets the CopyNES CPU.
In "play" mode, this runs the cartridge program
In "copy" mode, the CopyNES will be waiting for commands
'''
self.control_channel.setDTR(False)
self.control_channel.setDTR(True)
def play_mode(self):
'''
Resets the CopyNES and puts it in "play" mode
This disables the BIOS, so no I/O operations will be available
until "copy" mode is enabled again
'''
self.control_channel.setRTS(False)
self.reset()
def copy_mode(self):
'''
Resets the CopyNES and puts it in "copy" mode
'''
self.control_channel.setRTS(True)
self.reset()
###########################################################################
##
## IO operations
##
###########################################################################
def write(self, data):
'''
Writes a sequence of bytes to the CopyNES
'''
if isinstance(data, int):
data = bytes((data,))
result = self.data_channel.write(data)
self.data_channel.flush()
return result
def wait_for_data(self, timeout):
'''
Wait until data is available for reading
'''
while self.data_channel.inWaiting() == 0 and timeout > 0:
time.sleep(0.1)
timeout -= 0.1 # XXX Not very exact method of waiting
return self.data_channel.inWaiting() > 0
def read(self, size = 1):
'''
Reads a sequence of bytes transferred from the CopyNES
'''
if size == 0:
return
result = self.data_channel.read(size)
if len(result) != size:
raise Exception("timeout")
return result
def read_int(self):
'''
Reads a single byte transferred from the CopyNES
'''
return int(self.read()[0])
def read_string(self):
'''
Reads a zero terminated string
'''
result = bytearray()
byte = self.read()
while ord(byte) != 0x00:
result += byte
byte = self.read()
return result.decode()
def flush(self):
self.data_channel.flush()
###########################################################################
##
## BIOS operations
##
###########################################################################
def version_string(self):
'''
Returns the CopyNES firmware version string
'''
self.write(0xa1)
return self.read_string()
def version(self):
'''
Returns the CopyNES firmware version number
'''
self.write(0xa2)
return self.read_int()
def __send_command(self, prolog, address, length, epilog):
'''
Send a CopyNES command
'''
address_lsb = (address & 0x00ff)
address_msb = (address & 0xff00) >> 8
length_lsb = (length & 0x00ff)
length_msb = (length & 0xff00) >> 8
if length_lsb != 0x00:
raise Exception('Memory may only be read/written in $0100 byte chunks, you are trying with a $%04x byte chunk' % length)
self.write(prolog)
self.write(address_lsb)
self.write(address_msb)
self.write(length_msb)
self.write(epilog)
def read_cpu_memory(self, address, length):
self.__send_command(0x3a, address, length, 0xa3)
return self.read(length)
def write_cpu_memory(self, address, data):
self.__send_command(0x4b, address, len(data), 0xb4)
return self.write(data)
def execute_code(self, address):
self.__send_command(0x7e, address, 0x00, 0xe7)
###########################################################################
##
## Convenience methods
##
###########################################################################
def run_plugin(self, plugin):
self.write_cpu_memory(0x0400, plugin.data)
self.execute_code(0x0400)
def download_rom(self, plugin, ines_mapper):
##
## Packet types sent by dumping plugins
##
PACKET_EOD = 0
PACKET_PRG_ROM = 1
PACKET_CHR_ROM = 2
PACKET_WRAM = 3
PACKET_RESET = 4
self.run_plugin(plugin)
prom = crom = wram = []
mirroring = self.read_int() ^ 0x01
(packet_type, data) = self.__read_packet()
while packet_type != PACKET_EOD:
if packet_type == PACKET_PRG_ROM:
prom = data
elif packet_type == PACKET_CHR_ROM:
crom = data
elif packet_type == PACKET_WRAM:
wram = data
elif packet_type == PACKET_RESET:
self.reset()
else:
raise Exception('Unexpected packet type: %d' % packet_type)
(packet_type, data) = self.__read_packet()
logging.debug('Downloaded prg: %d bytes, chr: %d bytes, wram: %d bytes, mirroring: %d, mapper: %d', len(prom), len(crom), len(wram), mirroring, ines_mapper)
return NESROM(prom, crom, ines_mapper, mirroring)
def __read_packet(self):
pages = self.read_int() | (self.read_int() << 8)
length = pages << 8
packet_type = self.read_int()
if packet_type == 0:
return (packet_type, 0)
data = self.read(length)
return (packet_type, data)
@staticmethod
def default_data_device():
if platform.system() == 'Windows':
return 'COM3'
elif platform.system() == 'Darwin':
serial_devices = glob.glob('/dev/tty.usbserial-*')
return serial_devices[0] if len(serial_devices) >= 2 else '?'
else:
return '/dev/ttyUSB0'
@staticmethod
def default_control_device():
if platform.system() == 'Windows':
return 'COM4'
elif platform.system() == 'Darwin':
serial_devices = glob.glob('/dev/tty.usbserial-*')
return serial_devices[1] if len(serial_devices) >= 2 else '?'
else:
return '/dev/ttyUSB1'
class S128P(object):
def __init__(self):
self.debug = True
print("Init lock.")
self.lock = Lock()
print("init s128p")
self.port = Serial('/dev/ttyAudio', 19200, timeout=1.0) #0.2s timeout from protocol doc
if self.is_connected():
print("connected ok!")
else:
print("not connected.")
print("init outputs")
self.outputs = [Output(i) for i in range(1, self.get_output_count()+1)]
print("Doing initial download of output data")
self.refresh_output_data()
print("outputs done")
print("init inputs")
self.inputs = [Input(i) for i in range(1, self.get_input_count()+1)]
print("Doing initial download of input data")
self.refresh_input_data()
print("inputs done")
def send(self, command):
self.lock.acquire()
if self.debug: print("got lock for", command)
x = "&S12," + command
result = ""
ch = None
while ch == '' or ch == None:
#we have not received a character. We need to send the command.
if self.debug: print("sending: " + x)
self.port.write(x + '\r')
self.port.flush()
self.port.reset_input_buffer()
if self.debug: print("waiting")
ch = self.port.read()
while ch != '\r':
result += ch
ch = self.port.read()
#Now you would think we would lose the last character here, because
#I just read it but didnt push it onto the string.
#But I have some good news for you: the last character is always \r.
if self.debug: print(" got: " + result)
if self.debug: print("Releasing lock for", command)
self.lock.release()
try:
result = result.split(',', 1)
return result[1]
except:
print("Bogus shit returned:", result)
return ""
def get_local_source_detect(self):
resp = self.send("LSD,1?")
resp = resp.split(',', 2)
return resp[2]
def get_output_count(self):
return len(self.get_local_source_detect())
def get_input_detect(self):
resp = self.send("ASD,1?")
resp = resp.split(',', 2)
return resp[2]
def get_input_count(self):
return len(self.get_input_detect())
def refresh_output_data(self):
try:
for output in self.outputs:
output.input = self.get_selected_input(output.id)
output.volume = self.get_volume(output.id)
except IndexError:
print("Output refresh failed!")
def refresh_input_data(self):
try:
detect = list(self.get_input_detect())
if self.debug: print(detect)
for input in self.inputs:
input.active = bool(int(detect[input.id-1]))
except IndexError:
print("Output refresh failed!")
# this fetches the input of a particular OUTPUT.
# this does NOT get, e.g. input 3 -- it gets the
# input connected to output 3, if any.
def get_selected_input(self, output_id):
result = self.send("SRC," + str(output_id).zfill(2) + "?")
result = result.split(',')
temp = int(result[2])
if temp == 0:
temp = None
return temp
def set_input(self, output_id, input_id):
result = self.send("SRC," + str(output_id).zfill(2) + ',' + str(input_id if input_id != None else 0).zfill(2))
if result[0:3] == "ACK":
self.outputs[output_id-1].input = input_id
else:
print("didn't get ack. :( got:", result[0:3])
try:
self.set_volume(output_id, self.outputs[output_id-1].default_volume)
except:
print("Failed to set volume on output", output_id)
def get_volume(self, output_id):
result = self.send("VOL," + str(output_id).zfill(2) + "?")
result = result.split(',')
return int(result[2])
def set_volume(self, output_id, volume):
print("Setting", output_id, "to volume", volume)
result = self.send("VOL," + str(output_id).zfill(2) + ',' + str(volume))
print(result)
return result
def is_connected(self):
return "SYSOFF" in self.send("SYSOFF?")
class BLE112:
def __init__(self, devpath=''):
self.devpath = devpath
self.hw_addr = ''
self.baud = 115200
self.serialdev = None
self.isopen = False
self.devgood = False
self.txpower = 0
def setMode(self, disc, connect):
print('Setting BLE112 Mode...')
# payload is [GAP discoverable mode, GAP connectable mode]
self.createAndSendCmdPacket(CID_GAP, CMD_SETMODE, [disc, connect])
def setAdvData(self, data):
print('Setting Adv data...')
if len(data) > 31:
print(' > data is longer than 31 bytes, aborting!')
return
payload = [ADV_ADVDATA]
payload.extend(data)
self.createAndSendCmdPacket(CID_GAP, CMD_ADVDATA, payload)
def constructAdvPacket(self, advTypes, payload):
# length of advertisement types
len_types = len(advTypes)
# length of payload
len_payload = len(payload)
# total packet length
len_total = 2 + len_types + len_payload
# check to make sure length doesn't exceed 31
if len_total > 31:
print(' Error: advertisement packet exceeds 31 bytes')
return None
# construct packet
packet = bytearray([len_total, len_types])
packet.extend(advTypes)
packet.extend([len_payload])
packet.extend(payload)
#print('adv payload = ' + ''.join('{:02x} '.format(x) for x in packet))
return packet
def setAdvRate(self, desired_rate_hz):
print('Setting BLE112 Adv. Rate...')
# make sure rate is in good range (20ms to 10s)
if desired_rate_hz > 20:
print(' > requested rate too high, setting to 20 Hz')
desired_rate_hz = 20
if desired_rate_hz < 0.10:
print(' > requested rate too low, setting to 0.10 Hz')
desired_rate_hz = 0.10
# convert desired rate to byte-value (0x20 (20ms) to 0x4000 (10+s) in steps of 625us)
desired_interval_ms = round((1e3*1.0/desired_rate_hz))
desired_interval_ticks = round(desired_interval_ms/0.625)
# rate is specified by desired +/- 30ms
int_ticks_min = desired_interval_ticks-48
int_ticks_max = desired_interval_ticks+48
# create payload (bytes are little endian!)
min_bytes = struct.pack('<H', int_ticks_min)
max_bytes = struct.pack('<H', int_ticks_max)
payload = bytearray()
payload.extend(min_bytes)
payload.extend(max_bytes)
payload.extend([ADV_CHANNELS_ALL])
# payload is: [adv_interval_min, adv_interval_max, adv_channels]
self.createAndSendCmdPacket(CID_GAP, CMD_ADVPARA, payload)
def setTxPower(self, power):
self.createAndSendCmdPacket(CID_HW, CMD_TXPOWER, [power])
self.txpower = power
def setBeaconParams(self, UUID, major, minor):
# construct adv data array (adv. types, custom adv. payload)
payload = IBCN_PREFIX
payload.extend(UUID)
# make major and minor into 2 byte arrays
payload.extend(struct.pack('!H', major))
payload.extend(struct.pack('!H', minor))
# determine calibrated transmit power
payload.extend([ TXPOW_1M_ARRAY[self.txpower] ])
advpkt = self.constructAdvPacket(IBCN_TYPES, payload)
self.setAdvData(advpkt)
def enableAdv(self):
self.setMode(GAP_USER_DATA, GAP_NON_CONNECTABLE)
def disableAdv(self):
self.setMode(GAP_NON_DISCOVERABLE, GAP_NON_CONNECTABLE)
def autoSetup(self):
# what OS are we using?
local_os = platform.system()
if local_os == OSTYPE_LINUX:
print('Auto-detecting BLE112 (Linux)...')
devlist = os.listdir('/dev')
ble_regex = '.*ttyUSB.*'
elif local_os == OSTYPE_OSX:
print('Auto-detecting BLE112 (OSX)...')
devlist = os.listdir('/dev')
ble_regex = '.*tty.usbserial.*'
else:
print('Error: unrecognized host OS')
return
candidates = []
for dev in devlist:
check = re.match(ble_regex, dev)
if check is not None:
candidates.append(dev)
if len(candidates) is 0:
print(' > no devices found, aborting.')
return NOSUCCESS
print(' > found ' + str(len(candidates)) + ' device(s)')
self.devpath = '/dev/' + candidates[0]
print(' > opening device: ' + self.devpath)
# open device
try:
self.serialdev = Serial(self.devpath, baudrate=self.baud, timeout=30e-3)
self.isopen = True
except Error:
print(' > Error: unable to open device')
return NOSUCCESS
# fire up device listener thread
self.listener = threading.Thread(target=self.listen)
self.listener.start()
# check to make sure the BLE device is functioning ok
print(' > checking BLE health')
self.probeDevHealth()
if self.devgood is False:
print(' > BLE Device has not issued "OK" signal, aborting.')
return NOSUCCESS
print(' BLE device is "OK"')
print(' > determining device MAC')
# probe device mac
self.probeDevAddr()
print(' Device MAC: ' + ''.join('{:02x} '.format(x) for x in self.hw_addr))
# set starting TX Power
self.setTxPower(TXPOW_HIGH)
print(' > setting starting TX Power to HIGH...')
# configure the LED pins
self.configureLeds()
# success!
return SUCCESS
def close(self):
self.serialdev.close()
def configureLeds(self):
# set IO direction to output (port 1, pins 0 and 1)
self.createAndSendCmdPacket(CID_HW, CMD_IODIR, [LED_PORT, (1<<LED_PIN_GREEN)+(1<<LED_PIN_RED)])
def setGreenLed(self, val):
self.createAndSendCmdPacket(CID_HW, CMD_IOWRITE, [LED_PORT, (1<<LED_PIN_GREEN), (val<<LED_PIN_GREEN)])
def setRedLed(self, val):
self.createAndSendCmdPacket(CID_HW, CMD_IOWRITE, [LED_PORT, (1<<LED_PIN_RED), (val<<LED_PIN_RED)])
def probeDevHealth(self):
pkt = self.createAndSendCmdPacket(CID_SYSTEM, CMD_HELLO, None)
def probeDevAddr(self):
self.createAndSendCmdPacket(CID_SYSTEM, CMD_GETADDR, None)
def createCmdPacket(self, classID, cmdID, payload):
# message type (0 = cmd/resp)
MT = 0
# technology Type (0 = BTSmart)
TT = 0
# Payload length
if payload is None:
PLL = 0
else:
PLL = len(payload)
# PLL High (3 bits)
LH = (PLL>>8)&0xFF
# PLL Low (8 bits)
LL = PLL&0xFF
# Class ID
CID = classID
# Command ID
CMD = cmdID
# Payload
PL = payload
# -- create header array
header = [(MT<<7)+(TT<<3)+(LH), LL, CID, CMD]
# -- packet array
if PL is not None:
header.extend(PL)
packet = bytes(header)
return packet
def unpackPacket(self, packet):
# message type
MT = (packet[0]>>7)&0x01
# Command class ID
CID = packet[2]
# Command ID
CMD = packet[3]
# Payload
if len(packet) > 4:
PL = packet[4:]
else:
PL = None
return [MT, CID, CMD, PL]
def sendPacket(self, packet):
if self.isopen is not True:
return
self.serialdev.write(packet)
def createAndSendCmdPacket(self, classID, cmdID, payload):
pkt = self.createCmdPacket(classID, cmdID, payload)
self.sendPacket(pkt)
print(' >> SENDING: ' + ''.join('{:02x} '.format(x) for x in pkt))
time.sleep(CMD_WAIT_DEFAULT)
def handleIncomingPacket(self, raw):
[MT, CID, CMD, PL] = self.unpackPacket(raw)
PL_str = 'None'
if PL is not None:
PL_str = ''.join('{:02x} '.format(x) for x in PL)
print(' << RECEIVED: MT = %d, CID = %d, CMD = %d, PL = %s' % (MT, CID, CMD, PL_str))
# handle address probe responses
if CID is CID_SYSTEM and CMD is CMD_GETADDR:
self.hw_addr = PL
return
# handle health probe responses
if CID is CID_SYSTEM and CMD is CMD_HELLO:
self.devgood = True
return
# handle parsing errors
if MT is TYPE_EVENT and CID is CID_SYSTEM and CMD is EVT_PROTOERR:
print(' ! BLE issued protocol error!')
# unhandled packets get here
#print(' (ignoring incoming packet from BLE)')
# to be threaded
def listen(self):
while True:
# is there any serial data waiting?
if self.serialdev.inWaiting() > 0:
rawbytes = self.serialdev.read(1024)
# check length to ensure packet is not malformed
if len(rawbytes) < 4:
self.serialdev.flush()
continue
PL_len_high = rawbytes[0]&0x07
PL_len_low = rawbytes[1]
PL_len = int(PL_len_high<<7) + int(PL_len_low)
if len(rawbytes) is not 4 + PL_len:
self.serialdev.flush()
continue
# hand off the packet
self.handleIncomingPacket(rawbytes)
class DfuTransportSerial():
DEFAULT_BAUD_RATE = 57600
DEFAULT_FLOW_CONTROL = False
DEFAULT_SERIAL_PORT_TIMEOUT = 1.0 # Timeout time on serial port read
ACK_PACKET_TIMEOUT = 1.0 # Timeout time for for ACK packet received before reporting timeout through event system
SEND_INIT_PACKET_WAIT_TIME = 1.0 # Time to wait before communicating with bootloader after init packet is sent
SEND_START_DFU_WAIT_TIME = 2.0 # Time to wait before communicating with bootloader after start DFU packet is sent ## was 10 sec
DFU_PACKET_MAX_SIZE = 512 # The DFU packet max size
def __init__(self, com_port, baud_rate=DEFAULT_BAUD_RATE, flow_control=DEFAULT_FLOW_CONTROL, timeout=DEFAULT_SERIAL_PORT_TIMEOUT):
self.com_port = com_port
self.baud_rate = baud_rate
self.flow_control = 1 if flow_control else 0
self.timeout = timeout
self.serial_port = None
""":type: serial.Serial """
def open(self):
self.serial_port = Serial(port=self.com_port, baudrate=self.baud_rate, rtscts=self.flow_control, timeout=self.timeout)
self.serial_port.flushInput()
self.serial_port.flushOutput()
self.serial_port.flush()
def close(self):
self.serial_port.close()
def is_open(self):
if self.serial_port is None:
return False
return self.serial_port.isOpen()
def send_validate_firmware(self):
super(DfuTransportSerial, self).send_validate_firmware()
return True
def send_init_packet(self,firmware):
# super(DfuTransportSerial, self).send_init_packet(init_packet)
init_packet = 'ffffffffffffffff0100feff'
init_packet += binascii.hexlify(struct.pack('<H', crc16.calc_crc16(firmware, crc=0xffff)))
init_packet = binascii.unhexlify(init_packet)
frame = int32_to_bytes(DFU_INIT_PACKET)
frame += init_packet
frame += int16_to_bytes(0x0000) # Padding required
packet = HciPacket(frame)
self.send_packet(packet)
time.sleep(DfuTransportSerial.SEND_INIT_PACKET_WAIT_TIME)
def send_start_dfu(self, mode, softdevice_size=None, bootloader_size=None, app_size=None):
# super(DfuTransportSerial, self).send_start_dfu(mode, softdevice_size, bootloader_size, app_size)
frame = int32_to_bytes(DFU_START_PACKET)
frame += int32_to_bytes(mode)
frame += self.create_image_size_packet(softdevice_size, bootloader_size, app_size)
packet = HciPacket(frame)
self.send_packet(packet)
time.sleep(DfuTransportSerial.SEND_START_DFU_WAIT_TIME)
def send_activate_firmware(self):
super(DfuTransportSerial, self).send_activate_firmware()
def send_firmware(self, firmware):
# super(DfuTransportSerial, self).send_firmware(firmware)
def progress_percentage(part, whole):
return int(100 * float(part)/float(whole))
frames = []
# self._send_event(DfuEvent.PROGRESS_EVENT, progress=0, done=False, log_message="")
for i in range(0, len(firmware), DfuTransportSerial.DFU_PACKET_MAX_SIZE):
data_packet = HciPacket(int32_to_bytes(DFU_DATA_PACKET) + firmware[i:i + DfuTransportSerial.DFU_PACKET_MAX_SIZE])
frames.append(data_packet)
frames_count = len(frames)
# Send firmware packets
for count, pkt in enumerate(frames):
self.send_packet(pkt)
# self._send_event(DfuEvent.PROGRESS_EVENT,
# log_message="",
# progress=progress_percentage(count, frames_count),
# done=False)
# Send data stop packet
frame = int32_to_bytes(DFU_STOP_DATA_PACKET)
packet = HciPacket(frame)
self.send_packet(packet)
# self._send_event(DfuEvent.PROGRESS_EVENT, progress=100, done=False, log_message="")
def send_packet(self, pkt):
attempts = 0
last_ack = None
packet_sent = False
logger.debug("PC -> target: {0}".format(pkt))
print format(binascii.hexlify(pkt.data))
print "Sequence number HCI: " + str(HciPacket.sequence_number)
while not packet_sent:
self.serial_port.write(pkt.data)
attempts += 1
time.sleep(0.2)
ack = self.get_ack_nr()
# print "attempts: " + str(attempts)
if last_ack is None:
print str(ack)
break
if ack == (last_ack + 1) % 8:
last_ack = ack
packet_sent = True
if attempts > 3:
raise Exception("Three failed tx attempts encountered on packet {0}".format(pkt.sequence_number))
def get_ack_nr(self):
def is_timeout(start_time, timeout_sec):
return not (datetime.now() - start_time <= timedelta(0, timeout_sec))
uart_buffer = ''
start = datetime.now()
while uart_buffer.count('\xC0') < 2:
# Disregard first of the two C0
temp = self.serial_port.read(6)
if temp:
uart_buffer += temp
print format(binascii.hexlify(uart_buffer))
if is_timeout(start, DfuTransportSerial.ACK_PACKET_TIMEOUT):
# reset HciPacket numbering back to 0
HciPacket.sequence_number = 0
print "Timed out waiting for acknowledgement from device."
# quit loop
break
# read until you get a new C0
# RESUME_WORK
if len(uart_buffer) < 2:
print "No data received on serial port. Not able to proceed."
return
logger.debug("PC <- target: {0}".format(binascii.hexlify(uart_buffer)))
data = self.decode_esc_chars(uart_buffer)
# Remove 0xC0 at start and beginning
data = data[1:-1]
# Extract ACK number from header
return (data[0] >> 3) & 0x07
@staticmethod
def decode_esc_chars(data):
"""Replace 0xDBDC with 0xCO and 0xDBDD with 0xDB"""
result = []
data = bytearray(data)
while len(data):
char = data.pop(0)
if char == 0xDB:
char2 = data.pop(0)
if char2 == 0xDC:
result.append(0xC0)
elif char2 == 0xDD:
result.append(0xDB)
else:
raise Exception('Char 0xDB NOT followed by 0xDC or 0xDD')
else:
result.append(char)
return result
@staticmethod
def create_image_size_packet(softdevice_size=0, bootloader_size=0, app_size=0):
"""
Creates an image size packet necessary for sending start dfu.
@param softdevice_size: Size of SoftDevice firmware
@type softdevice_size: int
@param bootloader_size: Size of bootloader firmware
@type softdevice_size: int
@param app_size: Size of application firmware
:return: The image size packet
:rtype: str
"""
softdevice_size_packet = int32_to_bytes(softdevice_size)
bootloader_size_packet = int32_to_bytes(bootloader_size)
app_size_packet = int32_to_bytes(app_size)
image_size_packet = softdevice_size_packet + bootloader_size_packet + app_size_packet
return image_size_packet
class SerialDaemon():
"""A wrapper class for Serial and DaemonContext with inet socket support.
Creates a DaemonContext object and a Serial object using the passed
arguments. Some arguments do not take effect after the daemon is started
and hence can be altered anytime after initialization until it is started
by calling <daemon>.start()
Communication with the daemon is done on a port number specified during
initialization (or anytime before start), defaulting to 57001. Data is
decoded using the user specified encoding (default is UTF-8) and must be
either exactly 'device', in which case the daemon will reply with the full
path to the device file it is communicating with (serial_context.port),
or it must be in the following format:
<0-F><0-F0-F...0-F><data to be sent to device>
where the first byte of data always signifies how many (base 16) bytes
following it give the the number (base 16) of bytes that are to be read
from device after the data is sent to it. 0 bytes are read if data[0] is
0 or is not a valid hex number. <data[0] number of bytes> + 1 are always
discarded from the beginning of data. For example:
22F8921 sends 8921 to device and reads 2F (47) bytes of data
X78192 sends 78192 to device and dos not read a reply
3A2G9130 sends 9130 to device BUT does not read a reply since A2G is
not a valid hex number. Note that these 3 bytes are still discarded
This class does not inherit from either DaemonContext or Serial.
The only export method is start() used to run the daemon.
Accepted options for the constructor:
name
:Default: ``None``
The name of the daemon, used for syslog and the default
name of the pidfile and configuration files. Changes to this value
after the daemon has started will only be reflected in syslog.
config_file
:Default: ``'/etc/<name>.conf'``
Configuration file used to set some of the hereby listed parameters.
All but name, daemon_context and serial_context are supported.
Reloading of the configuration without restarting is done by sending
SIGHUP to the daemon but please note that changes to pidfile_path,
socket_path and log_file require restart to take effect.
The format is as follows:
<option_name> = <option value>
Option names are case-INsensitive and ``_`` may be replaced by ``-``.
Spaces around ``=`` are optional and have no effect.
Option values may optionally be enclosed in either single or double
quotes. # and any text following it on the line are ignored.
log_file
:Default: ``/tmp/<name>.log``
Log file used to log exceptions during daemon run.
pidfile_path
:Default: ``'/var/run/<name>.pid'``
Path to the pidfile. A pidfile lock is created and passed to
DaemonContext. Alternatively, you may pass a pid lockfile directly by
setting <daemon>.daemon_context.pidfile to the lockfile after
initialization but before start. Changing either of them after the
daemon is started requires a restart.
socket_path
:Default: ``'/var/run/<name>.socket'``
Path for the unix daemon socket which the daemon will be listening on.
Changing this after the daemon is started requires a restart. Also see
documentation for serial.py.
data_length
:Default: ``1024``
Number of bytes to be read from the socket. This MUST be at least the
number of bytes that have been sent, otherwise the remainder is read
afterwards and is confused for a new packet. See above for details on
the data format.
data_encoding
:Default: ``'utf-8'``
Valid encoding (accepted by the str.decode() and bytes() methods) for
the data read from and sent to the socket.
reply_length_strict
:Default: ``False``
If True daemon will not send data read from device unless the length
matches the expected reply length given as part of the data sent over
the socket. See above for details on the data format.
daemon_context
:Default: ``None``
If this is not None, it must be a DaemonContext object and is used
instead of creating a new one. All options relating to DaemoonContext
are then ignored.
serial_context
:Default: ``None``
If this is not None, it must be a Serial object and is used instead of
creating a new one. All options relating to Serial are then ignored.
In addition to the above arguments, SerialDaemon accepts all arguments
valid for DaemonContext and Serial and uses them to create the
corresponding objects (unless daemon_context or serial_context are given)
"""
def __init__(
self,
name = 'seriald',
config_file = 0,
log_file = None,
pidfile_path = 0,
socket_path = 0,
reply_length_strict = False,
data_length = 1024,
data_encoding = 'utf-8',
daemon_context = None,
serial_context = None,
**kwargs
):
self.name = name
self.config_file = config_file
if self.config_file == 0:
self.config_file = '/etc/{name}.conf'.format(name = self.name)
self.log_file = log_file
# log file will be used even if user specified None
if self.log_file is None:
self.log_file = os.path.join(
tempfile.gettempdir(), '{name}.log'.format(
name = self.name))
self.pidfile_path = pidfile_path
if self.pidfile_path == 0:
self.pidfile_path = '/var/run/{name}.pid'.format(name = self.name)
self.socket_path = socket_path
if self.socket_path == 0:
self.socket_path = '/var/run/{name}.socket'.format(name = self.name)
self.reply_length_strict = reply_length_strict
self.data_length = data_length
self.data_encoding = data_encoding
self.daemon_context = daemon_context
if self.daemon_context is None:
self.daemon_context = DaemonContext(
signal_map = {
signal.SIGHUP: self.__accept_signal,
signal.SIGINT: self.__accept_signal,
signal.SIGQUIT: self.__accept_signal,
signal.SIGTERM: self.__accept_signal,
}
)
for attr in filter(lambda s: not s.startswith('_'),
dir(self.daemon_context)):
if kwargs.get(attr) is not None:
setattr(self.daemon_context, attr, kwargs.get(attr))
self.serial_context = serial_context
if self.serial_context is None:
self.serial_context = Serial()
for attr in filter(lambda s: not s.startswith('_'),
dir(self.serial_context)):
if kwargs.get(attr) is not None:
setattr(self.serial_context, attr, kwargs.get(attr))
def __run(self):
with open(self.log_file, 'a') as log_file:
try:
soc = None
# flush doesn't work in daemon mode for ttyS?
# close and reopen instead
device = self.serial_context.port
is_ttyS = True
try:
# is it a number? Serial defaults to /dev/ttyS? if so
device += 0
except TypeError:
# not a number, assume string
try:
if not device.startswith('/dev/ttyS'):
raise AttributeError
except AttributeError:
# not a string or not a ttyS? device
# assume flushing works
is_ttyS = False
else:
device = '/dev/ttyS{num}'.format(num = device)
while True:
if not soc or soc.fileno() < 0:
logsyslog(LOG_INFO, 'Waiting for connection')
soc, soc_addr = self.socket.accept()
logsyslog(LOG_INFO, ('Connected to {addr}').format(
addr = soc_addr))
data = soc.recv(self.data_length).decode(
self.data_encoding)
if data == '':
logsyslog(LOG_INFO, 'Closing connection')
soc.close()
continue
elif data == 'device':
logsyslog(LOG_INFO, 'Device path requested')
try:
soc.sendall(device.encode(self.data_encoding))
except ConnectionResetError:
soc.close()
continue
logsyslog(LOG_INFO, 'Read from socket: {data}'.format(
data = data))
reply_length_byte_length = 0
try:
reply_length_byte_length = int(data[0], 16)
reply_length = int(
data[1 : reply_length_byte_length + 1], 16)
except ValueError:
reply_length = 0
data = data[reply_length_byte_length + 1:]
if not self.serial_context.isOpen():
# first time in the loop
logsyslog(LOG_INFO, 'Opening serial port')
self.serial_context.open()
logsyslog(LOG_INFO, 'Sending {data}'.format(
data = data))
# discard any input or output
self.serial_context.flushOutput()
self.serial_context.flushInput()
self.serial_context.write(data.encode(self.data_encoding))
if is_ttyS:
self.serial_context.close()
self.serial_context.open()
else:
self.serial_context.flush()
logsyslog(LOG_INFO, ('Will read {length} bytes').format(
length = reply_length))
if reply_length > 0:
reply = self.serial_context.read(reply_length)
reply_decoded = reply.decode(self.data_encoding)
logsyslog(LOG_INFO, 'Received {data}'.format(
data = reply_decoded))
if len(reply_decoded) == reply_length \
or not self.reply_length_strict:
try:
soc.sendall(reply)
except ConnectionResetError:
soc.close()
continue
except:
traceback.print_exc(file = log_file)
self.__stop()
def __load_config(self):
def reset_invalid_value(opt):
logsyslog(LOG_ERR,
('{conf}: Invalid value for ' +
'{option}').format(
conf = self.config_file,
option = opt))
return getattr(self, opt)
conf = _openfile(self.config_file, 'r')
if conf is not None:
with conf:
regex_pat = regex.compile(r"""\s* (?|
(?P<option>
reply_length_strict |
data[-_]length |
data[-_]encoding |
log[-_]file |
pidfile[-_]path |
socket[-_]path
) \s* (?: =\s* )?
(?|
" (?P<value> [^"]+ ) " |
' (?P<value> [^']+ ) ' |
(?P<value> [^#\r\n]+ )
) )
""", regex.X|regex.I)
line_num = 0
for line in conf:
line_num += 1
if line.startswith('#'):
continue
match = regex_pat.match(line.strip())
if match:
# translate the option name to the object's attribute
opt = match.group('option').lower().replace('-', '_')
if opt.endswith(('file',
'dir',
'path',
'encoding')):
# value is a string
val = match.group('value')
elif opt == 'reply_length_strict':
# value must be a boolean
if val.lower() not in ['0', '1', 'false', 'true']:
val = reset_invalid_value(opt)
elif val.lower() in ['0', 'false']:
val = False
else:
val = True
else:
# value must be numeric and positive
val = int(match.group('value'))
if val <= 0:
val = reset_invalid_value(opt)
setattr(self, opt, val)
else:
logsyslog(LOG_ERR, ('{conf}: Invalid syntax at line ' +
'{line}').format(
conf = self.config_file,
line = line_num))
logsyslog(LOG_INFO, 'Loaded configuration from {conf}'.format(
conf = self.config_file))
def start(self):
"""
Load config, daemonize, connect to serial port, listen on socket
"""
opensyslog(ident = self.name, facility = LOG_DAEMON)
self.__load_config()
if self.pidfile_path is not None:
self.daemon_context.pidfile = lockfile.FileLock(self.pidfile_path)
if _pidfile_isbusy(self.daemon_context.pidfile):
logsyslog(LOG_ERR, 'Already running (pidfile is locked)')
closesyslog()
return
if _socket_isbusy(self.socket_path):
logsyslog(LOG_ERR, 'Already running (socket is in use)')
closesyslog()
return
self.daemon_context.open()
with _openfile(self.daemon_context.pidfile.path, 'w',
fail = self.__stop) as file:
file.write('{pid}'.format(pid = os.getpid()))
# opening the serial port here doesn't work
# open it in __run instead
# self.serial_context.open()
logsyslog(LOG_INFO, 'Started')
self.socket = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
self.socket.bind(self.socket_path)
self.socket.listen(1)
logsyslog(LOG_INFO, ('Listening on socket {socket}').format(
socket = self.socket_path))
self.__run()
def __stop(self):
pid = _get_pid(self.daemon_context.pidfile.path)
if pid is None:
return
logsyslog(LOG_INFO, 'Stopping')
os.remove(self.socket.getsockname())
os.remove(self.daemon_context.pidfile.path)
self.socket.close()
if self.serial_context.isOpen():
self.serial_context.close()
self.daemon_context.close()
try:
os.kill(pid, signal.SIGKILL)
except OSError:
logsyslog(LOG_ERR, 'Could not stop process id {pid}'.format(
pid = pid))
closesyslog()
def __accept_signal(self, sig, frame):
if sig == signal.SIGHUP:
self.__load_config()
else:
logsyslog(LOG_INFO, 'Caught signal {sig}'.format(sig = sig))
self.__stop()
class TagReader:
def __init__(self, port):
self.serial_port = Serial(port, baudrate=9600, timeout=0.01)
self.serial_port.close()
self.serial_port.open()
self.serial_port.flush() # Flush any old data
self.should_do_checksum = True
def getBufferSize(self):
return self.serial_port.inWaiting()
"""
RFID Tag is 16 characters: STX(02h) DATA (10 ASCII) CHECK SUM (2 ASCII) CR LF ETX(03h)
1 char of junk, 10 of hexadecimal data, 2 of hexadecimal check sum, 3 of junk
XOR-ing 5 1-byte hex values will give the 1-byte check sum - if requested
"""
def readTag(self):
# run the while loop since it may read a tag twice.
# May not be necessary
while (self.getBufferSize() >= 16):
junk = self.serial_port.read(1) # Junk byte 1
tag = self.serial_port.read(10) # tag bytes 10
check_sum = self.serial_port.read(2) # check sum 2 bytes
junk = self.serial_port.read(3) # Last 3 bytes are junk
if (self.should_do_checksum):
self.doCheckSum(tag, check_sum)
return int(tag, base=16) # Convert the tag to an integer.
def doCheckSum(self, tag, check_sum):
try:
checked_val = 0
for i in range(0,5):
checked_val = checked_val ^ int(tag [(2 * i) : (2 * (i + 1))], 16)
if checked_val != int(check_sum, 16):
print ("Tag reader checksum error. Tag was not read fully...")
except ValueError:
print ("Error reading the tag properly.")
print ("Tag: " + tag)
def close(self):
self.serial_port.close()
