diff --git a/cap_read.py b/cap_read.py index 21e44d4..1cc9bfb 100644 --- a/cap_read.py +++ b/cap_read.py @@ -30,7 +30,7 @@ class ClassCapRead: # 最大连接5个手指 self.fingers = list() # 传感器列表 - for i in range(5): + for i in range(2): self.fingers.append(ClassFinger(2+i, self.ch341)) self.currCh341State = 0 # 当前ch341连接状态 @@ -71,6 +71,34 @@ class ClassCapRead: if self.vofaClient: self.vofaClient.close() + def debugPrint(self): + if self.socketConnected == True: + fingerIndex = 0 + _log1 = "" + # 输出原始通道数值 + # for index in range(0, self.fingers[fingerIndex].projectPara.sensor_num): + # _log1 += str(self.fingers[fingerIndex].readData.channelCapData[index]) + # _log1 += ',' + for index in range(0, self.fingers[fingerIndex].projectPara.ydds_num): + _log1 += str(int(self.fingers[fingerIndex].readData.nf[index]*1000)) + _log1 += ',' + _log1 += str(int(self.fingers[fingerIndex].readData.tf[index]*1000)) + _log1 += ',' + _log1 += str(self.fingers[fingerIndex].readData.tfDir[index]) + _log1 += ',' + for index in range(0, self.fingers[fingerIndex].projectPara.s_prox_num): + _log1 += str(self.fingers[fingerIndex].readData.sProxCapData[index]) + _log1 += ',' + for index in range(0, self.fingers[fingerIndex].projectPara.m_prox_num): + _log1 += str(self.fingers[fingerIndex].readData.mProxCapData[index]) + _log1 += ',' + + _log1 += str(0) + _log1 += '\r\n' + #print(_log1) + if self.socketConnected == 1: + self.vofaClient.send(_log1.encode()) + def set_sensor_enable(self, idx): _pack = list() _pack.append(idx) @@ -130,25 +158,7 @@ class ClassCapRead: self.fingers[fingerIndex].capRead() connectedSensorCnt += 1 - if self.socketConnected == True: - fingerIndex = 0 - _log1 = "" - # 输出原始通道数值 - for index in range(0, self.fingers[fingerIndex].projectPara.sensor_num): - _log1 += str(self.fingers[fingerIndex].readData.channelCapData[index]) - _log1 += ',' - for index in range(0, self.fingers[fingerIndex].projectPara.ydds_num): - _log1 += str(int(self.fingers[fingerIndex].readData.nf[index]*1000)) - _log1 += ',' - _log1 += str(int(self.fingers[fingerIndex].readData.tf[index]*1000)) - _log1 += ',' - _log1 += str(self.fingers[fingerIndex].readData.tfDir[index]) - _log1 += ',' - _log1 += str(0) - _log1 += '\r\n' - #print(_log1) - if self.socketConnected == 1: - self.vofaClient.send(_log1.encode()) + self.debugPrint() # 大于1个传感器连接需要设置接近采集序列 if connectedSensorCnt > 1 and (time.time() - self.syncTimer) > DEF_CDC_SYNC_MS: diff --git a/class_finger.py b/class_finger.py index c1814e2..95a65fa 100644 --- a/class_finger.py +++ b/class_finger.py @@ -63,7 +63,7 @@ class ClassFinger: print(f"setSensorCapOffset err, addr = {addrRead}") # 实际用户使用中只需要根据使用的传感器来定义参数即可,不需要读取项目号 - projectRead = self.snsCmd.getSensorProjectIdex(self.addr) + projectRead = self.snsCmd.getSensorProjectIdex(addrRead) print(f"project={projectRead}") findProjectFlg = False if projectRead > 0: @@ -142,14 +142,38 @@ class ClassFinger: self.readData.tfDir[i] = instance.tfDir self.readData.sProxCapData[i] = instance.prox elif self.projectPara.ydds_type == 4: - pass + 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"sProxCapData[{i}] = {self.readData.mProxCapData[i]}") break else: diff --git a/sensorPara.py b/sensorPara.py index 94779d3..3fd584b 100644 --- a/sensorPara.py +++ b/sensorPara.py @@ -13,6 +13,14 @@ class DynamicYddsComTs(Structure): ("prox", c_uint32), ] +class DynamicYddsU16Ts(Structure): + _pack_ = 1 # 按 1 字节对齐 + _fields_ = [ + ("nf", c_uint16), + ("tf", c_uint16), + ("tfDir", c_uint16), + ] + #todo 其他三维力类型待补充