feat(devkit): 动态阈值公式改为K*mean, POST_INIT_STABLE_CNT=10, get_pzt_angle返回cop/base坐标, CSV增加cop_x/y/base_x/y列

devkit/sensor_server.py

@@ -246,7 +246,7 @@ class SensorPushServicer(sensor_stream_pb2_grpc.SensorPushServicer):
         SensorPushServicer._csv_path = os.path.join(os.getcwd(), f"sensor_log_{ts}.csv")
         self._csv_file = open(SensorPushServicer._csv_path, "w", newline="", encoding="utf-8-sig")
         self._csv_writer = csv.writer(self._csv_file)
-        header = ["seq", "timestamp_ms", "dts_ms", "angle", "magnitude", "state", "resultant_force"] + [f"ch{i}" for i in range(SENSOR_ROWS * SENSOR_COLS)]
+        header = ["seq", "timestamp_ms", "dts_ms", "angle", "magnitude", "state", "cop_x", "cop_y", "base_x", "base_y", "resultant_force"] + [f"ch{i}" for i in range(SENSOR_ROWS * SENSOR_COLS)]
         self._csv_writer.writerow(header)
         self._csv_file.flush()
         print(f"[SensorPush] CSV logging to: {SensorPushServicer._csv_path}")
@@ -273,9 +273,10 @@ class SensorPushServicer(sensor_stream_pb2_grpc.SensorPushServicer):
             state = 0
             ok = True
             message = "OK"
+            cop_x = cop_y = base_x = base_y = 0.0
             if len(frame.matrix) == SENSOR_ROWS * SENSOR_COLS:
                 try:
-                    angle, magnitude, state = get_pzt_angle(frame.matrix)
+                    angle, magnitude, state, cop_x, cop_y, base_x, base_y = get_pzt_angle(frame.matrix)
                     self.last_angle = angle
                     # 打印接收到的数据
                     print(f"[Recv #{frame.seq}] seq={frame.seq} ts={frame.timestamp_ms} dts={frame.dts_ms} "
@@ -292,7 +293,9 @@ class SensorPushServicer(sensor_stream_pb2_grpc.SensorPushServicer):
             # 持续写入 CSV
             if self._csv_writer:
                 row = [frame.seq, frame.timestamp_ms, frame.dts_ms,
-                       f"{angle:.4f}", f"{magnitude:.4f}", state, frame.resultant_force]
+                       f"{angle:.4f}", f"{magnitude:.4f}", state,
+                       f"{cop_x:.4f}", f"{cop_y:.4f}", f"{base_x:.4f}", f"{base_y:.4f}",
+                       frame.resultant_force]
                 row += list(frame.matrix)
                 self._csv_writer.writerow(row)
                 if self.frame_count % 10 == 0:
@@ -398,7 +401,7 @@ SENSOR_COLS = 7
 
 # ===================== 二次静置精修参数 =====================
 POST_INIT_WINDOW_CNT = 60000
-POST_INIT_STABLE_CNT = 200
+POST_INIT_STABLE_CNT = 10
 POST_INIT_STABLE_THRESH = 0.1
 
 # ===================== 线程安全全局状态 =====================
@@ -474,7 +477,7 @@ def compute_pressure_direction(baseline_subtracted_frame):
         noise_sum_buf.append(total_pressure)
         if len(noise_sum_buf) >= NOISE_COLLECT_FRAMES:
             sums = np.array(noise_sum_buf)
-            dynamic_thresh = float(np.mean(sums) + THRESH_K * np.std(sums))
+            dynamic_thresh = THRESH_K * float(np.mean(sums))
 
     # 低压重置
     if dynamic_thresh is not None and total_pressure < dynamic_thresh:
@@ -577,20 +580,20 @@ def get_pzt_angle(adc_data):
         raise ValueError("ADC数据长度必须为84")
     baseline_subtracted = subtract_baseline(adc_data)
     result = compute_pressure_direction(baseline_subtracted)
+    cop_x, cop_y = result[0], result[1]
     dx, dy = result[6], result[7]
+    base_x, base_y = result[8], result[9]
     magnitude = result[10]
     state = int(result[11])
     pzt_angle, _ = compute_PZT_angle(dx, dy)
-    return pzt_angle, magnitude, state
+    return pzt_angle, magnitude, state, cop_x, cop_y, base_x, base_y
 
 
 # ===================== 重置基线（校准用） =====================
 def reset_baseline():
-    global first_frame, dynamic_thresh
+    global first_frame
     with first_frame_lock:
         first_frame = None
-    dynamic_thresh = None
-    noise_sum_buf.clear()
     reset_cop_state()