from class_sensorcmd import * from sensorPara import * import struct # 电容数据存储结构 class capData: def __init__(self): self.sensorIndex = 0 # 电容序号，与iic addr相同 self.channelCapData = list() # 原始通道数值 self.tf = list() # 切向力数组 self.tfDir = list() # 切向力方向数组 self.nf = list() # 法向力数组 self.sProxCapData = list() # 接近(自电容)数组 self.mProxCapData = list() # 接近（互电容）数组 self.cnt = 0 # 计数，测试用 def init(self, addr, yddsNum, sProxNum, mProxNum, capChannelNum): self.sensorIndex = addr # 电容序号，与iic addr相同 self.channelCapData = list(range(capChannelNum))# 原始通道数值 self.tf = list(range(yddsNum)) # 切向力数组 self.tfDir = list(range(yddsNum)) # 切向力方向数组 self.nf = list(range(yddsNum)) # 法向力数组 self.sProxCapData = list(range(sProxNum)) # 接近(自电容)数组 self.mProxCapData = list(range(mProxNum)) # 接近（互电容）数组 self.cnt = 0 # 计数，测试用 def deinit(self): self.channelCapData = None self.tf = None self.tfDir = None self.nf = None self.sProxCapData = None self.mProxCapData = None # 传感器类：包换传感器相关参数 class ClassFinger: def __init__(self, pca_idx, ch341): self.snsCmd = ClassSensorCmd(ch341) self.pcaIdx = pca_idx # iic使能芯片序号，从2开始 self.readData = capData() self.disconnected() # 检查传感器是否连接，如果读写地址正确则认为连接正常 def checkSensor(self): # 广播的方式读取当前传感器地址，并默认将iic地址配置为和pca相同的地址 addrRead = self.snsCmd.getAddr(0) if addrRead != self.pcaIdx: if self.pcaIdx != self.snsCmd.setAddr(addrRead, self.pcaIdx): print(f"set addr false, setaddr={self.pcaIdx}") return False else: addrRead = self.pcaIdx print(f"update iic addr, new addr ={addrRead}") # 设置发送数据类型为原始值 if True != self.snsCmd.setSensorSendType(addrRead, 0): print(f"setSensorSendType err, addr = {addrRead}") # 设置电容采集序列，这里按照地址来分配采集时序，只要每个传感器不同即可 if self.snsCmd.setSensorCapOffset(addrRead, addrRead) != True: print(f"setSensorCapOffset err, addr = {addrRead}") # 实际用户使用中只需要根据使用的传感器来定义参数即可，不需要读取项目号 projectRead = self.snsCmd.getSensorProjectIdex(addrRead) print(f"project={projectRead}") findProjectFlg = False if projectRead > 0: for pro in finger_params: if pro.prg == projectRead: self.projectPara = pro print(f"finger connected, project = {self.projectPara.name}") findProjectFlg = True if not findProjectFlg: raise RuntimeError( f"Unknown sensor project ID: {projectRead}. " f"Expected one of: {[p.prg for p in finger_params]}. " "Check sensorPara.py or update sensor firmware." ) self.connected(addrRead) return True # 传感器连接，初始化参数 def connected(self, addr): self.addr = addr self.connect = True self.connectTimer = time.time() self.packIdx = 0 self.data = list() self.data.extend(range(self.projectPara.pack_len))# self.readData.init(addr, self.projectPara.ydds_num, self.projectPara.s_prox_num, self.projectPara.m_prox_num, self.projectPara.sensor_num) #print(f"datalen={len(self.data)}") # 传感器断开,复位参数 def disconnected(self): self.addr = 0xFF # iic地址 self.connect = False # 连接标志位 self.packIdx = 0 # 采样序号 self.connectTimer = 0 # 连接超时计时 self.projectPara = finger_params[0] self.readData.deinit() def capRead(self): for retry in range(0, 3): if self.snsCmd.getSensorCapData(self.addr, self.data) == True: if self.data[5] != self.projectPara.sensor_num: print(f"cap channel num err, read num = {self.data[5]}, expect num = {self.projectPara.sensor_num}") if self.data[4] != self.packIdx: self.packIdx = self.data[4] self.connectTimer = time.time() # 根据通道值占用字节大小获取通道数据 if self.projectPara.cap_byte == 4: for j in range(0, self.projectPara.sensor_num): self.readData.channelCapData[j] = ((self.data[6 + j * self.projectPara.cap_byte] & 0xFF) + ((self.data[6 + j * self.projectPara.cap_byte + 1] & 0xFF) << 8) + ((self.data[6 + j * self.projectPara.cap_byte + 2] & 0xFF) << 16) + ((self.data[6 + j * 4 + 3] & 0xFF) << 24)) else: for j in range(0, self.projectPara.sensor_num): self.readData.channelCapData[j] = ((self.data[6 + j * self.projectPara.cap_byte] & 0xFF) + ((self.data[6 + j * self.projectPara.cap_byte + 1] & 0xFF) << 8) + ((self.data[6 + j * self.projectPara.cap_byte + 2] & 0xFF) << 16)) yddsOffset = 6 + self.projectPara.sensor_num*self.projectPara.cap_byte if self.projectPara.ydds_type == 2: struct_size = sizeof(DynamicYddsComTs) for i in range(self.projectPara.ydds_num): offset = yddsOffset + i * struct_size struct_data = self.data[offset : offset + struct_size] struct_data = [value & 0xFF for value in struct_data] struct_data = bytes(struct_data) # 转换为 bytes 类型 instance = DynamicYddsComTs.from_buffer_copy(struct_data) self.readData.nf[i] = instance.nf self.readData.tf[i] = instance.tf self.readData.tfDir[i] = instance.tfDir self.readData.sProxCapData[i] = instance.prox elif self.projectPara.ydds_type == 4: struct_size = sizeof(DynamicYddsU16Ts) for i in range(self.projectPara.ydds_num): offset = yddsOffset + i * struct_size struct_data = self.data[offset : offset + struct_size] # print(f"struct={struct_data}") struct_data = [value & 0xFF for value in struct_data] struct_data = bytes(struct_data) # 转换为 bytes 类型 instance = DynamicYddsU16Ts.from_buffer_copy(struct_data) self.readData.nf[i] = instance.nf/1000.0 self.readData.tf[i] = instance.tf/1000.0 self.readData.tfDir[i] = instance.tfDir sProxOffset = yddsOffset + self.projectPara.ydds_num*struct_size for i in range(self.projectPara.s_prox_num): self.readData.sProxCapData[i] = ((self.data[sProxOffset + i*self.projectPara.cap_byte] & 0xFF) + ((self.data[sProxOffset + i*self.projectPara.cap_byte + 1] & 0xFF) << 8) + ((self.data[sProxOffset + i*self.projectPara.cap_byte + 2] & 0xFF) << 16)) mProxOffset = yddsOffset + self.projectPara.ydds_num*struct_size for i in range(self.projectPara.m_prox_num): self.readData.mProxCapData[i] = ((self.data[mProxOffset + i*self.projectPara.cap_byte] & 0xFF) + ((self.data[mProxOffset + i*self.projectPara.cap_byte + 1] & 0xFF) << 8) + ((self.data[mProxOffset + i*self.projectPara.cap_byte + 2] & 0xFF) << 16)) # print(f"capChannelDat={self.readData.channelCapData}") # for i in range(self.projectPara.ydds_num): # print(f"nf[{i}] = {self.readData.nf[i]}") # print(f"tf[{i}] = {self.readData.tf[i]}") # print(f"tfDir[{i}] = {self.readData.tfDir[i]}") # for i in range(self.projectPara.s_prox_num): # print(f"sProxCapData[{i}] = {self.readData.sProxCapData[i]}") # for i in range(self.projectPara.m_prox_num): # print(f"mProxCapData[{i}] = {self.readData.mProxCapData[i]}") break else: pass # 2S未接收到数据超时 if (time.time() - self.connectTimer) > 2: self.disconnected()