初始化提交
This commit is contained in:
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,92 @@
|
||||
from machine import I2C
|
||||
import time
|
||||
import ustruct
|
||||
|
||||
DATA_FORMAT = 0x31
|
||||
BW_RATE = 0x2c
|
||||
POWER_CTL = 0x2d
|
||||
INT_ENABLE = 0x2E
|
||||
OFSX = 0x1e
|
||||
OFSY =0x1f
|
||||
OFSZ =0x20
|
||||
|
||||
class ADXL345:
|
||||
def __init__(self, i2c):
|
||||
self.i2c = i2c
|
||||
slv = self.i2c.scan()
|
||||
print(slv)
|
||||
for s in slv:
|
||||
buf = self.i2c.readfrom_mem(s, 0, 1)
|
||||
print(buf)
|
||||
if(buf[0] == 0xe5):
|
||||
self.slvAddr = s
|
||||
print('adxl345 found')
|
||||
break
|
||||
#self.writeByte(POWER_CTL,0x00) #sleep
|
||||
#time.sleep(0.001)
|
||||
#低电平中断输出,13位全分辨率,输出数据右对齐,16g量程
|
||||
self.writeByte(DATA_FORMAT,0x2B)
|
||||
#数据输出速度为100Hz
|
||||
self.writeByte(BW_RATE,0x0A)
|
||||
#不使用中断
|
||||
self.writeByte(INT_ENABLE,0x00)
|
||||
|
||||
self.writeByte(OFSX,0x00)
|
||||
self.writeByte(OFSY,0x00)
|
||||
self.writeByte(OFSZ,0x00)
|
||||
#链接使能,测量模式
|
||||
self.writeByte(POWER_CTL,0x28)
|
||||
time.sleep(1)
|
||||
|
||||
def readXYZ(self):
|
||||
fmt = '<h' #little-endian
|
||||
buf1 = self.readByte(0x32)
|
||||
buf2 = self.readByte(0x33)
|
||||
buf = bytearray([buf1[0], buf2[0]])
|
||||
x, = ustruct.unpack(fmt, buf)
|
||||
x = x*3.9
|
||||
#print('x:',x)
|
||||
|
||||
buf1 = self.readByte(0x34)
|
||||
buf2 = self.readByte(0x35)
|
||||
buf = bytearray([buf1[0], buf2[0]])
|
||||
y, = ustruct.unpack(fmt, buf)
|
||||
y = y*3.9
|
||||
#print('y:',y)
|
||||
|
||||
buf1 = self.readByte(0x36)
|
||||
buf2 = self.readByte(0x37)
|
||||
buf = bytearray([buf1[0], buf2[0]])
|
||||
z, = ustruct.unpack(fmt, buf)
|
||||
z = z*3.9
|
||||
#print('z:',z)
|
||||
#print('************************')
|
||||
#time.sleep(0.5)
|
||||
return (x,y,z)
|
||||
|
||||
def readX(self):
|
||||
return self.readXYZ()[0]
|
||||
|
||||
def readY(self):
|
||||
return self.readXYZ()[1]
|
||||
|
||||
def readZ(self):
|
||||
return self.readXYZ()[2]
|
||||
|
||||
def writeByte(self, addr, data):
|
||||
d = bytearray([data])
|
||||
self.i2c.writeto_mem(self.slvAddr, addr, d)
|
||||
def readByte(self, addr):
|
||||
return self.i2c.readfrom_mem(self.slvAddr, addr, 1)
|
||||
|
||||
if __name__ == '__main__':
|
||||
#初始化
|
||||
sensor = ADXL345(i2c)
|
||||
|
||||
#getXYZ
|
||||
x,y,z = sensor.readXYZ()
|
||||
|
||||
#getX/Y/Z
|
||||
x0 = sensor.readX()
|
||||
y0 = sensor.readY()
|
||||
z0 = sensor.readZ()
|
||||
@@ -0,0 +1,133 @@
|
||||
import KPU as kpu
|
||||
import gc,image,time
|
||||
import board
|
||||
|
||||
try:
|
||||
|
||||
kpu.deinit(task_fe)
|
||||
kpu.deinit(task_ld)
|
||||
kpu.deinit(task_fd)
|
||||
del task_fe
|
||||
del task_ld
|
||||
del task_fd
|
||||
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
gc.collect()
|
||||
|
||||
record_ftr = []
|
||||
record_ftrs = []
|
||||
img_face = image.Image(size=(128, 128))
|
||||
a = img_face.pix_to_ai()
|
||||
dst_point = [(44, 59), (84, 59), (64, 82), (47, 105),(81, 105)]
|
||||
|
||||
start_processing = False
|
||||
tim2 = time.ticks_ms()
|
||||
|
||||
task_fd=None
|
||||
task_ld=None
|
||||
task_fe=None
|
||||
info=None
|
||||
bb=1
|
||||
|
||||
def set_key_state(*_):
|
||||
global start_processing
|
||||
global tim2
|
||||
if (time.ticks_ms() - tim2 )> 4000:
|
||||
start_processing = True
|
||||
tim2 = time.ticks_ms()
|
||||
|
||||
|
||||
def init(FD,LD,FE):
|
||||
global task_fd
|
||||
global task_ld
|
||||
global task_fe
|
||||
# task_fd = kpu.load(0x200000)
|
||||
# task_ld = kpu.load(0x300000)
|
||||
# task_fe = kpu.load(0x400000)
|
||||
|
||||
task_fd = kpu.load(FD)
|
||||
task_ld = kpu.load(LD)
|
||||
task_fe = kpu.load(FE)
|
||||
|
||||
gc.collect()
|
||||
key_gpio = board.pin(9,board.GPIO.IN,board.GPIO.PULL_UP)
|
||||
key_gpio.irq(set_key_state,board.GPIO.IRQ_RISING, board.GPIO.WAKEUP_NOT_SUPPORT)
|
||||
|
||||
anchor = (1.889, 2.5245, 2.9465, 3.94056, 3.99987, 5.3658, 5.155437,6.92275, 6.718375, 9.01025) # anchor for face detect
|
||||
kpu.init_yolo2(task_fd, 0.5, 0.3, 5, anchor)
|
||||
|
||||
|
||||
def train(img,names,threshold):
|
||||
global task_fd
|
||||
global task_ld
|
||||
global task_fe
|
||||
global start_processing
|
||||
global info
|
||||
global bb
|
||||
|
||||
code = kpu.run_yolo2(task_fd, img)
|
||||
if code:
|
||||
for i in code:
|
||||
face_cut = img.cut(i.x(), i.y(), i.w(), i.h())
|
||||
face_cut_128 = face_cut.resize(128, 128)
|
||||
a = face_cut_128.pix_to_ai()
|
||||
fmap = kpu.forward(task_ld, face_cut_128)
|
||||
plist = fmap[:]
|
||||
le = (i.x()+int(plist[0]*i.w() - 10), i.y()+int(plist[1]*i.h()))
|
||||
re = (i.x()+int(plist[2]*i.w()), i.y()+int(plist[3]*i.h()))
|
||||
nose = (i.x()+int(plist[4]*i.w()), i.y()+int(plist[5]*i.h()))
|
||||
lm = (i.x()+int(plist[6]*i.w()), i.y()+int(plist[7]*i.h()))
|
||||
rm = (i.x()+int(plist[8]*i.w()), i.y()+int(plist[9]*i.h()))
|
||||
lb=i.rect()
|
||||
src_point = [le, re, nose, lm, rm]
|
||||
T = image.get_affine_transform(src_point, dst_point)
|
||||
a = image.warp_affine_ai(img, img_face, T)
|
||||
a = img_face.ai_to_pix()
|
||||
del(face_cut_128)
|
||||
fmap = kpu.forward(task_fe, img_face)
|
||||
feature = kpu.face_encode(fmap[:])
|
||||
reg_flag = False
|
||||
scores = []
|
||||
for j in range(len(record_ftrs)):
|
||||
score = kpu.face_compare(record_ftrs[j], feature)
|
||||
scores.append(score)
|
||||
max_score = 0
|
||||
index = 0
|
||||
for k in range(len(scores)):
|
||||
if max_score < scores[k]:
|
||||
max_score = scores[k]
|
||||
index = k
|
||||
if start_processing:
|
||||
record_ftr = feature
|
||||
record_ftrs.append(record_ftr)
|
||||
start_processing = False
|
||||
if max_score > threshold:
|
||||
info=[names[index],max_score,lb,src_point]
|
||||
else:
|
||||
if bb==1:
|
||||
print("Please press BOOT key to enter the face")
|
||||
bb=0
|
||||
info=[None,max_score,lb,src_point]
|
||||
return True
|
||||
break
|
||||
else:
|
||||
info=None
|
||||
bb=1
|
||||
return False
|
||||
gc.collect()
|
||||
|
||||
def info_name():
|
||||
gc.collect()
|
||||
return info[0]
|
||||
|
||||
def info_score():
|
||||
return info[1]
|
||||
|
||||
def info_face():
|
||||
return info[2]
|
||||
|
||||
def info_organs():
|
||||
return info[3]
|
||||
|
||||
@@ -0,0 +1,137 @@
|
||||
import network,time,random,request,base64,json,board
|
||||
from machine import UART
|
||||
|
||||
wifi_en=board.pin(19,board.GPIO.OUT)
|
||||
board.register(18,board.FPIOA.UART2_TX)
|
||||
board.register(17,board.FPIOA.UART2_RX)
|
||||
|
||||
def wifi_enable(en):
|
||||
global wifi_en
|
||||
wifi_en.value(en)
|
||||
|
||||
def wifi_reset():
|
||||
global uart
|
||||
wifi_enable(0)
|
||||
time.sleep_ms(200)
|
||||
wifi_enable(1)
|
||||
time.sleep(2)
|
||||
uart = UART(UART.UART2,115200,timeout=1000, read_buf_len=4096)
|
||||
tmp = uart.read()
|
||||
uart.write("AT+UART_CUR=921600,8,1,0,0\r\n")
|
||||
print(uart.read())
|
||||
uart = UART(UART.UART2,921600,timeout=1000, read_buf_len=10240) # important! baudrate too low or read_buf_len too small will loose data
|
||||
uart.write("AT\r\n")
|
||||
tmp = uart.read()
|
||||
print(tmp)
|
||||
if not tmp.endswith("OK\r\n"):
|
||||
print("reset fail")
|
||||
return None
|
||||
try:
|
||||
nic = network.ESP8285(uart)
|
||||
except Exception:
|
||||
return None
|
||||
return nic
|
||||
|
||||
def nic_init(account,password):
|
||||
nic=wifi_reset()
|
||||
if not nic:
|
||||
raise Exception("[Cool.AI]:WiFi init fail")
|
||||
|
||||
nic.connect(account,password)
|
||||
nic.ifconfig()
|
||||
|
||||
|
||||
class SimpleEncode():
|
||||
keyStr = "ABCDEFGHIJKLMNOPQRSTUVWXYZ1234567890abcdefghijklmnopqrstuvwxyz~!@#$%^&*()_+-={}[]:;<,>.?/|"
|
||||
keyLength = len(keyStr)
|
||||
encryptionA = 17
|
||||
encryptionB = 8
|
||||
decodeA = 0
|
||||
preCountMax = 15
|
||||
postCount = 5
|
||||
randomChar = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnop"
|
||||
randomCharLength = len(randomChar)
|
||||
#base64字符
|
||||
ALPHABET = "ABCDEFGHIJKLMN0123456789OPQRSTUVWXYZ+/abcdefghijklmnopqrstuvwxyz"
|
||||
STANDARD = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/" # 标准的字符串索引
|
||||
|
||||
#找到密钥
|
||||
for i in range(1,keyLength):
|
||||
if (encryptionA * i) % keyLength == 1:
|
||||
decodeA = i
|
||||
|
||||
def base64Encode(self,sourceStr):
|
||||
encode = ""
|
||||
for ch in base64.b64encode(sourceStr.encode()).decode():
|
||||
if ch == '=':
|
||||
encode += '='
|
||||
else:
|
||||
for i in range(64):
|
||||
if ch == self.STANDARD[i]:
|
||||
encode += self.ALPHABET[i]
|
||||
return encode
|
||||
|
||||
def encrpyt(self,sourceStr):
|
||||
srcLength = len(sourceStr)
|
||||
#先加入干扰字符的数量
|
||||
addCharCount = random.randint(1,self.preCountMax) if srcLength < self.preCountMax else 0
|
||||
#随机字符
|
||||
sb = str(addCharCount)+"|"
|
||||
for i in range(addCharCount):
|
||||
sb += self.randomChar[random.randint(0,self.randomCharLength-1)]
|
||||
|
||||
sb += sourceStr
|
||||
#尾部固定增加x个字符
|
||||
for i in range(self.postCount):
|
||||
sb += self.randomChar[random.randint(0,self.randomCharLength-1)]
|
||||
|
||||
#base64 加密
|
||||
base64Str = self.base64Encode(sb)
|
||||
|
||||
destStr = ''
|
||||
for i in range(len(base64Str)):
|
||||
#找到字符所在位置
|
||||
position = self.keyStr.find(base64Str[i])
|
||||
|
||||
#对字符进行转换
|
||||
y = (self.encryptionA * position + self.encryptionB) % self.keyLength
|
||||
|
||||
#找到替换后的字符
|
||||
|
||||
destStr += self.keyStr[y]
|
||||
|
||||
return destStr
|
||||
|
||||
def token(account,password):
|
||||
url = 'http://ai.heclouds.com:9090/v1/user/oneNetLogin'
|
||||
headers = {
|
||||
'Content-Type': 'application/json',
|
||||
}
|
||||
simpleencode = SimpleEncode()
|
||||
data = {'account': account, 'password': simpleencode.encrpyt(password)}
|
||||
|
||||
req = request.post(url, data=json.dumps(data), headers=headers)
|
||||
#print(req.text)
|
||||
try:
|
||||
return eval(req.text)['data']['loginToken']
|
||||
except:
|
||||
raise ValueError( "[Cool.AI]:Wrong account or password ")
|
||||
|
||||
|
||||
def post_ai(img,urlx,Token):
|
||||
ur = 'http://183.230.40.32:9090/v1/aiApi/picture/MixPY'
|
||||
url=ur.replace("MixPY",urlx)
|
||||
headers ={
|
||||
'Content-Type':'application/json',
|
||||
'Login-Token':'Token'
|
||||
}
|
||||
|
||||
headers['Login-Token']=Token
|
||||
|
||||
imge = img.compressed(quality=50)
|
||||
file =imge.to_bytes()
|
||||
str = base64.b64encode(file).decode()
|
||||
data = {'picture':[str]}
|
||||
req = request.post(url,data=json.dumps(data),headers=headers)
|
||||
return json.loads(req.text)
|
||||
#return eval(req.text)
|
||||
@@ -0,0 +1,256 @@
|
||||
import re
|
||||
import struct
|
||||
import binascii
|
||||
|
||||
__all__ = [
|
||||
'encode', 'decode', 'encodebytes', 'decodebytes',
|
||||
'b64encode', 'b64decode', 'b32encode', 'b32decode',
|
||||
'b16encode', 'b16decode',
|
||||
'standard_b64encode', 'standard_b64decode',
|
||||
'urlsafe_b64encode', 'urlsafe_b64decode',
|
||||
]
|
||||
|
||||
bytes_types = (bytes, bytearray)
|
||||
|
||||
def _bytes_from_decode_data(s):
|
||||
if isinstance(s, str):
|
||||
try:
|
||||
return s.encode('ascii')
|
||||
except:
|
||||
raise ValueError('string argument should contain only ASCII characters')
|
||||
elif isinstance(s, bytes_types):
|
||||
return s
|
||||
else:
|
||||
raise TypeError("argument should be bytes or ASCII string, not %s" % s.__class__.__name__)
|
||||
|
||||
def b64encode(s, altchars=None):
|
||||
if not isinstance(s, bytes_types):
|
||||
raise TypeError("expected bytes, not %s" % s.__class__.__name__)
|
||||
encoded = binascii.b2a_base64(s)[:-1]
|
||||
if altchars is not None:
|
||||
if not isinstance(altchars, bytes_types):
|
||||
raise TypeError("expected bytes, not %s"
|
||||
% altchars.__class__.__name__)
|
||||
assert len(altchars) == 2, repr(altchars)
|
||||
return encoded.translate(bytes.maketrans(b'+/', altchars))
|
||||
return encoded
|
||||
|
||||
def b64decode(s, altchars=None, validate=False):
|
||||
s = _bytes_from_decode_data(s)
|
||||
if altchars is not None:
|
||||
altchars = _bytes_from_decode_data(altchars)
|
||||
assert len(altchars) == 2, repr(altchars)
|
||||
s = s.translate(bytes.maketrans(altchars, b'+/'))
|
||||
if validate and not re.match(b'^[A-Za-z0-9+/]*={0,2}$', s):
|
||||
raise binascii.Error('Non-base64 digit found')
|
||||
return binascii.a2b_base64(s)
|
||||
|
||||
def standard_b64encode(s):
|
||||
return b64encode(s)
|
||||
|
||||
def standard_b64decode(s):
|
||||
return b64decode(s)
|
||||
|
||||
def urlsafe_b64encode(s):
|
||||
raise NotImplementedError()
|
||||
|
||||
def urlsafe_b64decode(s):
|
||||
raise NotImplementedError()
|
||||
|
||||
_b32alphabet = {
|
||||
0: b'A', 9: b'J', 18: b'S', 27: b'3',
|
||||
1: b'B', 10: b'K', 19: b'T', 28: b'4',
|
||||
2: b'C', 11: b'L', 20: b'U', 29: b'5',
|
||||
3: b'D', 12: b'M', 21: b'V', 30: b'6',
|
||||
4: b'E', 13: b'N', 22: b'W', 31: b'7',
|
||||
5: b'F', 14: b'O', 23: b'X',
|
||||
6: b'G', 15: b'P', 24: b'Y',
|
||||
7: b'H', 16: b'Q', 25: b'Z',
|
||||
8: b'I', 17: b'R', 26: b'2',
|
||||
}
|
||||
|
||||
_b32tab = [v[0] for k, v in sorted(_b32alphabet.items())]
|
||||
_b32rev = dict([(v[0], k) for k, v in _b32alphabet.items()])
|
||||
|
||||
|
||||
def b32encode(s):
|
||||
if not isinstance(s, bytes_types):
|
||||
raise TypeError("expected bytes, not %s" % s.__class__.__name__)
|
||||
quanta, leftover = divmod(len(s), 5)
|
||||
if leftover:
|
||||
s = s + bytes(5 - leftover) # Don't use += !
|
||||
quanta += 1
|
||||
encoded = bytearray()
|
||||
for i in range(quanta):
|
||||
c1, c2, c3 = struct.unpack('!HHB', s[i*5:(i+1)*5])
|
||||
c2 += (c1 & 1) << 16
|
||||
c3 += (c2 & 3) << 8
|
||||
encoded += bytes([_b32tab[c1 >> 11],
|
||||
_b32tab[(c1 >> 6) & 0x1f],
|
||||
_b32tab[(c1 >> 1) & 0x1f],
|
||||
_b32tab[c2 >> 12],
|
||||
_b32tab[(c2 >> 7) & 0x1f],
|
||||
_b32tab[(c2 >> 2) & 0x1f],
|
||||
_b32tab[c3 >> 5],
|
||||
_b32tab[c3 & 0x1f],
|
||||
])
|
||||
if leftover == 1:
|
||||
encoded = encoded[:-6] + b'======'
|
||||
elif leftover == 2:
|
||||
encoded = encoded[:-4] + b'===='
|
||||
elif leftover == 3:
|
||||
encoded = encoded[:-3] + b'==='
|
||||
elif leftover == 4:
|
||||
encoded = encoded[:-1] + b'='
|
||||
return bytes(encoded)
|
||||
|
||||
|
||||
def b32decode(s, casefold=False, map01=None):
|
||||
s = _bytes_from_decode_data(s)
|
||||
quanta, leftover = divmod(len(s), 8)
|
||||
if leftover:
|
||||
raise binascii.Error('Incorrect padding')
|
||||
if map01 is not None:
|
||||
map01 = _bytes_from_decode_data(map01)
|
||||
assert len(map01) == 1, repr(map01)
|
||||
s = s.translate(bytes.maketrans(b'01', b'O' + map01))
|
||||
if casefold:
|
||||
s = s.upper()
|
||||
padchars = s.find(b'=')
|
||||
if padchars > 0:
|
||||
padchars = len(s) - padchars
|
||||
s = s[:-padchars]
|
||||
else:
|
||||
padchars = 0
|
||||
|
||||
parts = []
|
||||
acc = 0
|
||||
shift = 35
|
||||
for c in s:
|
||||
val = _b32rev.get(c)
|
||||
if val is None:
|
||||
raise binascii.Error('Non-base32 digit found')
|
||||
acc += _b32rev[c] << shift
|
||||
shift -= 5
|
||||
if shift < 0:
|
||||
parts.append(binascii.unhexlify(bytes('%010x' % acc, "ascii")))
|
||||
acc = 0
|
||||
shift = 35
|
||||
last = binascii.unhexlify(bytes('%010x' % acc, "ascii"))
|
||||
if padchars == 0:
|
||||
last = b''
|
||||
elif padchars == 1:
|
||||
last = last[:-1]
|
||||
elif padchars == 3:
|
||||
last = last[:-2]
|
||||
elif padchars == 4:
|
||||
last = last[:-3]
|
||||
elif padchars == 6:
|
||||
last = last[:-4]
|
||||
else:
|
||||
raise binascii.Error('Incorrect padding')
|
||||
parts.append(last)
|
||||
return b''.join(parts)
|
||||
|
||||
def b16encode(s):
|
||||
if not isinstance(s, bytes_types):
|
||||
raise TypeError("expected bytes, not %s" % s.__class__.__name__)
|
||||
return binascii.hexlify(s).upper()
|
||||
|
||||
|
||||
def b16decode(s, casefold=False):
|
||||
s = _bytes_from_decode_data(s)
|
||||
if casefold:
|
||||
s = s.upper()
|
||||
if re.search(b'[^0-9A-F]', s):
|
||||
raise binascii.Error('Non-base16 digit found')
|
||||
return binascii.unhexlify(s)
|
||||
|
||||
MAXLINESIZE = 76
|
||||
MAXBINSIZE = (MAXLINESIZE//4)*3
|
||||
|
||||
def encode(input, output):
|
||||
while True:
|
||||
s = input.read(MAXBINSIZE)
|
||||
if not s:
|
||||
break
|
||||
while len(s) < MAXBINSIZE:
|
||||
ns = input.read(MAXBINSIZE-len(s))
|
||||
if not ns:
|
||||
break
|
||||
s += ns
|
||||
line = binascii.b2a_base64(s)
|
||||
output.write(line)
|
||||
|
||||
|
||||
def decode(input, output):
|
||||
while True:
|
||||
line = input.readline()
|
||||
if not line:
|
||||
break
|
||||
s = binascii.a2b_base64(line)
|
||||
output.write(s)
|
||||
|
||||
def encodebytes(s):
|
||||
if not isinstance(s, bytes_types):
|
||||
raise TypeError("expected bytes, not %s" % s.__class__.__name__)
|
||||
pieces = []
|
||||
for i in range(0, len(s), MAXBINSIZE):
|
||||
chunk = s[i : i + MAXBINSIZE]
|
||||
pieces.append(binascii.b2a_base64(chunk))
|
||||
return b"".join(pieces)
|
||||
|
||||
def encodestring(s):
|
||||
import warnings
|
||||
warnings.warn("encodestring() is a deprecated alias, use encodebytes()",
|
||||
DeprecationWarning, 2)
|
||||
return encodebytes(s)
|
||||
|
||||
|
||||
def decodebytes(s):
|
||||
if not isinstance(s, bytes_types):
|
||||
raise TypeError("expected bytes, not %s" % s.__class__.__name__)
|
||||
return binascii.a2b_base64(s)
|
||||
|
||||
def decodestring(s):
|
||||
import warnings
|
||||
warnings.warn("decodestring() is a deprecated alias, use decodebytes()",
|
||||
DeprecationWarning, 2)
|
||||
return decodebytes(s)
|
||||
|
||||
def main():
|
||||
import sys, getopt
|
||||
try:
|
||||
opts, args = getopt.getopt(sys.argv[1:], 'deut')
|
||||
except getopt.error as msg:
|
||||
sys.stdout = sys.stderr
|
||||
print(msg)
|
||||
print("""usage: %s [-d|-e|-u|-t] [file|-]
|
||||
-d, -u: decode
|
||||
-e: encode (default)
|
||||
-t: encode and decode string 'Aladdin:open sesame'"""%sys.argv[0])
|
||||
sys.exit(2)
|
||||
func = encode
|
||||
for o, a in opts:
|
||||
if o == '-e': func = encode
|
||||
if o == '-d': func = decode
|
||||
if o == '-u': func = decode
|
||||
if o == '-t': test(); return
|
||||
if args and args[0] != '-':
|
||||
with open(args[0], 'rb') as f:
|
||||
func(f, sys.stdout.buffer)
|
||||
else:
|
||||
func(sys.stdin.buffer, sys.stdout.buffer)
|
||||
|
||||
def test():
|
||||
s0 = b"Aladdin:open sesame"
|
||||
print(repr(s0))
|
||||
s1 = encodebytes(s0)
|
||||
print(repr(s1))
|
||||
s2 = decodebytes(s1)
|
||||
print(repr(s2))
|
||||
assert s0 == s2
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,178 @@
|
||||
from ustruct import unpack as unp
|
||||
import utime
|
||||
from machine import I2C
|
||||
# Author David Wahlund david@dafnet.se
|
||||
|
||||
# Power Modes
|
||||
NORMAL = 0
|
||||
|
||||
BMP280_TEMP_OS_SKIP = 0
|
||||
BMP280_TEMP_OS_1 = 1
|
||||
BMP280_TEMP_OS_2 = 2
|
||||
BMP280_TEMP_OS_4 = 3
|
||||
BMP280_TEMP_OS_8 = 4
|
||||
BMP280_TEMP_OS_16 = 5
|
||||
|
||||
BMP280_PRES_OS_SKIP = 0
|
||||
BMP280_PRES_OS_1 = 1
|
||||
BMP280_PRES_OS_2 = 2
|
||||
BMP280_PRES_OS_4 = 3
|
||||
BMP280_PRES_OS_8 = 4
|
||||
BMP280_PRES_OS_16 = 5
|
||||
|
||||
# BMP280 Temperature Registers
|
||||
BMP280_REGISTER_DIG_T1 = 0x88
|
||||
BMP280_REGISTER_DIG_T2 = 0x8A
|
||||
BMP280_REGISTER_DIG_T3 = 0x8C
|
||||
# BMP280 Pressure Registers
|
||||
BMP280_REGISTER_DIG_P1 = 0x8E
|
||||
BMP280_REGISTER_DIG_P2 = 0x90
|
||||
BMP280_REGISTER_DIG_P3 = 0x92
|
||||
BMP280_REGISTER_DIG_P4 = 0x94
|
||||
BMP280_REGISTER_DIG_P5 = 0x96
|
||||
BMP280_REGISTER_DIG_P6 = 0x98
|
||||
BMP280_REGISTER_DIG_P7 = 0x9A
|
||||
BMP280_REGISTER_DIG_P8 = 0x9C
|
||||
BMP280_REGISTER_DIG_P9 = 0x9E
|
||||
|
||||
BMP280_REGISTER_ID = 0xD0
|
||||
BMP280_REGISTER_RESET = 0xE0
|
||||
BMP280_REGISTER_STATUS = 0xF3
|
||||
BMP280_REGISTER_CONTROL = 0xF4
|
||||
BMP280_REGISTER_CONFIG = 0xF5 # IIR filter config
|
||||
|
||||
BMP280_REGISTER_DATA = 0xF7
|
||||
|
||||
|
||||
class BMP280:
|
||||
def __init__(self, i2c_bus, addr=0x77):
|
||||
self._bmp_i2c = i2c_bus
|
||||
self._i2c_addr = addr
|
||||
|
||||
|
||||
self.chip_id = self._read(BMP280_REGISTER_ID, 2)
|
||||
|
||||
self._T1 = unp('<H', self._read(BMP280_REGISTER_DIG_T1, 2))[0]
|
||||
self._T2 = unp('<h', self._read(BMP280_REGISTER_DIG_T2, 2))[0]
|
||||
self._T3 = unp('<h', self._read(BMP280_REGISTER_DIG_T3, 2))[0]
|
||||
self._P1 = unp('<H', self._read(BMP280_REGISTER_DIG_P1, 2))[0]
|
||||
self._P2 = unp('<h', self._read(BMP280_REGISTER_DIG_P2, 2))[0]
|
||||
self._P3 = unp('<h', self._read(BMP280_REGISTER_DIG_P3, 2))[0]
|
||||
self._P4 = unp('<h', self._read(BMP280_REGISTER_DIG_P4, 2))[0]
|
||||
self._P5 = unp('<h', self._read(BMP280_REGISTER_DIG_P5, 2))[0]
|
||||
self._P6 = unp('<h', self._read(BMP280_REGISTER_DIG_P6, 2))[0]
|
||||
self._P7 = unp('<h', self._read(BMP280_REGISTER_DIG_P7, 2))[0]
|
||||
self._P8 = unp('<h', self._read(BMP280_REGISTER_DIG_P8, 2))[0]
|
||||
self._P9 = unp('<h', self._read(BMP280_REGISTER_DIG_P9, 2))[0]
|
||||
|
||||
self._t_os = BMP280_TEMP_OS_2 # temperature oversampling
|
||||
self._p_os = BMP280_PRES_OS_16 # pressure oversampling
|
||||
|
||||
# output raw
|
||||
self._t_raw = 0
|
||||
self._t_fine = 0
|
||||
self._t = 0
|
||||
|
||||
self._p_raw = 0
|
||||
self._p = 0
|
||||
|
||||
self._read_wait_ms = 100 # interval between forced measure and readout
|
||||
self._new_read_ms = 200 # interval between
|
||||
self._last_read_ts = 0
|
||||
|
||||
def _read(self, addr, size=1):
|
||||
return self._bmp_i2c.readfrom_mem(self._i2c_addr, addr, size)
|
||||
|
||||
def _write(self, addr, b_arr):
|
||||
if not type(b_arr) is bytearray:
|
||||
b_arr = bytearray([b_arr])
|
||||
return self._bmp_i2c.writeto_mem(self._i2c_addr, addr, b_arr)
|
||||
|
||||
def _gauge(self):
|
||||
# TODO limit new reads
|
||||
now = utime.ticks_ms()
|
||||
if utime.ticks_diff(now, self._last_read_ts) > self._new_read_ms:
|
||||
self._last_read_ts = now
|
||||
r = self._t_os + (self._p_os << 3) + (1 << 6)
|
||||
self._write(BMP280_REGISTER_CONTROL, r)
|
||||
utime.sleep_ms(100) # TODO calc sleep
|
||||
d = self._read(BMP280_REGISTER_DATA, 6) # read all data at once (as by spec)
|
||||
|
||||
self._p_raw = (d[0] << 12) + (d[1] << 4) + (d[2] >> 4)
|
||||
self._t_raw = (d[3] << 12) + (d[4] << 4) + (d[5] >> 4)
|
||||
|
||||
self._t_fine = 0
|
||||
self._t = 0
|
||||
self._p = 0
|
||||
|
||||
def load_test_calibration(self):
|
||||
self._T1 = 27504
|
||||
self._T2 = 26435
|
||||
self._T3 = -1000
|
||||
self._P1 = 36477
|
||||
self._P2 = -10685
|
||||
self._P3 = 3024
|
||||
self._P4 = 2855
|
||||
self._P5 = 140
|
||||
self._P6 = -7
|
||||
self._P7 = 15500
|
||||
self._P8 = -14600
|
||||
self._P9 = 6000
|
||||
|
||||
def load_test_data(self):
|
||||
self._t_raw = 519888
|
||||
self._p_raw = 415148
|
||||
|
||||
def print_calibration(self):
|
||||
print("T1: {} {}".format(self._T1, type(self._T1)))
|
||||
print("T2: {} {}".format(self._T2, type(self._T2)))
|
||||
print("T3: {} {}".format(self._T3, type(self._T3)))
|
||||
print("P1: {} {}".format(self._P1, type(self._P1)))
|
||||
print("P2: {} {}".format(self._P2, type(self._P2)))
|
||||
print("P3: {} {}".format(self._P3, type(self._P3)))
|
||||
print("P4: {} {}".format(self._P4, type(self._P4)))
|
||||
print("P5: {} {}".format(self._P5, type(self._P5)))
|
||||
print("P6: {} {}".format(self._P6, type(self._P6)))
|
||||
print("P7: {} {}".format(self._P7, type(self._P7)))
|
||||
print("P8: {} {}".format(self._P8, type(self._P8)))
|
||||
print("P9: {} {}".format(self._P9, type(self._P9)))
|
||||
|
||||
def _calc_t_fine(self):
|
||||
# From datasheet page 22
|
||||
self._gauge()
|
||||
if self._t_fine == 0:
|
||||
var1 = (((self._t_raw >> 3) - (self._T1 << 1)) * self._T2) >> 11
|
||||
var2 = (((((self._t_raw >> 4) - self._T1) * ((self._t_raw >> 4) - self._T1)) >> 12) * self._T3) >> 14
|
||||
self._t_fine = var1 + var2
|
||||
|
||||
# @property
|
||||
def get_BMP_temperature(self):
|
||||
self._calc_t_fine()
|
||||
if self._t == 0:
|
||||
self._t = ((self._t_fine * 5 + 128) >> 8) / 100.
|
||||
return self._t
|
||||
|
||||
# @property
|
||||
def get_BMP_pressure(self):
|
||||
# From datasheet page 22
|
||||
self._calc_t_fine()
|
||||
if self._p == 0:
|
||||
var1 = self._t_fine - 128000
|
||||
var2 = var1 * var1 * self._P6
|
||||
var2 = var2 + ((var1 * self._P5) << 17)
|
||||
var2 = var2 + (self._P4 << 35)
|
||||
var1 = ((var1 * var1 * self._P3) >> 8) + ((var1 * self._P2) << 12)
|
||||
var1 = (((1 << 47) + var1) * self._P1) >> 33
|
||||
|
||||
if var1 == 0:
|
||||
return 0
|
||||
|
||||
p = 1048576 - self._p_raw
|
||||
p = int((((p << 31) - var2) * 3125) / var1)
|
||||
var1 = (self._P9 * (p >> 13) * (p >> 13)) >> 25
|
||||
var2 = (self._P8 * p) >> 19
|
||||
|
||||
p = ((p + var1 + var2) >> 8) + (self._P7 << 4)
|
||||
self._p = p / 256.0
|
||||
return self._p
|
||||
|
||||
@@ -0,0 +1,29 @@
|
||||
"""
|
||||
MxiGo AI to MxiGo CE COM
|
||||
|
||||
MicroPython library for the MxiGo AI
|
||||
=======================================================
|
||||
20211213
|
||||
mixly
|
||||
"""
|
||||
data_a=None
|
||||
|
||||
def uart_tx(uart,data,repeat=True):
|
||||
global data_a
|
||||
data_b = data
|
||||
if data_b != data_a:
|
||||
uart.write((str(data)+'\n'))
|
||||
#print(data)
|
||||
if not repeat:
|
||||
data_a=data_b
|
||||
|
||||
def uart_rx(uart):
|
||||
data = uart.readline()
|
||||
if data:
|
||||
data_str = data.strip()
|
||||
try:
|
||||
data_str=data_str.decode()
|
||||
return eval(data_str)
|
||||
except:
|
||||
return data_str
|
||||
|
||||
@@ -0,0 +1,58 @@
|
||||
import time,board
|
||||
|
||||
def read_data(pin_name):
|
||||
data=[]
|
||||
j=0
|
||||
time.sleep_ms(1200)
|
||||
N1 = board.pin(pin_name, board.GPIO.OUT)
|
||||
N1.value(0)
|
||||
time.sleep_ms(20)
|
||||
N1.value(1)
|
||||
time.sleep_us(30)
|
||||
N1 =board.pin(pin_name, board.GPIO.IN)
|
||||
T1 = time.ticks_us()
|
||||
while N1.value()==0:
|
||||
continue
|
||||
while N1.value()==1:
|
||||
T2 =time.ticks_us()
|
||||
if time.ticks_diff(T2, T1) >200000:
|
||||
#raise ValueError("[MixNo]:Sensor read error")
|
||||
break
|
||||
continue
|
||||
while j<40:
|
||||
k=0
|
||||
while N1.value()==0:
|
||||
continue
|
||||
while N1.value()==1:
|
||||
k+=1
|
||||
if k>100:break
|
||||
if k<15:
|
||||
data.append(0)
|
||||
else:
|
||||
data.append(1)
|
||||
j=j+1
|
||||
del N1
|
||||
humidity_bit=data[0:8]
|
||||
humidity_point_bit=data[8:16]
|
||||
temperature_bit=data[16:24]
|
||||
temperature_point_bit=data[24:32]
|
||||
check_bit=data[32:40]
|
||||
humidity=0
|
||||
humidity_point=0
|
||||
temperature=0.0
|
||||
temperature_point=0
|
||||
check=0
|
||||
for i in range(8):
|
||||
humidity+=humidity_bit[i]*2**(7-i)
|
||||
humidity_point+=humidity_point_bit[i]*2**(7-i)
|
||||
temperature+=temperature_bit[i]*2**(7-i)
|
||||
temperature_point+=temperature_point_bit[i]*2**(7-i)
|
||||
check+=check_bit[i]*2**(7-i)
|
||||
tmp=humidity+humidity_point+temperature+temperature_point
|
||||
#print(humidity_point,temperature_point)
|
||||
if check==tmp:
|
||||
#print('temperature is',temperature,'-wet is',humidity,'%')
|
||||
return (temperature+temperature_point/10,humidity)
|
||||
else:
|
||||
#print('Error:',humidity,humidity_point,temperature,temperature_point,check)
|
||||
return (None,None)
|
||||
@@ -0,0 +1,27 @@
|
||||
import time,board
|
||||
|
||||
def Sonar(trig1, echo1):
|
||||
|
||||
trig = board.pin(trig1, board.GPIO.OUT)
|
||||
echo = board.pin(echo1, board.GPIO.IN)
|
||||
time.sleep_ms(10)
|
||||
trig.value(1)
|
||||
time.sleep_us(10)
|
||||
trig.value(0)
|
||||
n1 = time.ticks_us()
|
||||
while(echo.value()==0):
|
||||
n2 =time.ticks_us()
|
||||
if time.ticks_diff(n2, n1) >200000:
|
||||
#raise ValueError("[MixNo]:Sensor read error")
|
||||
break
|
||||
pass
|
||||
t1 = time.ticks_us()
|
||||
while(echo.value()==1):
|
||||
n3 =time.ticks_us()
|
||||
if time.ticks_diff(n3, t1) >200000:
|
||||
#raise ValueError("[MixNo]:Sensor read error")
|
||||
break
|
||||
pass
|
||||
t2 = time.ticks_us()
|
||||
time.sleep_ms(10)
|
||||
return round(time.ticks_diff(t2, t1) / 10000 * 340 / 2, 2)
|
||||
@@ -0,0 +1,47 @@
|
||||
import time,board
|
||||
|
||||
def read_id(pin):
|
||||
|
||||
L1 = board.pin(pin, board.GPIO.IN, board.GPIO.PULL_UP)
|
||||
a = []
|
||||
t1 = time.ticks_us()
|
||||
while L1.value() == 1:
|
||||
t2 = time.ticks_us()
|
||||
if time.ticks_diff(t2, t1) >1000000:
|
||||
#raise ValueError("[MixNo]:Sensor read error")
|
||||
break
|
||||
pass
|
||||
|
||||
time.sleep_us(13560)
|
||||
|
||||
for i in range(1000):
|
||||
v = L1.value()
|
||||
a.append(v)
|
||||
time.sleep_us(56)
|
||||
|
||||
a_c = []
|
||||
count = 0
|
||||
|
||||
for i in a:
|
||||
if i == 1:
|
||||
count += 1
|
||||
|
||||
elif i == 0:
|
||||
if count > 0 :
|
||||
a_c.append(count)
|
||||
count =0
|
||||
|
||||
for i in range(len(a_c)):
|
||||
if a_c[i] > 10:
|
||||
a_c[i] = "1"
|
||||
else:
|
||||
a_c[i] = "0"
|
||||
|
||||
B1 = "".join(a_c)
|
||||
B2 = B1[16:32]
|
||||
#print(len(B1))
|
||||
if len(B1)==32 or len(B1)==33 or len(B1)==46:
|
||||
B3=int(B2,2)
|
||||
return B3
|
||||
else:
|
||||
return None
|
||||
@@ -0,0 +1,97 @@
|
||||
# SPDX-FileCopyrightText: 2017 Yoch <https://github.com/yoch>
|
||||
#
|
||||
# SPDX-License-Identifier: EPL-1.0
|
||||
|
||||
"""
|
||||
`matcher`
|
||||
====================================================================================
|
||||
|
||||
MQTT topic filter matcher from the Eclipse Project's Paho.MQTT.Python
|
||||
https://github.com/eclipse/paho.mqtt.python/blob/master/src/paho/mqtt/matcher.py
|
||||
* Author(s): Yoch (https://github.com/yoch)
|
||||
"""
|
||||
|
||||
|
||||
class MQTTMatcher:
|
||||
"""Intended to manage topic filters including wildcards.
|
||||
|
||||
Internally, MQTTMatcher use a prefix tree (trie) to store
|
||||
values associated with filters, and has an iter_match()
|
||||
method to iterate efficiently over all filters that match
|
||||
some topic name.
|
||||
"""
|
||||
|
||||
# pylint: disable=too-few-public-methods
|
||||
class Node:
|
||||
"""Individual node on the MQTT prefix tree."""
|
||||
|
||||
__slots__ = "children", "content"
|
||||
|
||||
def __init__(self):
|
||||
self.children = {}
|
||||
self.content = None
|
||||
|
||||
def __init__(self):
|
||||
self._root = self.Node()
|
||||
|
||||
def __setitem__(self, key, value):
|
||||
"""Add a topic filter :key to the prefix tree
|
||||
and associate it to :value"""
|
||||
node = self._root
|
||||
for sym in key.split("/"):
|
||||
node = node.children.setdefault(sym, self.Node())
|
||||
node.content = value
|
||||
|
||||
def __getitem__(self, key):
|
||||
"""Retrieve the value associated with some topic filter :key"""
|
||||
try:
|
||||
node = self._root
|
||||
for sym in key.split("/"):
|
||||
node = node.children[sym]
|
||||
if node.content is None:
|
||||
raise KeyError(key)
|
||||
return node.content
|
||||
except KeyError:
|
||||
raise KeyError(key) from None
|
||||
|
||||
def __delitem__(self, key):
|
||||
"""Delete the value associated with some topic filter :key"""
|
||||
lst = []
|
||||
try:
|
||||
parent, node = None, self._root
|
||||
for k in key.split("/"):
|
||||
parent, node = node, node.children[k]
|
||||
lst.append((parent, k, node))
|
||||
node.content = None
|
||||
except KeyError:
|
||||
raise KeyError(key) from None
|
||||
else: # cleanup
|
||||
for parent, k, node in reversed(lst):
|
||||
if node.children or node.content is not None:
|
||||
break
|
||||
del parent.children[k]
|
||||
|
||||
def iter_match(self, topic):
|
||||
"""Return an iterator on all values associated with filters
|
||||
that match the :topic"""
|
||||
lst = topic.split("/")
|
||||
normal = not topic.startswith("$")
|
||||
|
||||
def rec(node, i=0):
|
||||
if i == len(lst):
|
||||
if node.content is not None:
|
||||
yield node.content
|
||||
else:
|
||||
part = lst[i]
|
||||
if part in node.children:
|
||||
for content in rec(node.children[part], i + 1):
|
||||
yield content
|
||||
if "+" in node.children and (normal or i > 0):
|
||||
for content in rec(node.children["+"], i + 1):
|
||||
yield content
|
||||
if "#" in node.children and (normal or i > 0):
|
||||
content = node.children["#"].content
|
||||
if content is not None:
|
||||
yield content
|
||||
|
||||
return rec(self._root)
|
||||
@@ -0,0 +1,260 @@
|
||||
import usocket as socket
|
||||
import ustruct as struct
|
||||
import network,time,board
|
||||
#from ubinascii import hexlify
|
||||
import ujson as json
|
||||
from machine import UART
|
||||
|
||||
wifi_en=board.pin(19,board.GPIO.OUT)
|
||||
board.register(18,board.FPIOA.UART2_TX)
|
||||
board.register(17,board.FPIOA.UART2_RX)
|
||||
|
||||
def wifi_enable(en):
|
||||
global wifi_en
|
||||
wifi_en.value(en)
|
||||
|
||||
def wifi_reset():
|
||||
global uart
|
||||
wifi_enable(0)
|
||||
time.sleep_ms(200)
|
||||
wifi_enable(1)
|
||||
time.sleep(2)
|
||||
uart = UART(UART.UART2,115200,timeout=1000, read_buf_len=4096)
|
||||
tmp = uart.read()
|
||||
uart.write("AT+UART_CUR=921600,8,1,0,0\r\n")
|
||||
print(uart.read())
|
||||
uart = UART(UART.UART2,921600,timeout=1000, read_buf_len=10240) # important! baudrate too low or read_buf_len too small will loose data
|
||||
uart.write("AT\r\n")
|
||||
tmp = uart.read()
|
||||
print(tmp)
|
||||
if not tmp.endswith("OK\r\n"):
|
||||
print("reset fail")
|
||||
return None
|
||||
try:
|
||||
nic = network.ESP8285(uart)
|
||||
except Exception:
|
||||
return None
|
||||
return nic
|
||||
|
||||
def get_data_dict(d):
|
||||
result = {"datastreams":[]}
|
||||
for x in d:
|
||||
result["datastreams"].append({"id":x,"datapoints":[{"value":d[x]}]})
|
||||
return result
|
||||
|
||||
def pubData(value, state):
|
||||
value = get_data_dict(value)
|
||||
jdata = json.dumps(value)
|
||||
jlen = len(jdata)
|
||||
bdata = bytearray(jlen+3)
|
||||
bdata[0] = 1 # publish data in type of json
|
||||
bdata[1] = int(jlen / 256) # data lenght
|
||||
bdata[2] = jlen % 256 # data lenght
|
||||
bdata[3:jlen+4] = jdata.encode('ascii') # json data
|
||||
if state:
|
||||
print(value)
|
||||
print(bdata)
|
||||
return bdata
|
||||
|
||||
def do_connect(account,password):
|
||||
nic=wifi_reset()
|
||||
if not nic:
|
||||
raise Exception("[Cool.AI]:WiFi init fail")
|
||||
|
||||
nic.connect(account,password)
|
||||
nic.ifconfig()
|
||||
|
||||
def init_MQTT_client(sid, address, cid, api, topic, callback):
|
||||
client = MQTTClient(sid, address, 6002, cid, api)
|
||||
client.set_callback(callback)
|
||||
client.connect()
|
||||
client.subscribe(bytes(topic, 'utf-8'))
|
||||
return client
|
||||
|
||||
class MQTTException(Exception):
|
||||
pass
|
||||
|
||||
class MQTTClient:
|
||||
def __init__(self, client_id, server, port=0, user=None, password=None, keepalive=0,ssl=False, ssl_params={}):
|
||||
if port == 0:
|
||||
port = 8883 if ssl else 1883
|
||||
self.client_id = client_id
|
||||
self.sock = None
|
||||
self.addr = socket.getaddrinfo(server, port)[0][-1]
|
||||
self.ssl = ssl
|
||||
self.ssl_params = ssl_params
|
||||
self.pid = 0
|
||||
self.cb = None
|
||||
self.user = user
|
||||
self.pswd = password
|
||||
self.keepalive = keepalive
|
||||
self.lw_topic = None
|
||||
self.lw_msg = None
|
||||
self.lw_qos = 0
|
||||
self.lw_retain = False
|
||||
|
||||
def _send_str(self, s):
|
||||
self.sock.write(struct.pack("!H", len(s)))
|
||||
self.sock.write(s)
|
||||
|
||||
def _recv_len(self):
|
||||
n = 0
|
||||
sh = 0
|
||||
while 1:
|
||||
b = self.sock.read(1)[0]
|
||||
n |= (b & 0x7f) << sh
|
||||
if not b & 0x80:
|
||||
return n
|
||||
sh += 7
|
||||
|
||||
def set_callback(self, f):
|
||||
self.cb = f
|
||||
|
||||
def set_last_will(self, topic, msg, retain=False, qos=0):
|
||||
assert 0 <= qos <= 2
|
||||
assert topic
|
||||
self.lw_topic = topic
|
||||
self.lw_msg = msg
|
||||
self.lw_qos = qos
|
||||
self.lw_retain = retain
|
||||
|
||||
def connect(self, clean_session=True):
|
||||
self.sock = socket.socket()
|
||||
self.sock.connect(self.addr)
|
||||
print(self.addr)
|
||||
if self.ssl:
|
||||
import ussl
|
||||
self.sock = ussl.wrap_socket(self.sock, **self.ssl_params)
|
||||
msg = bytearray(b"\x10\0\0\x04MQTT\x04\x02\0\0")
|
||||
msg[1] = 10 + 2 + len(self.client_id)
|
||||
msg[9] = clean_session << 1
|
||||
if self.user is not None:
|
||||
msg[1] += 2 + len(self.user) + 2 + len(self.pswd)
|
||||
msg[9] |= 0xC0
|
||||
if self.keepalive:
|
||||
assert self.keepalive < 65536
|
||||
msg[10] |= self.keepalive >> 8
|
||||
msg[11] |= self.keepalive & 0x00FF
|
||||
if self.lw_topic:
|
||||
msg[1] += 2 + len(self.lw_topic) + 2 + len(self.lw_msg)
|
||||
msg[9] |= 0x4 | (self.lw_qos & 0x1) << 3 | (self.lw_qos & 0x2) << 3
|
||||
msg[9] |= self.lw_retain << 5
|
||||
self.sock.write(msg)
|
||||
#print(hex(len(msg)), hexlify(msg, ":"))
|
||||
self._send_str(self.client_id)
|
||||
if self.lw_topic:
|
||||
self._send_str(self.lw_topic)
|
||||
self._send_str(self.lw_msg)
|
||||
if self.user is not None:
|
||||
self._send_str(self.user)
|
||||
self._send_str(self.pswd)
|
||||
resp = self.sock.read(4)
|
||||
assert resp[0] == 0x20 and resp[1] == 0x02
|
||||
if resp[3] != 0:
|
||||
raise MQTTException(resp[3])
|
||||
return resp[2] & 1
|
||||
|
||||
def disconnect(self):
|
||||
self.sock.write(b"\xe0\0")
|
||||
self.sock.close()
|
||||
|
||||
def ping(self):
|
||||
self.sock.write(b"\xc0\0")
|
||||
|
||||
def publish(self, msg, is_print=True, topic='$dp', retain=False, qos=0):
|
||||
msg = pubData(msg, is_print)
|
||||
pkt = bytearray(b"\x30\0\0\0")
|
||||
pkt[0] |= qos << 1 | retain
|
||||
sz = 2 + len(topic) + len(msg)
|
||||
if qos > 0:
|
||||
sz += 2
|
||||
assert sz < 2097152
|
||||
i = 1
|
||||
while sz > 0x7f:
|
||||
pkt[i] = (sz & 0x7f) | 0x80
|
||||
sz >>= 7
|
||||
i += 1
|
||||
pkt[i] = sz
|
||||
#print(hex(len(pkt)), hexlify(pkt, ":"))
|
||||
self.sock.write(pkt, i + 1)
|
||||
self._send_str(topic)
|
||||
if qos > 0:
|
||||
self.pid += 1
|
||||
pid = self.pid
|
||||
struct.pack_into("!H", pkt, 0, pid)
|
||||
self.sock.write(pkt, 2)
|
||||
self.sock.write(msg)
|
||||
if qos == 1:
|
||||
while 1:
|
||||
op = self.wait_msg()
|
||||
if op == 0x40:
|
||||
sz = self.sock.read(1)
|
||||
assert sz == b"\x02"
|
||||
rcv_pid = self.sock.read(2)
|
||||
rcv_pid = rcv_pid[0] << 8 | rcv_pid[1]
|
||||
if pid == rcv_pid:
|
||||
return
|
||||
elif qos == 2:
|
||||
assert 0
|
||||
|
||||
def subscribe(self, topic, qos=0):
|
||||
assert self.cb is not None, "Subscribe callback is not set"
|
||||
pkt = bytearray(b"\x82\0\0\0")
|
||||
self.pid += 1
|
||||
struct.pack_into("!BH", pkt, 1, 2 + 2 + len(topic) + 1, self.pid)
|
||||
#print(hex(len(pkt)), hexlify(pkt, ":"))
|
||||
self.sock.write(pkt)
|
||||
self._send_str(topic)
|
||||
self.sock.write(qos.to_bytes(1, "little"))
|
||||
while 1:
|
||||
op = self.wait_msg()
|
||||
if op == 0x90:
|
||||
resp = self.sock.read(4)
|
||||
#print(resp)
|
||||
assert resp[1] == pkt[2] and resp[2] == pkt[3]
|
||||
if resp[3] == 0x80:
|
||||
raise MQTTException(resp[3])
|
||||
return
|
||||
|
||||
# Wait for a single incoming MQTT message and process it.
|
||||
# Subscribed messages are delivered to a callback previously
|
||||
# set by .set_callback() method. Other (internal) MQTT
|
||||
# messages processed internally.
|
||||
def wait_msg(self):
|
||||
res = self.sock.read(1)
|
||||
self.sock.setblocking(True)
|
||||
if res is None:
|
||||
return None
|
||||
if res == b"":
|
||||
raise OSError(-1)
|
||||
if res == b"\xd0": # PINGRESP
|
||||
sz = self.sock.read(1)[0]
|
||||
assert sz == 0
|
||||
return None
|
||||
op = res[0]
|
||||
if op & 0xf0 != 0x30:
|
||||
return op
|
||||
sz = self._recv_len()
|
||||
topic_len = self.sock.read(2)
|
||||
topic_len = (topic_len[0] << 8) | topic_len[1]
|
||||
topic = self.sock.read(topic_len)
|
||||
sz -= topic_len + 2
|
||||
if op & 6:
|
||||
pid = self.sock.read(2)
|
||||
pid = pid[0] << 8 | pid[1]
|
||||
sz -= 2
|
||||
msg = self.sock.read(sz)
|
||||
self.cb(topic, msg)
|
||||
if op & 6 == 2:
|
||||
pkt = bytearray(b"\x40\x02\0\0")
|
||||
struct.pack_into("!H", pkt, 2, pid)
|
||||
self.sock.write(pkt)
|
||||
elif op & 6 == 4:
|
||||
assert 0
|
||||
|
||||
# Checks whether a pending message from server is available.
|
||||
# If not, returns immediately with None. Otherwise, does
|
||||
# the same processing as wait_msg.
|
||||
def check_msg(self):
|
||||
self.sock.setblocking(False)
|
||||
return self.wait_msg()
|
||||
@@ -0,0 +1,312 @@
|
||||
import usocket as socket
|
||||
import ustruct as struct
|
||||
import time
|
||||
import machine
|
||||
from ubinascii import hexlify
|
||||
import ujson as json
|
||||
from matcher import MQTTMatcher
|
||||
from machine import Timer
|
||||
|
||||
ADDITIONAL_TOPIC = 'b640a0ce465fa2a4150c36b305c1c11b'
|
||||
WILL_TOPIC = '9d634e1a156dc0c1611eb4c3cff57276'
|
||||
|
||||
|
||||
def init_MQTT_client(address, username, password,MQTT_USR_PRJ):
|
||||
client = MQTTClient(hexlify(machine.unique_id()), address, 1883, username, password)
|
||||
client.set_last_will(topic=MQTT_USR_PRJ+WILL_TOPIC, msg=client.client_id, qos=2)
|
||||
if client.connect()==0:
|
||||
client.publish(MQTT_USR_PRJ+ADDITIONAL_TOPIC, client.client_id, qos=1)
|
||||
Timer(Timer.TIMER2,Timer.CHANNEL3,mode=Timer.MODE_PERIODIC,period = 10000, callback = lambda x : client.ping())
|
||||
return client
|
||||
|
||||
len_overrided = len
|
||||
|
||||
# Add by Mixly Team
|
||||
def len(object):
|
||||
if isinstance(object, str):
|
||||
return len_overrided(object.encode('utf-8'))
|
||||
else:
|
||||
return len_overrided(object)
|
||||
#####################################################
|
||||
|
||||
class MQTTException(Exception):
|
||||
pass
|
||||
|
||||
class MQTTClient:
|
||||
def __init__(self, client_id, server, port=0, username=None, password=None, keepalive=60, ssl=False, ssl_params={}):
|
||||
if port == 0:
|
||||
port = 8883 if ssl else 1883
|
||||
self.client_id = client_id
|
||||
self.sock = None
|
||||
self.addr = socket.getaddrinfo(server, port)[0][-1]
|
||||
self.ssl = ssl
|
||||
self.ssl_params = ssl_params
|
||||
self.pid = 0
|
||||
#self.cb = None
|
||||
self._on_message = None
|
||||
self.username = username
|
||||
self.password = password
|
||||
#self.project = project
|
||||
self.keepalive = keepalive
|
||||
self.lw_topic = None
|
||||
self.lw_msg = None
|
||||
self.lw_qos = 0
|
||||
self.lw_retain = False
|
||||
self._on_message_filtered = MQTTMatcher()
|
||||
|
||||
def _send_str(self, s):
|
||||
self.sock.write(struct.pack("!H", len(s)))
|
||||
self.sock.write(s)
|
||||
|
||||
def _recv_len(self):
|
||||
n = 0
|
||||
sh = 0
|
||||
while 1:
|
||||
b = self.sock.read(1)[0]
|
||||
n |= (b & 0x7f) << sh
|
||||
if not b & 0x80:
|
||||
return n
|
||||
sh += 7
|
||||
|
||||
# def set_callback(self, f):
|
||||
# self.cb = f
|
||||
|
||||
def set_callback(self, mqtt_topic, callback_method, MQTT_USR_PRJ):
|
||||
"""Registers a callback_method for a specific MQTT topic.
|
||||
|
||||
:param str mqtt_topic: MQTT topic identifier.
|
||||
:param str callback_method: Name of callback method.
|
||||
"""
|
||||
if mqtt_topic is None or callback_method is None:
|
||||
raise ValueError("MQTT topic and callback method must both be defined.")
|
||||
self._on_message_filtered[MQTT_USR_PRJ+mqtt_topic] = callback_method
|
||||
|
||||
def remove_callback(self, mqtt_topic):
|
||||
"""Removes a registered callback method.
|
||||
|
||||
:param str mqtt_topic: MQTT topic identifier string.
|
||||
"""
|
||||
if mqtt_topic is None:
|
||||
raise ValueError("MQTT Topic must be defined.")
|
||||
try:
|
||||
del self._on_message_filtered[mqtt_topic]
|
||||
except KeyError:
|
||||
raise KeyError(
|
||||
"MQTT topic callback not added with add_topic_callback."
|
||||
) from None
|
||||
|
||||
@property
|
||||
def on_message(self):
|
||||
"""Called when a new message has been received on a subscribed topic.
|
||||
|
||||
Expected method signature is ``on_message(client, topic, message)``
|
||||
"""
|
||||
return self._on_message
|
||||
|
||||
@on_message.setter
|
||||
def on_message(self, method):
|
||||
self._on_message = method
|
||||
|
||||
def _handle_on_message(self, client, topic, message):
|
||||
matched = False
|
||||
if topic is not None:
|
||||
for callback in self._on_message_filtered.iter_match(topic):
|
||||
callback(client, topic, message) # on_msg with callback
|
||||
matched = True
|
||||
|
||||
if not matched and self.on_message: # regular on_message
|
||||
self.on_message(client, topic, message)
|
||||
|
||||
def set_last_will(self, topic, msg, retain=False, qos=0):
|
||||
assert 0 <= qos <= 2
|
||||
assert topic
|
||||
self.lw_topic = topic
|
||||
self.lw_msg = msg
|
||||
self.lw_qos = qos
|
||||
self.lw_retain = retain
|
||||
|
||||
def connect(self, clean_session=True):
|
||||
self.sock = socket.socket()
|
||||
self.sock.connect(self.addr)
|
||||
print(self.addr)
|
||||
if self.ssl:
|
||||
import ussl
|
||||
self.sock = ussl.wrap_socket(self.sock, **self.ssl_params)
|
||||
msg_header=bytearray([0x10])
|
||||
msg = bytearray(b"\x04MQTT\x04\x02\0\0")
|
||||
msg_length = 12 + len(self.client_id)
|
||||
msg[6] = clean_session << 1
|
||||
|
||||
if self.username is not None:
|
||||
msg_length += 2 + len(self.username) + 2 + len(self.password)
|
||||
msg[6] |= 0xC0
|
||||
if self.keepalive:
|
||||
assert self.keepalive < 65536
|
||||
msg[7] |= self.keepalive >> 8
|
||||
msg[8] |= self.keepalive & 0x00FF
|
||||
if self.lw_topic:
|
||||
msg_length += 2 + len(self.lw_topic) + 2 + len(self.lw_msg)
|
||||
msg[6] |= 0x4 | (self.lw_qos & 0x1) << 3 | (self.lw_qos & 0x2) << 3
|
||||
msg[6] |= self.lw_retain << 5
|
||||
|
||||
if msg_length > 0x7F:
|
||||
while msg_length>0:
|
||||
encoded_byte = msg_length % 0x80
|
||||
msg_length = msg_length // 0x80
|
||||
if msg_length > 0:
|
||||
encoded_byte |= 0x80
|
||||
msg_header.append(encoded_byte)
|
||||
msg_header.append(0x00)
|
||||
else:
|
||||
msg_header.append(msg_length)
|
||||
msg_header.append(0x00)
|
||||
|
||||
self.sock.write(msg_header)
|
||||
self.sock.write(msg)
|
||||
#print(hexlify(msg_header, ":"), hexlify(msg, ":"))
|
||||
self._send_str(self.client_id)
|
||||
if self.lw_topic:
|
||||
self._send_str(self.lw_topic)
|
||||
self._send_str(self.lw_msg)
|
||||
if self.username is not None:
|
||||
self._send_str(self.username)
|
||||
self._send_str(self.password)
|
||||
resp = self.sock.read(4)
|
||||
assert resp[0] == 0x20 and resp[1] == 0x02
|
||||
if resp[3] != 0:
|
||||
raise MQTTException(resp[3])
|
||||
return resp[2] & 1
|
||||
|
||||
|
||||
def disconnect(self,MQTT_USR_PRJ):
|
||||
#MQTT_USR_PRJ = "{}/{}/".format(self.username,self.project)
|
||||
self.publish(MQTT_USR_PRJ+WILL_TOPIC, self.client_id, qos=1)
|
||||
self.sock.write(b"\xe0\0")
|
||||
self.sock.close()
|
||||
|
||||
def ping(self):
|
||||
self.sock.write(b"\xc0\0")
|
||||
|
||||
def pingSync(self):
|
||||
time.ticks_ms()
|
||||
self.ping()
|
||||
for i in range(0,10):
|
||||
msg = self.check_msg()
|
||||
if msg == "PINGRESP":
|
||||
return True
|
||||
time.sleep_ms(100)
|
||||
return False
|
||||
|
||||
def publish(self, topic, msg, retain=False, qos=0):
|
||||
# msg = pubData(msg)
|
||||
if "+" in topic or "#" in topic:
|
||||
raise MQTTException("Publish topic can not contain wildcards.")
|
||||
# check msg/qos kwargs
|
||||
if msg is None:
|
||||
raise MQTTException("Message can not be None.")
|
||||
if isinstance(msg, (int, float)):
|
||||
msg = str(msg).encode("ascii")
|
||||
elif isinstance(msg, str):
|
||||
msg = str(msg).encode("utf-8")
|
||||
elif isinstance(msg, bytes):
|
||||
pass
|
||||
else:
|
||||
raise MQTTException("Invalid message data type.")
|
||||
pkt = bytearray(b"\x30\0\0\0")
|
||||
pkt[0] |= qos << 1 | retain
|
||||
sz = 2 + len(topic) + len(msg)
|
||||
if qos > 0:
|
||||
sz += 2
|
||||
assert sz < 2097152
|
||||
i = 1
|
||||
while sz > 0x7f:
|
||||
pkt[i] = (sz & 0x7f) | 0x80
|
||||
sz >>= 7
|
||||
i += 1
|
||||
pkt[i] = sz
|
||||
#print(hex(len(pkt)), hexlify(pkt, ":"))
|
||||
self.sock.write(pkt, i + 1)
|
||||
self._send_str(topic)
|
||||
if qos > 0:
|
||||
self.pid += 1
|
||||
pid = self.pid
|
||||
struct.pack_into("!H", pkt, 0, pid)
|
||||
self.sock.write(pkt, 2)
|
||||
self.sock.write(msg)
|
||||
if qos == 1:
|
||||
while 1:
|
||||
op = self.wait_msg()
|
||||
if op == 0x40:
|
||||
sz = self.sock.read(1)
|
||||
assert sz == b"\x02"
|
||||
rcv_pid = self.sock.read(2)
|
||||
rcv_pid = rcv_pid[0] << 8 | rcv_pid[1]
|
||||
if pid == rcv_pid:
|
||||
return
|
||||
elif qos == 2:
|
||||
assert 0
|
||||
|
||||
def subscribe(self, topic, qos=0):
|
||||
#assert self.cb is not None, "Subscribe callback is not set"
|
||||
pkt = bytearray(b"\x82\0\0\0")
|
||||
self.pid += 1
|
||||
if isinstance(topic, str):
|
||||
topic=topic.encode()
|
||||
struct.pack_into("!BH", pkt, 1, 2 + 2 + len(topic) + 1, self.pid)
|
||||
#print(hex(len(pkt)), hexlify(pkt, ":"))
|
||||
self.sock.write(pkt)
|
||||
self._send_str(topic)
|
||||
self.sock.write(qos.to_bytes(1, "little"))
|
||||
while 1:
|
||||
op = self.wait_msg()
|
||||
if op == 0x90:
|
||||
resp = self.sock.read(4)
|
||||
#print(resp)
|
||||
assert resp[1] == pkt[2] and resp[2] == pkt[3]
|
||||
if resp[3] == 0x80:
|
||||
raise MQTTException(resp[3])
|
||||
return
|
||||
|
||||
# Wait for a single incoming MQTT message and process it.
|
||||
# Subscribed messages are delivered to a callback previously
|
||||
# set by .set_callback() method. Other (internal) MQTT
|
||||
# messages processed internally.
|
||||
def wait_msg(self):
|
||||
res = self.sock.read(1)
|
||||
self.sock.setblocking(True)
|
||||
if res is None:
|
||||
return None
|
||||
if res == b"":
|
||||
raise OSError(-1)
|
||||
if res == b"\xd0": # PINGRESP
|
||||
sz = self.sock.read(1)[0]
|
||||
assert sz == 0
|
||||
return "PINGRESP"
|
||||
op = res[0]
|
||||
if op & 0xf0 != 0x30:
|
||||
return op
|
||||
sz = self._recv_len()
|
||||
topic_len = self.sock.read(2)
|
||||
topic_len = (topic_len[0] << 8) | topic_len[1]
|
||||
topic = self.sock.read(topic_len)
|
||||
sz -= topic_len + 2
|
||||
if op & 6:
|
||||
pid = self.sock.read(2)
|
||||
pid = pid[0] << 8 | pid[1]
|
||||
sz -= 2
|
||||
msg = self.sock.read(sz)
|
||||
self._handle_on_message(self, str(topic, "utf-8"), str(msg, "utf-8"))
|
||||
#self.cb(topic.decode(), msg.decode())
|
||||
if op & 6 == 2:
|
||||
pkt = bytearray(b"\x40\x02\0\0")
|
||||
struct.pack_into("!H", pkt, 2, pid)
|
||||
self.sock.write(pkt)
|
||||
elif op & 6 == 4:
|
||||
assert 0
|
||||
|
||||
# Checks whether a pending message from server is available.
|
||||
# If not, returns immediately with None. Otherwise, does
|
||||
# the same processing as wait_msg.
|
||||
def check_msg(self):
|
||||
self.sock.setblocking(False)
|
||||
return self.wait_msg()
|
||||
@@ -0,0 +1,25 @@
|
||||
import hashlib
|
||||
import os
|
||||
|
||||
def sha_file(f):
|
||||
if f not in set(os.listdir(".")):
|
||||
return 'None:::sha_file_end'
|
||||
else:
|
||||
sha = hashlib.sha256()
|
||||
with open(f, encoding='utf-8') as fd:
|
||||
file_buffer = fd.read(128).encode("utf-8")
|
||||
while len(file_buffer) > 0:
|
||||
sha.update(file_buffer)
|
||||
file_buffer = fd.read(128).encode("utf-8")
|
||||
h = sha.digest()
|
||||
return ''.join(['%.2x' % i for i in h]) + ":::sha_file_end"
|
||||
|
||||
def reload(mod):
|
||||
import sys
|
||||
mod_name = mod.__name__
|
||||
try:
|
||||
del sys.modules[mod_name]
|
||||
__import__(mod_name)
|
||||
except:
|
||||
pass
|
||||
|
||||
@@ -0,0 +1,84 @@
|
||||
#coding=utf-8
|
||||
import math
|
||||
|
||||
def math_map(v, al, ah, bl, bh):
|
||||
if al==ah:
|
||||
return bl
|
||||
if al > ah:
|
||||
al, ah = ah, al
|
||||
if v > ah:
|
||||
v = ah
|
||||
if v < al:
|
||||
v = al
|
||||
return bl + (bh - bl) * (v - al) / (ah - al)
|
||||
|
||||
def math_mean(myList):
|
||||
localList = [e for e in myList if type(e) == int or type(e) == float]
|
||||
if not localList: return
|
||||
return float(sum(localList)) / len(localList)
|
||||
|
||||
def math_median(myList):
|
||||
localList = sorted([e for e in myList if type(e) == int or type(e) == float])
|
||||
if not localList: return
|
||||
if len(localList) % 2 == 0:
|
||||
return (localList[len(localList) // 2 - 1] + localList[len(localList) // 2]) / 2.0
|
||||
else:
|
||||
return localList[(len(localList) - 1) // 2]
|
||||
|
||||
def math_modes(some_list):
|
||||
modes = []
|
||||
# Using a lists of [item, count] to keep count rather than dict
|
||||
# to avoid "unhashable" errors when the counted item is itself a list or dict.
|
||||
counts = []
|
||||
maxCount = 1
|
||||
for item in some_list:
|
||||
found = False
|
||||
for count in counts:
|
||||
if count[0] == item:
|
||||
count[1] += 1
|
||||
maxCount = max(maxCount, count[1])
|
||||
found = True
|
||||
if not found:
|
||||
counts.append([item, 1])
|
||||
for counted_item, item_count in counts:
|
||||
if item_count == maxCount:
|
||||
modes.append(counted_item)
|
||||
return modes
|
||||
|
||||
def math_standard_deviation(numbers):
|
||||
n = len(numbers)
|
||||
if n == 0: return
|
||||
mean = float(sum(numbers)) / n
|
||||
variance = sum((x - mean) ** 2 for x in numbers) / n
|
||||
return math.sqrt(variance)
|
||||
|
||||
def lists_sort(my_list, type, reverse):
|
||||
def try_float(s):
|
||||
try:
|
||||
return float(s)
|
||||
except:
|
||||
return 0
|
||||
key_funcs = {
|
||||
"NUMERIC": try_float,
|
||||
"TEXT": str,
|
||||
"IGNORE_CASE": lambda s: str(s).lower()
|
||||
}
|
||||
key_func = key_funcs[type]
|
||||
list_cpy = list(my_list)
|
||||
return sorted(list_cpy, key=key_func, reverse=reverse)
|
||||
|
||||
def format_content(mydict, cid):
|
||||
if 'lat' in mydict and 'long' in mydict:
|
||||
res = '{'+'"lat": "{}", "long": "{}", "clientid": "{}"'.format(mydict.pop('lat'),mydict.pop('long'),cid)
|
||||
if len(mydict)>0:
|
||||
res += ', "message": ['
|
||||
for d in mydict:
|
||||
res += '{{"label": "{}", "value": "{}"}},'.format(d,mydict[d])
|
||||
res = res[:-1] + "]"
|
||||
res += '}'
|
||||
return res
|
||||
else:
|
||||
print('Invalid Input')
|
||||
|
||||
def format_str(d):
|
||||
return str(d).replace("'",'"')
|
||||
@@ -0,0 +1,566 @@
|
||||
#
|
||||
# This file is part of MicroPython MPU9250 driver
|
||||
# Copyright (c) 2018 Mika Tuupola
|
||||
#
|
||||
# Licensed under the MIT license:
|
||||
# http://www.opensource.org/licenses/mit-license.php
|
||||
#
|
||||
# Project home:
|
||||
# https://github.com/tuupola/micropython-mpu9250
|
||||
#
|
||||
|
||||
"""
|
||||
MicroPython I2C driver for MPU9250 9-axis motion tracking device
|
||||
"""
|
||||
|
||||
# pylint: disable=import-error
|
||||
from micropython import const
|
||||
import ustruct
|
||||
import utime
|
||||
import time
|
||||
import math
|
||||
#from machine import I2C, Pin
|
||||
# pylint: enable=import-error
|
||||
__version__ = "0.2.0"
|
||||
# pylint: disable=import-error
|
||||
# pylint: enable=import-error
|
||||
|
||||
_GYRO_CONFIG = const(0x1b)
|
||||
_ACCEL_CONFIG = const(0x1c)
|
||||
_ACCEL_CONFIG2 = const(0x1d)
|
||||
_INT_PIN_CFG = const(0x37)
|
||||
_ACCEL_XOUT_H = const(0x3b)
|
||||
_ACCEL_XOUT_L = const(0x3c)
|
||||
_ACCEL_YOUT_H = const(0x3d)
|
||||
_ACCEL_YOUT_L = const(0x3e)
|
||||
_ACCEL_ZOUT_H = const(0x3f)
|
||||
_ACCEL_ZOUT_L= const(0x40)
|
||||
_TEMP_OUT_H = const(0x41)
|
||||
_TEMP_OUT_L = const(0x42)
|
||||
_GYRO_XOUT_H = const(0x43)
|
||||
_GYRO_XOUT_L = const(0x44)
|
||||
_GYRO_YOUT_H = const(0x45)
|
||||
_GYRO_YOUT_L = const(0x46)
|
||||
_GYRO_ZOUT_H = const(0x47)
|
||||
_GYRO_ZOUT_L = const(0x48)
|
||||
_WHO_AM_I = const(0x75)
|
||||
|
||||
#_ACCEL_FS_MASK = const(0b00011000)
|
||||
ACCEL_FS_SEL_2G = const(0b00000000)
|
||||
ACCEL_FS_SEL_4G = const(0b00001000)
|
||||
ACCEL_FS_SEL_8G = const(0b00010000)
|
||||
ACCEL_FS_SEL_16G = const(0b00011000)
|
||||
|
||||
_ACCEL_SO_2G = 16384 # 1 / 16384 ie. 0.061 mg / digit
|
||||
_ACCEL_SO_4G = 8192 # 1 / 8192 ie. 0.122 mg / digit
|
||||
_ACCEL_SO_8G = 4096 # 1 / 4096 ie. 0.244 mg / digit
|
||||
_ACCEL_SO_16G = 2048 # 1 / 2048 ie. 0.488 mg / digit
|
||||
|
||||
#_GYRO_FS_MASK = const(0b00011000)
|
||||
GYRO_FS_SEL_250DPS = const(0b00000000)
|
||||
GYRO_FS_SEL_500DPS = const(0b00001000)
|
||||
GYRO_FS_SEL_1000DPS = const(0b00010000)
|
||||
GYRO_FS_SEL_2000DPS = const(0b00011000)
|
||||
|
||||
_GYRO_SO_250DPS = 131
|
||||
_GYRO_SO_500DPS = 62.5
|
||||
_GYRO_SO_1000DPS = 32.8
|
||||
_GYRO_SO_2000DPS = 16.4
|
||||
|
||||
# Used for enablind and disabling the i2c bypass access
|
||||
_I2C_BYPASS_MASK = const(0b00000010)
|
||||
_I2C_BYPASS_EN = const(0b00000010)
|
||||
_I2C_BYPASS_DIS = const(0b00000000)
|
||||
|
||||
SF_G = 1
|
||||
SF_M_S2 = 9.80665 # 1 g = 9.80665 m/s2 ie. standard gravity
|
||||
SF_DEG_S = 1
|
||||
SF_RAD_S = 57.295779578552 # 1 rad/s is 57.295779578552 deg/s
|
||||
|
||||
|
||||
_WIA = const(0x00)
|
||||
_HXL = const(0x03)
|
||||
_HXH = const(0x04)
|
||||
_HYL = const(0x05)
|
||||
_HYH = const(0x06)
|
||||
_HZL = const(0x07)
|
||||
_HZH = const(0x08)
|
||||
_ST2 = const(0x09)
|
||||
_CNTL1 = const(0x0a)
|
||||
_ASAX = const(0x10)
|
||||
_ASAY = const(0x11)
|
||||
_ASAZ = const(0x12)
|
||||
|
||||
_MODE_POWER_DOWN = 0b00000000
|
||||
MODE_SINGLE_MEASURE = 0b00000001
|
||||
MODE_CONTINOUS_MEASURE_1 = 0b00000010 # 8Hz
|
||||
MODE_CONTINOUS_MEASURE_2 = 0b00000110 # 100Hz
|
||||
MODE_EXTERNAL_TRIGGER_MEASURE = 0b00000100
|
||||
_MODE_SELF_TEST = 0b00001000
|
||||
_MODE_FUSE_ROM_ACCESS = 0b00001111
|
||||
|
||||
OUTPUT_14_BIT = 0b00000000
|
||||
OUTPUT_16_BIT = 0b00010000
|
||||
|
||||
_SO_14BIT = 0.6 # 渭T per digit when 14bit mode
|
||||
_SO_16BIT = 0.15 # 渭T per digit when 16bit mode
|
||||
|
||||
class MPU6500:
|
||||
"""Class which provides interface to MPU6500 6-axis motion tracking device."""
|
||||
def __init__(
|
||||
self, i2c, address=0x68,
|
||||
accel_fs=ACCEL_FS_SEL_2G, gyro_fs=GYRO_FS_SEL_250DPS,
|
||||
accel_sf=SF_M_S2, gyro_sf=SF_RAD_S
|
||||
):
|
||||
self.i2c = i2c
|
||||
self.address = address
|
||||
|
||||
if 0x71 != self.whoami:
|
||||
raise RuntimeError("MPU6500 not found in I2C bus.")
|
||||
|
||||
self._accel_so = self._accel_fs(accel_fs)
|
||||
self._gyro_so = self._gyro_fs(gyro_fs)
|
||||
self._accel_sf = accel_sf
|
||||
self._gyro_sf = gyro_sf
|
||||
|
||||
# Enable I2C bypass to access for MPU9250 magnetometer access.
|
||||
char = self._register_char(_INT_PIN_CFG)
|
||||
char &= ~_I2C_BYPASS_MASK # clear I2C bits
|
||||
char |= _I2C_BYPASS_EN
|
||||
self._register_char(_INT_PIN_CFG, char)
|
||||
|
||||
@property
|
||||
def temperature(self):
|
||||
tempbuf=self._register_short(0x41)
|
||||
return tempbuf/333.87 + 21 # I think
|
||||
|
||||
# @property
|
||||
def acceleration(self):
|
||||
"""
|
||||
Acceleration measured by the sensor. By default will return a
|
||||
3-tuple of X, Y, Z axis acceleration values in m/s^2 as floats. Will
|
||||
return values in g if constructor was provided `accel_sf=SF_M_S2`
|
||||
parameter.
|
||||
"""
|
||||
so = self._accel_so
|
||||
sf = self._accel_sf
|
||||
|
||||
xyz = self._register_three_shorts(_ACCEL_XOUT_H)
|
||||
return tuple([value / so * sf for value in xyz])
|
||||
|
||||
@property
|
||||
def gyro(self):
|
||||
"""
|
||||
X, Y, Z radians per second as floats.
|
||||
"""
|
||||
so = self._gyro_so
|
||||
sf = self._gyro_sf
|
||||
|
||||
xyz = self._register_three_shorts(_GYRO_XOUT_H)
|
||||
return tuple([value / so * sf for value in xyz])
|
||||
|
||||
@property
|
||||
def whoami(self):
|
||||
""" Value of the whoami register. """
|
||||
return self._register_char(_WHO_AM_I)
|
||||
|
||||
def _register_short(self, register, value=None, buf=bytearray(2)):
|
||||
if value is None:
|
||||
self.i2c.readfrom_mem_into(self.address, register, buf)
|
||||
return ustruct.unpack(">h", buf)[0]
|
||||
|
||||
ustruct.pack_into(">h", buf, 0, value)
|
||||
return self.i2c.writeto_mem(self.address, register, buf)
|
||||
|
||||
def _register_three_shorts(self, register, buf=bytearray(6)):
|
||||
self.i2c.readfrom_mem_into(self.address, register, buf)
|
||||
return ustruct.unpack(">hhh", buf)
|
||||
|
||||
def _register_char(self, register, value=None, buf=bytearray(1)):
|
||||
if value is None:
|
||||
self.i2c.readfrom_mem_into(self.address, register, buf)
|
||||
return buf[0]
|
||||
|
||||
ustruct.pack_into("<b", buf, 0, value)
|
||||
return self.i2c.writeto_mem(self.address, register, buf)
|
||||
|
||||
def _accel_fs(self, value):
|
||||
self._register_char(_ACCEL_CONFIG, value)
|
||||
|
||||
# Return the sensitivity divider
|
||||
if ACCEL_FS_SEL_2G == value:
|
||||
return _ACCEL_SO_2G
|
||||
elif ACCEL_FS_SEL_4G == value:
|
||||
return _ACCEL_SO_4G
|
||||
elif ACCEL_FS_SEL_8G == value:
|
||||
return _ACCEL_SO_8G
|
||||
elif ACCEL_FS_SEL_16G == value:
|
||||
return _ACCEL_SO_16G
|
||||
|
||||
def _gyro_fs(self, value):
|
||||
self._register_char(_GYRO_CONFIG, value)
|
||||
|
||||
# Return the sensitivity divider
|
||||
if GYRO_FS_SEL_250DPS == value:
|
||||
return _GYRO_SO_250DPS
|
||||
elif GYRO_FS_SEL_500DPS == value:
|
||||
return _GYRO_SO_500DPS
|
||||
elif GYRO_FS_SEL_1000DPS == value:
|
||||
return _GYRO_SO_1000DPS
|
||||
elif GYRO_FS_SEL_2000DPS == value:
|
||||
return _GYRO_SO_2000DPS
|
||||
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
def __exit__(self, exception_type, exception_value, traceback):
|
||||
pass
|
||||
|
||||
class AK8963:
|
||||
"""Class which provides interface to AK8963 magnetometer."""
|
||||
def __init__(
|
||||
self, i2c, address=0x0c,
|
||||
mode=MODE_CONTINOUS_MEASURE_1, output=OUTPUT_16_BIT,
|
||||
offset=(0, 0, 0), scale=(1, 1, 1)
|
||||
):
|
||||
self.i2c = i2c
|
||||
self.address = address
|
||||
self._offset = offset
|
||||
self._scale = scale
|
||||
|
||||
if 0x48 != self.whoami:
|
||||
raise RuntimeError("AK8963 not found in I2C bus.")
|
||||
|
||||
# Sensitivity adjustement values
|
||||
self._register_char(_CNTL1, _MODE_FUSE_ROM_ACCESS)
|
||||
asax = self._register_char(_ASAX)
|
||||
asay = self._register_char(_ASAY)
|
||||
asaz = self._register_char(_ASAZ)
|
||||
self._register_char(_CNTL1, _MODE_POWER_DOWN)
|
||||
|
||||
# Should wait atleast 100us before next mode
|
||||
self._adjustement = (
|
||||
(0.5 * (asax - 128)) / 128 + 1,
|
||||
(0.5 * (asay - 128)) / 128 + 1,
|
||||
(0.5 * (asaz - 128)) / 128 + 1
|
||||
)
|
||||
|
||||
# Power on
|
||||
self._register_char(_CNTL1, (mode | output))
|
||||
|
||||
if output is OUTPUT_16_BIT:
|
||||
self._so = _SO_16BIT
|
||||
else:
|
||||
self._so = _SO_14BIT
|
||||
|
||||
|
||||
@property
|
||||
def magnetic(self):
|
||||
"""
|
||||
X, Y, Z axis micro-Tesla (uT) as floats.
|
||||
"""
|
||||
xyz = list(self._register_three_shorts(_HXL))
|
||||
self._register_char(_ST2) # Enable updating readings again
|
||||
|
||||
# Apply factory axial sensitivy adjustements
|
||||
xyz[0] *= self._adjustement[0]
|
||||
xyz[1] *= self._adjustement[1]
|
||||
xyz[2] *= self._adjustement[2]
|
||||
|
||||
# Apply output scale determined in constructor
|
||||
so = self._so
|
||||
xyz[0] *= so
|
||||
xyz[1] *= so
|
||||
xyz[2] *= so
|
||||
|
||||
# Apply hard iron ie. offset bias from calibration
|
||||
xyz[0] -= self._offset[0]
|
||||
xyz[1] -= self._offset[1]
|
||||
xyz[2] -= self._offset[2]
|
||||
|
||||
# Apply soft iron ie. scale bias from calibration
|
||||
xyz[0] *= self._scale[0]
|
||||
xyz[1] *= self._scale[1]
|
||||
xyz[2] *= self._scale[2]
|
||||
|
||||
return tuple(xyz)
|
||||
|
||||
@property
|
||||
def adjustement(self):
|
||||
return self._adjustement
|
||||
|
||||
@property
|
||||
def whoami(self):
|
||||
""" Value of the whoami register. """
|
||||
return self._register_char(_WIA)
|
||||
|
||||
def calibrate(self, count=3, delay=200):
|
||||
self._offset = (0, 0, 0)
|
||||
self._scale = (1, 1, 1)
|
||||
|
||||
reading = self.magnetic
|
||||
minx = maxx = reading[0]
|
||||
miny = maxy = reading[1]
|
||||
minz = maxz = reading[2]
|
||||
|
||||
while count:
|
||||
utime.sleep_ms(delay)
|
||||
reading = self.magnetic
|
||||
minx = min(minx, reading[0])
|
||||
maxx = max(maxx, reading[0])
|
||||
miny = min(miny, reading[1])
|
||||
maxy = max(maxy, reading[1])
|
||||
minz = min(minz, reading[2])
|
||||
maxz = max(maxz, reading[2])
|
||||
count -= 1
|
||||
|
||||
|
||||
# Hard iron correction
|
||||
offset_x = (maxx + minx) / 2
|
||||
offset_y = (maxy + miny) / 2
|
||||
offset_z = (maxz + minz) / 2
|
||||
|
||||
self._offset = (offset_x, offset_y, offset_z)
|
||||
|
||||
# Soft iron correction
|
||||
avg_delta_x = (maxx - minx) / 2
|
||||
avg_delta_y = (maxy - miny) / 2
|
||||
avg_delta_z = (maxz - minz) / 2
|
||||
|
||||
avg_delta = (avg_delta_x + avg_delta_y + avg_delta_z) / 3
|
||||
|
||||
scale_x = avg_delta / avg_delta_x
|
||||
scale_y = avg_delta / avg_delta_y
|
||||
scale_z = avg_delta / avg_delta_z
|
||||
|
||||
self._scale = (scale_x, scale_y, scale_z)
|
||||
|
||||
return self._offset, self._scale
|
||||
|
||||
def _register_short(self, register, value=None, buf=bytearray(2)):
|
||||
if value is None:
|
||||
self.i2c.readfrom_mem_into(self.address, register, buf)
|
||||
return ustruct.unpack("<h", buf)[0]
|
||||
|
||||
ustruct.pack_into("<h", buf, 0, value)
|
||||
return self.i2c.writeto_mem(self.address, register, buf)
|
||||
|
||||
def _register_three_shorts(self, register, buf=bytearray(6)):
|
||||
self.i2c.readfrom_mem_into(self.address, register, buf)
|
||||
return ustruct.unpack("<hhh", buf)
|
||||
|
||||
def _register_char(self, register, value=None, buf=bytearray(1)):
|
||||
if value is None:
|
||||
self.i2c.readfrom_mem_into(self.address, register, buf)
|
||||
return buf[0]
|
||||
|
||||
ustruct.pack_into("<b", buf, 0, value)
|
||||
return self.i2c.writeto_mem(self.address, register, buf)
|
||||
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
def __exit__(self, exception_type, exception_value, traceback):
|
||||
pass
|
||||
|
||||
|
||||
class MPU9250:
|
||||
"""Class which provides interface to MPU9250 9-axis motion tracking device."""
|
||||
def __init__(self, i2c, mpu6500 = None, ak8963 = None):
|
||||
if mpu6500 is None:
|
||||
self.mpu6500 = MPU6500(i2c)
|
||||
else:
|
||||
self.mpu6500 = mpu6500
|
||||
|
||||
if ak8963 is None:
|
||||
self.ak8963 = AK8963(i2c)
|
||||
else:
|
||||
self.ak8963 = ak8963
|
||||
|
||||
# @property
|
||||
# def acceleration(self):
|
||||
# """
|
||||
# Acceleration measured by the sensor. By default will return a
|
||||
# 3-tuple of X, Y, Z axis values in m/s^2 as floats. To get values in g
|
||||
# pass `accel_fs=SF_G` parameter to the MPU6500 constructor.
|
||||
# """
|
||||
# return self.mpu6500.acceleration
|
||||
def mpu9250_get_temperature(self):
|
||||
return self.mpu6500.temperature
|
||||
|
||||
def mpu9250_get_values(self):
|
||||
"""
|
||||
Acceleration measured by the sensor. By default will return a
|
||||
3-tuple of X, Y, Z axis values in m/s^2 as floats. To get values in g
|
||||
pass `accel_fs=SF_G` parameter to the MPU6500 constructor.
|
||||
"""
|
||||
g = self.mpu6500.acceleration()
|
||||
a = [round(x/9.8, 2) for x in g]
|
||||
return tuple(a)
|
||||
|
||||
def mpu9250_get_x(self):
|
||||
"""
|
||||
Acceleration measured by the sensor. By default will return a
|
||||
3-tuple of X, Y, Z axis values in m/s^2 as floats. To get values in g
|
||||
pass `accel_fs=SF_G` parameter to the MPU6500 constructor.
|
||||
"""
|
||||
return round(self.mpu6500.acceleration()[0]/9.8, 2)
|
||||
|
||||
def mpu9250_get_y(self):
|
||||
"""
|
||||
Acceleration measured by the sensor. By default will return a
|
||||
3-tuple of X, Y, Z axis values in m/s^2 as floats. To get values in g
|
||||
pass `accel_fs=SF_G` parameter to the MPU6500 constructor.
|
||||
"""
|
||||
return round(self.mpu6500.acceleration()[1]/9.8, 2)
|
||||
|
||||
def mpu9250_get_z(self):
|
||||
"""
|
||||
Acceleration measured by the sensor. By default will return a
|
||||
3-tuple of X, Y, Z axis values in m/s^2 as floats. To get values in g
|
||||
pass `accel_fs=SF_G` parameter to the MPU6500 constructor.
|
||||
"""
|
||||
return round(self.mpu6500.acceleration()[2]/9.8, 2)
|
||||
|
||||
|
||||
def mpu9250_is_gesture(self,choice):
|
||||
if choice == 'face up':
|
||||
if self.mpu6500.acceleration()[2] <= -9:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
if choice == 'face down':
|
||||
if self.mpu6500.acceleration()[2] >= 9:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
if choice == 'shake':
|
||||
if abs(self.mpu6500.acceleration()[0]) >= 9 and abs(self.mpu6500.acceleration()[1]) >= 9 :
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
if choice == 'up':
|
||||
if self.mpu6500.acceleration()[1] >= 9:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
if choice == 'down':
|
||||
if self.mpu6500.acceleration()[1] <= -9:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
if choice == 'right':
|
||||
if self.mpu6500.acceleration()[0] <= -9:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
if choice == 'left':
|
||||
if self.mpu6500.acceleration()[0] >= 9:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
@property
|
||||
def mpu9250_gyro(self):
|
||||
"""
|
||||
Gyro measured by the sensor. By default will return a 3-tuple of
|
||||
X, Y, Z axis values in rad/s as floats. To get values in deg/s pass
|
||||
`gyro_sf=SF_DEG_S` parameter to the MPU6500 constructor.
|
||||
"""
|
||||
return self.mpu6500.gyro
|
||||
|
||||
def mpu9250_gyro_x(self):
|
||||
return self.mpu6500.gyro[0]
|
||||
|
||||
def mpu9250_gyro_y(self):
|
||||
return self.mpu6500.gyro[1]
|
||||
|
||||
def mpu9250_gyro_z(self):
|
||||
return self.mpu6500.gyro[2]
|
||||
|
||||
def mpu9250_gyro_values(self):
|
||||
return self.mpu6500.gyro
|
||||
|
||||
@property
|
||||
def mpu9250_magnetic(self):
|
||||
"""
|
||||
X, Y, Z axis micro-Tesla (uT) as floats.
|
||||
"""
|
||||
return self.ak8963.magnetic
|
||||
|
||||
def mpu9250_magnetic_x(self):
|
||||
return self.mpu9250_magnetic[0]
|
||||
|
||||
def mpu9250_magnetic_y(self):
|
||||
return self.mpu9250_magnetic[1]
|
||||
|
||||
def mpu9250_magnetic_z(self):
|
||||
return self.mpu9250_magnetic[2]
|
||||
|
||||
def mpu9250_magnetic_values(self):
|
||||
return self.mpu9250_magnetic
|
||||
|
||||
# @property
|
||||
def mpu9250_get_field_strength(self):
|
||||
x=self.mpu9250_magnetic[0]
|
||||
y=self.mpu9250_magnetic[1]
|
||||
z=self.mpu9250_magnetic[2]
|
||||
return (x**2+y**2+z**2)**0.5*1000
|
||||
|
||||
def mpu9250_heading(self):
|
||||
x=self.mpu9250_magnetic[0]
|
||||
y=self.mpu9250_magnetic[1]
|
||||
z=self.mpu9250_magnetic[2]
|
||||
a=math.atan(z/x)
|
||||
b=math.atan(z/y)
|
||||
xr=x*math.cos(a)+y*math.sin(a)*math.sin(b)-z*math.cos(b)*math.sin(a)
|
||||
yr=x*math.cos(b)+z*math.sin(b)
|
||||
return 60*math.atan(yr/xr)
|
||||
|
||||
@property
|
||||
def whoami(self):
|
||||
return self.mpu6500.whoami
|
||||
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
def __exit__(self, exception_type, exception_value, traceback):
|
||||
pass
|
||||
|
||||
class Compass:
|
||||
RAD_TO_DEG = 57.295779513082320876798154814105
|
||||
|
||||
def __init__(self, sensor):
|
||||
self.sensor = sensor
|
||||
|
||||
def get_x(self):
|
||||
return self.sensor.mpu9250_magnetic[0]
|
||||
|
||||
def get_y(self):
|
||||
return self.sensor.mpu9250_magnetic[1]
|
||||
|
||||
def get_z(self):
|
||||
return self.sensor.mpu9250_magnetic[2]
|
||||
|
||||
def get_field_strength(self):
|
||||
return self.sensor.mpu9250_get_field_strength()
|
||||
|
||||
def heading(self):
|
||||
from math import atan2
|
||||
xyz = self.sensor.mpu9250_magnetic
|
||||
return int(((atan2(xyz[1], xyz[0]) * Compass.RAD_TO_DEG) + 180) % 360)
|
||||
|
||||
|
||||
def is_calibrate(self):
|
||||
try:
|
||||
import compass_cfg
|
||||
return True
|
||||
except Exception as e:
|
||||
return False
|
||||
|
||||
def reset_calibrate(self):
|
||||
import os
|
||||
os.remove("compass_cfg.py")
|
||||
# compass = mpu
|
||||
# accelerometer = mpu
|
||||
|
||||
@@ -0,0 +1,142 @@
|
||||
"""
|
||||
MS32006
|
||||
|
||||
Micropython library for the MS32006 step diever
|
||||
=======================================================
|
||||
#Changed from circuitpython to micropython 20211206
|
||||
|
||||
dahanzimin From the Mixly Team
|
||||
|
||||
"""
|
||||
import time
|
||||
from micropython import const
|
||||
|
||||
|
||||
MS32006_REG_RESET = const(0x00) #复位
|
||||
MS32006_FCLK = const(25000000) #芯片输入时钟选择,此参数与运动速度有关。 范围是:5-30MHZ
|
||||
|
||||
ADDRESS_A = 0x10
|
||||
ADDRESS_B = 0x18
|
||||
MOT_FULL = 0
|
||||
MOT_HALF = 1
|
||||
MOT_A = 0
|
||||
MOT_B = 4
|
||||
MOT_N = 0
|
||||
MOT_CW = 1
|
||||
MOT_CCW = 2
|
||||
MOT_P = 3
|
||||
class MS32006:
|
||||
|
||||
_buffer = bytearray(2)
|
||||
|
||||
def __init__(self, i2c_bus,addr=ADDRESS_A,mode=MOT_FULL):
|
||||
self._device = i2c_bus
|
||||
self._address = addr
|
||||
self.reset()
|
||||
self.mode=mode
|
||||
|
||||
|
||||
def _read_u8(self, address):
|
||||
self._buffer[0] = address & 0xFF
|
||||
self._device.writeto(self._address,self._buffer)
|
||||
self._device.readfrom_into(self._address,self._buffer)
|
||||
return self._buffer[0]
|
||||
|
||||
def _write_u8(self, address, val):
|
||||
self._buffer[0] = address & 0xFF
|
||||
self._buffer[1] = val & 0xFF
|
||||
self._device.writeto(self._address,self._buffer)
|
||||
|
||||
def reset(self):
|
||||
self._write_u8(MS32006_REG_RESET,0x00)
|
||||
time.sleep(0.1)
|
||||
self._write_u8(MS32006_REG_RESET,0xC1)
|
||||
|
||||
def move(self,moto,mot_dir,mot_pps,mot_step):
|
||||
readstate_0H = self._read_u8(0x00)
|
||||
readstate_9H = self._read_u8(0x09)
|
||||
speed_data=MS32006_FCLK//mot_pps//128 #设置速度 xx pps 128是固定参数
|
||||
|
||||
if speed_data<32: #限定转速
|
||||
speed_data=32
|
||||
elif speed_data>16383:
|
||||
speed_data=16383
|
||||
|
||||
mot_speed_l=speed_data&0x00ff #取低8位
|
||||
mot_speed_h=speed_data//0x100 #取高6位
|
||||
|
||||
if self.mode==MOT_FULL: #设置整步、半步驱动模式
|
||||
mot_speed_h|=0x80
|
||||
else:
|
||||
mot_speed_h&=0x7f
|
||||
|
||||
if mot_step>2047:
|
||||
raise AttributeError("Reach the set upper limit, up to 2047 step")
|
||||
|
||||
mot_step_l=mot_step&0x00ff
|
||||
mot_step_h=mot_step//0x100
|
||||
mot_step_h|=0x80
|
||||
|
||||
if mot_dir==MOT_CW:
|
||||
mot_step_h&=0xBF
|
||||
else:
|
||||
mot_step_h|=0x40
|
||||
self._write_u8(0x01+moto,mot_speed_l)
|
||||
self._write_u8(0x02+moto,mot_speed_h)
|
||||
self._write_u8(0x03+moto,mot_step_l)
|
||||
self._write_u8(0x04+moto,mot_step_h)
|
||||
|
||||
if moto==MOT_A:
|
||||
self._write_u8(0x00, readstate_0H&0xfb)
|
||||
self._write_u8(0x09, readstate_9H|0x80)
|
||||
else:
|
||||
self._write_u8(0x00, readstate_0H&0xfd)
|
||||
self._write_u8(0x09, readstate_9H|0x40)
|
||||
|
||||
def close(self,moto): #停止并关闭输出
|
||||
if moto==MOT_A:
|
||||
self._write_u8(0x04,0x00)
|
||||
else:
|
||||
self._write_u8(0x08,0x00)
|
||||
|
||||
def stop(self,moto): #此停止函数,强制让电机停止
|
||||
readstate = self._read_u8(0x00)
|
||||
if moto==MOT_A:
|
||||
self._write_u8(0x00,readstate|0x04)
|
||||
else:
|
||||
self._write_u8(0x00,readstate|0x02)
|
||||
|
||||
def readstep(self,moto): #读取电机运动步数
|
||||
if moto==MOT_A:
|
||||
rdb =self._read_u8(0x0b)
|
||||
rdc =self._read_u8(0x0c)
|
||||
else:
|
||||
rdb =self._read_u8(0x0d)
|
||||
rdc =self._read_u8(0x0e)
|
||||
return (rdb*0x100+rdc)&0xfff
|
||||
|
||||
def readbusy(self,moto): #读取电机缓存是否有数据
|
||||
if moto==MOT_A:
|
||||
busy =(self._read_u8(0x0b)>>6)&1
|
||||
else:
|
||||
busy =(self._read_u8(0x0d)>>6)&1
|
||||
return bool(busy)
|
||||
|
||||
def readwork(self,moto): #读取电机是否在运行
|
||||
if moto==MOT_A:
|
||||
busy =(self._read_u8(0x0b)>>4)&1
|
||||
else:
|
||||
busy =(self._read_u8(0x0d)>>4)&1
|
||||
return bool(busy)
|
||||
|
||||
def dc_motor(self,state,speed): #直流电机驱动
|
||||
if (state==MOT_CW) | (state==MOT_CCW) :
|
||||
speed_st=speed*127//100 |0x80
|
||||
self._write_u8(0x0A,speed_st)
|
||||
|
||||
readstate = self._read_u8(0x09) & 0xA0
|
||||
state_st=(state<<2) | 0X03 | readstate
|
||||
self._write_u8(0x09,state_st)
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,48 @@
|
||||
import network,time,board
|
||||
from machine import UART
|
||||
import time
|
||||
|
||||
def wifi_init(RX_Pin,TX_Pin):
|
||||
board.register(TX_Pin,board.FPIOA.UART2_TX)
|
||||
board.register(RX_Pin,board.FPIOA.UART2_RX)
|
||||
|
||||
uart = UART(UART.UART2,115200,timeout=1000, read_buf_len=1024*16)
|
||||
T1=time.ticks_ms()
|
||||
while True:
|
||||
tmp=uart.read()
|
||||
if tmp:
|
||||
if tmp.endswith("OK\r\n"):
|
||||
break
|
||||
else:
|
||||
uart.write("AT+RST\r\n")
|
||||
time.sleep_ms(20)
|
||||
if time.ticks_diff(time.ticks_ms(), T1) >2000:
|
||||
raise AttributeError("ESP-AT not connected or needs to be reset")
|
||||
try:
|
||||
nic = network.ESP8285(uart)
|
||||
time.sleep(1)
|
||||
print("ESP-AT OK")
|
||||
return nic
|
||||
except Exception:
|
||||
raise AttributeError("ESP-AT Connection Failed")
|
||||
|
||||
|
||||
def wifi_deal_ap_info(info):
|
||||
res = []
|
||||
for ap_str in info:
|
||||
ap_str = ap_str.split(",")
|
||||
info_one = []
|
||||
for node in ap_str:
|
||||
if node.startswith('"'):
|
||||
info_one.append(node[1:-1])
|
||||
else:
|
||||
info_one.append(int(node))
|
||||
res.append(info_one)
|
||||
return res
|
||||
|
||||
def scans(nic):
|
||||
ap_info = nic.scan()
|
||||
ap_info = wifi_deal_ap_info(ap_info)
|
||||
ap_info.sort(key=lambda x:x[2], reverse=True)
|
||||
return ap_info
|
||||
|
||||
@@ -0,0 +1,46 @@
|
||||
import time
|
||||
from math import pi, isnan
|
||||
|
||||
class PID:
|
||||
_kp = _ki = _kd = _integrator = _imax = 0
|
||||
_last_error = _last_derivative = _last_t = 0
|
||||
_RC = 1/(2 * pi * 20)
|
||||
def __init__(self, p=0, i=0, d=0, imax=0):
|
||||
self._kp = float(p)
|
||||
self._ki = float(i)
|
||||
self._kd = float(d)
|
||||
self._imax = abs(imax)
|
||||
self._last_derivative = float('nan')
|
||||
|
||||
def get_pid(self, error, scaler):
|
||||
tnow = time.ticks_ms()
|
||||
dt = tnow - self._last_t
|
||||
output = 0
|
||||
if self._last_t == 0 or dt > 1000:
|
||||
dt = 0
|
||||
self.reset_I()
|
||||
self._last_t = tnow
|
||||
delta_time = float(dt) / float(1000)
|
||||
output += error * self._kp
|
||||
if abs(self._kd) > 0 and dt > 0:
|
||||
if isnan(self._last_derivative):
|
||||
derivative = 0
|
||||
self._last_derivative = 0
|
||||
else:
|
||||
derivative = (error - self._last_error) / delta_time
|
||||
derivative = self._last_derivative + \
|
||||
((delta_time / (self._RC + delta_time)) * \
|
||||
(derivative - self._last_derivative))
|
||||
self._last_error = error
|
||||
self._last_derivative = derivative
|
||||
output += self._kd * derivative
|
||||
output *= scaler
|
||||
if abs(self._ki) > 0 and dt > 0:
|
||||
self._integrator += (error * self._ki) * scaler * delta_time
|
||||
if self._integrator < -self._imax: self._integrator = -self._imax
|
||||
elif self._integrator > self._imax: self._integrator = self._imax
|
||||
output += self._integrator
|
||||
return output
|
||||
def reset_I(self):
|
||||
self._integrator = 0
|
||||
self._last_derivative = float('nan')
|
||||
@@ -0,0 +1,128 @@
|
||||
import board
|
||||
import audio,video
|
||||
from Maix import I2S
|
||||
import gc
|
||||
|
||||
spk_b=None
|
||||
spk_d=None
|
||||
spk_w=None
|
||||
|
||||
def spk_init(BLK=8,WS=9,DAT=10,sample_rate=16000):
|
||||
global spk_b
|
||||
global spk_d
|
||||
global spk_w
|
||||
spk_b=BLK
|
||||
spk_d=DAT
|
||||
spk_w=WS
|
||||
board.register(DAT,board.FPIOA.I2S0_OUT_D1)
|
||||
board.register(BLK,board.FPIOA.I2S0_SCLK)
|
||||
board.register(WS,board.FPIOA.I2S0_WS)
|
||||
wav_dev = I2S(I2S.DEVICE_0)
|
||||
wav_dev.channel_config(I2S.CHANNEL_1, I2S.TRANSMITTER,resolution = I2S.RESOLUTION_16_BIT, cycles = I2S.SCLK_CYCLES_32, align_mode = I2S.STANDARD_MODE)
|
||||
wav_dev.set_sample_rate(sample_rate)
|
||||
spk_rep=wav_dev
|
||||
return wav_dev
|
||||
|
||||
def mic_init(BLK=35,WS=33,DAT=34,sample_rate=16000):
|
||||
board.register(DAT,board.FPIOA.I2S2_IN_D0)
|
||||
board.register(BLK,board.FPIOA.I2S2_SCLK)
|
||||
board.register(WS,board.FPIOA.I2S2_WS)
|
||||
wav_dev = I2S(I2S.DEVICE_2)
|
||||
wav_dev.channel_config(I2S.CHANNEL_0, I2S.RECEIVER, resolution = I2S.RESOLUTION_16_BIT, cycles = I2S.SCLK_CYCLES_32, align_mode=I2S.STANDARD_MODE)
|
||||
wav_dev.set_sample_rate(sample_rate)
|
||||
return wav_dev
|
||||
|
||||
|
||||
def audio_play(I2S,path,num=80):
|
||||
try:
|
||||
player = audio.Audio(path=path)
|
||||
except Exception as e:
|
||||
raise NameError("No audio file loaded or {}".format(e))
|
||||
player.volume(num)
|
||||
wav_info=player.play_process(I2S)
|
||||
I2S.set_sample_rate(wav_info[1])
|
||||
while True:
|
||||
ret = player.play()
|
||||
if ret == None:
|
||||
print("Format Error")
|
||||
break
|
||||
elif ret == 0:
|
||||
print("Play end \n")
|
||||
player.finish()
|
||||
break
|
||||
player.__deinit__()
|
||||
gc.collect()
|
||||
|
||||
def audio_record(I2S,path,record_time,sample_rate=16000):
|
||||
try:
|
||||
recorder = audio.Audio(path=path, is_create=True, samplerate=sample_rate)
|
||||
except Exception as e:
|
||||
raise NameError("Need audio storage location or {}".format(e))
|
||||
queue = []
|
||||
frame_cnt = record_time*sample_rate//2048
|
||||
for i in range(frame_cnt):
|
||||
tmp = I2S.record(2048*2)
|
||||
if len(queue) > 0:
|
||||
ret = recorder.record(queue[0])
|
||||
queue.pop(0)
|
||||
I2S.wait_record()
|
||||
queue.append(tmp)
|
||||
print("record:{}s".format(round(((frame_cnt-i-1)/7.7) ,1)))
|
||||
recorder.finish()
|
||||
recorder.__deinit__()
|
||||
del recorder
|
||||
print("Audio record finish \n")
|
||||
gc.collect()
|
||||
|
||||
|
||||
def video_play(I2S1,path,num=80):
|
||||
try:
|
||||
global spk_b
|
||||
global spk_d
|
||||
global spk_w
|
||||
import lcd
|
||||
lcd.init()
|
||||
I2S=spk_init(spk_b,spk_w,spk_d)
|
||||
vide = video.open(path)
|
||||
except Exception as e:
|
||||
raise NameError("No video file loaded or {}".format(e))
|
||||
vide.volume(num)
|
||||
while True:
|
||||
try:
|
||||
ret = vide.play()
|
||||
except Exception as e:
|
||||
raise NameError("Video format error or {}".format(e))
|
||||
if ret == None:
|
||||
print("Format Error")
|
||||
break
|
||||
elif ret == 0:
|
||||
print("Play end \n")
|
||||
break
|
||||
vide.__del__()
|
||||
del vide
|
||||
del I2S
|
||||
gc.collect()
|
||||
|
||||
|
||||
def video_record(I2S,path,record_time):
|
||||
import sensor,lcd
|
||||
lcd.init()
|
||||
try:
|
||||
v = video.open(path, audio=False, record=True, interval=200000, quality=80,width=240, height=240)
|
||||
except Exception as e:
|
||||
raise NameError("Need video storage location or {}".format(e))
|
||||
record_time=record_time*5
|
||||
for i in range(record_time):
|
||||
try:
|
||||
img = sensor.snapshot()
|
||||
except :
|
||||
raise NameError("Need to initialize camera")
|
||||
lcd.display(img)
|
||||
v.record(img)
|
||||
print("record {}s".format(round((record_time-i-1)*0.2,1)))
|
||||
v.record_finish()
|
||||
print("Video record finish \n")
|
||||
v.__del__()
|
||||
gc.collect()
|
||||
|
||||
|
||||
@@ -0,0 +1,33 @@
|
||||
try:
|
||||
import image
|
||||
image.font_free()
|
||||
except:
|
||||
pass
|
||||
|
||||
try:
|
||||
import lcd,time,gc,machine
|
||||
|
||||
lcd.init(color=0x0000)
|
||||
lcd.draw_string(48,100, "Welcome to MixGo!", lcd.YELLOW, lcd.BLACK)
|
||||
lcd.draw_string(62,132, "loading .", lcd.YELLOW, lcd.BLACK)
|
||||
time.sleep_ms(50)
|
||||
lcd.draw_string(62,132, "loading ..", lcd.YELLOW, lcd.BLACK)
|
||||
time.sleep_ms(50)
|
||||
lcd.draw_string(62,132, "loading ...", lcd.YELLOW, lcd.BLACK)
|
||||
time.sleep_ms(50)
|
||||
lcd.draw_string(62,132, "loading ....", lcd.YELLOW, lcd.BLACK)
|
||||
time.sleep_ms(50)
|
||||
lcd.draw_string(62,132, "loading .....", lcd.YELLOW, lcd.BLACK)
|
||||
time.sleep_ms(50)
|
||||
lcd.draw_string(62,132, "loading ......", lcd.YELLOW, lcd.BLACK)
|
||||
time.sleep_ms(50)
|
||||
lcd.draw_string(62,132, "loading .......", lcd.YELLOW, lcd.BLACK)
|
||||
time.sleep_ms(50)
|
||||
lcd.clear(0x0000)
|
||||
del time
|
||||
del lcd
|
||||
del gc
|
||||
|
||||
finally:
|
||||
import gc
|
||||
gc.collect()
|
||||
@@ -0,0 +1,90 @@
|
||||
# 使用 PAC9685
|
||||
import math
|
||||
import ustruct
|
||||
import time
|
||||
class PCA9685:
|
||||
def __init__(self, i2c, address=0x40):
|
||||
self.i2c = i2c
|
||||
self.address = address
|
||||
self.reset()
|
||||
|
||||
def _write(self, address, value):
|
||||
self.i2c.writeto_mem(self.address, address, bytearray([value]))
|
||||
|
||||
def _read(self, address):
|
||||
return self.i2c.readfrom_mem(self.address, address, 1)[0]
|
||||
|
||||
def reset(self):
|
||||
self._write(0x00, 0x00) # Mode1
|
||||
|
||||
def freq(self, freq=None):
|
||||
if freq is None:
|
||||
return int(25000000.0 / 4096 / (self._read(0xfe) - 0.5))
|
||||
prescale = int(25000000.0 / 4096.0 / freq + 0.5)
|
||||
old_mode = self._read(0x00) # Mode 1
|
||||
self._write(0x00, (old_mode & 0x7F) | 0x10) # Mode 1, sleep
|
||||
self._write(0xfe, prescale) # Prescale
|
||||
self._write(0x00, old_mode) # Mode 1
|
||||
time.sleep_us(5)
|
||||
self._write(0x00, old_mode | 0xa1) # Mode 1, autoincrement on
|
||||
|
||||
def pwm(self, index, on=None, off=None):
|
||||
if on is None or off is None:
|
||||
data = self.i2c.readfrom_mem(self.address, 0x06 + 4 * index, 4)
|
||||
return ustruct.unpack('<HH', data)
|
||||
data = ustruct.pack('<HH', on, off)
|
||||
self.i2c.writeto_mem(self.address, 0x06 + 4 * index, data)
|
||||
|
||||
def duty(self, index, value=None, invert=False):
|
||||
if value is None:
|
||||
pwm = self.pwm(index)
|
||||
if pwm == (0, 4096):
|
||||
value = 0
|
||||
elif pwm == (4096, 0):
|
||||
value = 4095
|
||||
value = pwm[1]
|
||||
if invert:
|
||||
value = 4095 - value
|
||||
return value
|
||||
if not 0 <= value <= 4095:
|
||||
raise ValueError("Out of range")
|
||||
if invert:
|
||||
value = 4095 - value
|
||||
if value == 0:
|
||||
self.pwm(index, 0, 4096)
|
||||
elif value == 4095:
|
||||
self.pwm(index, 4096, 0)
|
||||
else:
|
||||
self.pwm(index, 0, value)
|
||||
|
||||
class Servos:
|
||||
def __init__(self, i2c, address=0x55, freq=50, min_us=600, max_us=2400,
|
||||
degrees=180):
|
||||
self.period = 1000000 / freq
|
||||
self.min_duty = self._us2duty(min_us)
|
||||
self.max_duty = self._us2duty(max_us)
|
||||
self.degrees = degrees
|
||||
self.freq = freq
|
||||
self.PCA9685 = PCA9685(i2c, address)
|
||||
self.PCA9685.freq(freq)
|
||||
|
||||
def _us2duty(self, value):
|
||||
return int(4095 * value / self.period)
|
||||
|
||||
def position(self, index, degrees=None, radians=None, us=None, duty=None):
|
||||
span = self.max_duty - self.min_duty
|
||||
if degrees is not None:
|
||||
duty = self.min_duty + span * degrees / self.degrees
|
||||
elif radians is not None:
|
||||
duty = self.min_duty + span * radians / math.radians(self.degrees)
|
||||
elif us is not None:
|
||||
duty = self._us2duty(us)
|
||||
elif duty is not None:
|
||||
pass
|
||||
else:
|
||||
return self.PCA9685.duty(index)
|
||||
duty = min(self.max_duty, max(self.min_duty, int(duty)))
|
||||
self.PCA9685.duty(index, duty)
|
||||
|
||||
def release(self, index):
|
||||
self.PCA9685.duty(index, 0)
|
||||
@@ -0,0 +1,38 @@
|
||||
from struct import unpack as unp
|
||||
from time import sleep_ms
|
||||
|
||||
# SHT20 default address
|
||||
SHT20_I2CADDR = 64
|
||||
|
||||
# SHT20 Command
|
||||
TRI_T_MEASURE_NO_HOLD = b'\xf3'
|
||||
TRI_RH_MEASURE_NO_HOLD = b'\xf5'
|
||||
READ_USER_REG = b'\xe7'
|
||||
WRITE_USER_REG = b'\xe6'
|
||||
SOFT_RESET = b'\xfe'
|
||||
|
||||
|
||||
|
||||
class SHT20(object):
|
||||
|
||||
def __init__(self, i2c_bus):
|
||||
self._address = SHT20_I2CADDR
|
||||
self._bus = i2c_bus
|
||||
|
||||
def get_SHT_temperature(self):
|
||||
self._bus.writeto(self._address, TRI_T_MEASURE_NO_HOLD)
|
||||
sleep_ms(150)
|
||||
origin_data = self._bus.readfrom(self._address, 2)
|
||||
origin_value = unp('>h', origin_data)[0]
|
||||
value = -46.85 + 175.72 * (origin_value / 65536)
|
||||
return value
|
||||
|
||||
def get_SHT_relative_humidity(self):
|
||||
self._bus.writeto(self._address, TRI_RH_MEASURE_NO_HOLD)
|
||||
sleep_ms(150)
|
||||
origin_data = self._bus.readfrom(self._address, 2)
|
||||
origin_value = unp('>H', origin_data)[0]
|
||||
value = -6 + 125 * (origin_value / 65536)
|
||||
return value
|
||||
|
||||
#sht=SHT20(I2C(scl = Pin(22), sda = Pin(21), freq = 100000))
|
||||
Reference in New Issue
Block a user