feat: 切向力算法更新 + AD反解x模块
This commit is contained in:
lenn
@@ -26,7 +26,6 @@ import sys
|
||||
import time
|
||||
from concurrent import futures
|
||||
from pathlib import Path
|
||||
|
||||
import grpc
|
||||
import sensor_stream_pb2
|
||||
import sensor_stream_pb2_grpc
|
||||
@@ -274,12 +273,13 @@ class SensorPushServicer(sensor_stream_pb2_grpc.SensorPushServicer):
|
||||
ok = True
|
||||
message = "OK"
|
||||
cop_x = cop_y = base_x = base_y = 0.0
|
||||
total_press = 0.0
|
||||
threshold = 0.0
|
||||
if len(frame.matrix) == SENSOR_ROWS * SENSOR_COLS:
|
||||
try:
|
||||
angle, magnitude, state, cop_x, cop_y, base_x, base_y = get_pzt_angle(frame.matrix)
|
||||
angle, magnitude, state, cop_x, cop_y, base_x, base_y, total_press, threshold = get_pzt_angle(frame.matrix)
|
||||
self.last_angle = angle
|
||||
# 打印接收到的数据和计算结果
|
||||
print(f"[PZT] seq={frame.seq} pzt_angle={angle:.2f} magnitude={magnitude:.4f} state={state}")
|
||||
print(f"devkit: angle={angle:.2f}, magnitude={magnitude:.4f}, state={state}, cop_x={cop_x:.4f}, cop_y={cop_y:.4f}, base_x={base_x:.4f}, base_y={base_y:.4f}, total_press={total_press:.2f}, thresh={threshold:.2f}")
|
||||
except Exception as e:
|
||||
ok = False
|
||||
message = str(e)
|
||||
@@ -307,6 +307,14 @@ class SensorPushServicer(sensor_stream_pb2_grpc.SensorPushServicer):
|
||||
dts_ms=frame.dts_ms,
|
||||
ok=ok,
|
||||
message=message,
|
||||
magnitude=magnitude,
|
||||
state=state,
|
||||
cop_x=cop_x,
|
||||
cop_y=cop_y,
|
||||
base_x=base_x,
|
||||
base_y=base_y,
|
||||
total_press=total_press,
|
||||
threshold=threshold,
|
||||
)
|
||||
|
||||
if self.frame_count % 100 == 0:
|
||||
@@ -388,25 +396,21 @@ def serve(port: int):
|
||||
|
||||
|
||||
import numpy as np
|
||||
import threading
|
||||
from collections import deque
|
||||
|
||||
# ===================== 算法参数=====================
|
||||
COP_INIT_MEDIAN_FRAMES = 15 # 初始COP取中位数的帧数
|
||||
NOISE_COLLECT_FRAMES = 20 # 动态阈值基线采集帧数
|
||||
COP_INIT_MEDIAN_FRAMES = 1 # 初始COP取中位数的帧数
|
||||
NOISE_COLLECT_FRAMES = 10 # 动态阈值基线采集帧数
|
||||
THRESH_K = 5 # 阈值 = K * mean
|
||||
SENSOR_ROWS = 12
|
||||
SENSOR_COLS = 7
|
||||
|
||||
# ===================== 二次静置精修参数 =====================
|
||||
POST_INIT_WINDOW_CNT = 60000
|
||||
POST_INIT_STABLE_CNT = 200
|
||||
POST_INIT_STABLE_CNT = 100
|
||||
POST_INIT_STABLE_THRESH = 0.1
|
||||
|
||||
# ===================== 线程安全全局状态 =====================
|
||||
first_frame = None
|
||||
first_frame_lock = threading.Lock()
|
||||
|
||||
first_contact_CoP_x = None
|
||||
first_contact_CoP_y = None
|
||||
contact_initialized = False
|
||||
@@ -427,19 +431,6 @@ post_cand_x = None
|
||||
post_cand_y = None
|
||||
|
||||
|
||||
# ===================== 基线减除 =====================
|
||||
def subtract_baseline(current_frame):
|
||||
global first_frame
|
||||
current_frame = np.array(current_frame, dtype=np.float32).flatten()
|
||||
|
||||
with first_frame_lock:
|
||||
if first_frame is None:
|
||||
first_frame = current_frame.copy()
|
||||
|
||||
diff = current_frame - first_frame
|
||||
return np.clip(diff, 0, None)
|
||||
|
||||
|
||||
# ===================== 重置CoP状态 =====================
|
||||
def reset_cop_state():
|
||||
global first_contact_CoP_x, first_contact_CoP_y, contact_initialized
|
||||
@@ -459,14 +450,14 @@ def reset_cop_state():
|
||||
|
||||
|
||||
# ===================== CoP压力中心计算 =====================
|
||||
def compute_pressure_direction(baseline_subtracted_frame):
|
||||
def compute_pressure_direction(raw_frame):
|
||||
global first_contact_CoP_x, first_contact_CoP_y, contact_initialized
|
||||
global post_init_frame_cnt, post_stable_cnt, post_refined_flag
|
||||
global post_cand_x, post_cand_y
|
||||
global noise_sum_buf, dynamic_thresh
|
||||
|
||||
rows, cols = SENSOR_ROWS, SENSOR_COLS
|
||||
frame_flat = np.asarray(baseline_subtracted_frame, dtype=np.float32).flatten()
|
||||
frame_flat = np.asarray(raw_frame, dtype=np.float32).flatten()
|
||||
frame2d = frame_flat.reshape(rows, cols)
|
||||
|
||||
total_pressure = np.sum(frame2d)
|
||||
@@ -479,13 +470,10 @@ def compute_pressure_direction(baseline_subtracted_frame):
|
||||
dynamic_thresh = THRESH_K * float(np.mean(sums))
|
||||
|
||||
# 低压重置
|
||||
if dynamic_thresh is not None and total_pressure < dynamic_thresh:
|
||||
if contact_initialized:
|
||||
if total_pressure == 0 or (dynamic_thresh is not None and total_pressure < dynamic_thresh):
|
||||
if contact_initialized and dynamic_thresh is not None:
|
||||
reset_cop_state()
|
||||
return 0.0, 0.0, 0, rows-1, 0, cols-1, 0.0, 0.0, 0.0, 0.0, 0.0, 0
|
||||
|
||||
if total_pressure == 0:
|
||||
return 0.0, 0.0, 0, rows-1, 0, cols-1, 0.0, 0.0, 0.0, 0.0, 0.0, 0
|
||||
return 0.0, 0.0, 0, rows-1, 0, cols-1, 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0.0, dynamic_thresh
|
||||
|
||||
x_grid = np.tile(np.arange(cols), (rows, 1))
|
||||
y_grid = np.repeat(np.arange(rows), cols).reshape(rows, cols)
|
||||
@@ -503,14 +491,8 @@ def compute_pressure_direction(baseline_subtracted_frame):
|
||||
cop_init_y_buf.append(cop_y)
|
||||
|
||||
if len(cop_init_x_buf) >= COP_INIT_MEDIAN_FRAMES:
|
||||
xs = list(cop_init_x_buf)
|
||||
ys = list(cop_init_y_buf)
|
||||
first_contact_CoP_x = float(np.median(xs))
|
||||
first_contact_CoP_y = float(np.median(ys))
|
||||
print(f"[CoP Init] 前{COP_INIT_MEDIAN_FRAMES}帧坐标:")
|
||||
for i in range(len(xs)):
|
||||
print(f" frame {i}: x={xs[i]:.3f}, y={ys[i]:.3f}")
|
||||
print(f" 中位数: x={first_contact_CoP_x:.3f}, y={first_contact_CoP_y:.3f}")
|
||||
first_contact_CoP_x = float(np.median(cop_init_x_buf))
|
||||
first_contact_CoP_y = float(np.median(cop_init_y_buf))
|
||||
contact_initialized = True
|
||||
cop_init_x_buf.clear()
|
||||
cop_init_y_buf.clear()
|
||||
@@ -557,7 +539,8 @@ def compute_pressure_direction(baseline_subtracted_frame):
|
||||
0, rows-1, 0, cols-1,
|
||||
delta_CoP_x, delta_CoP_y,
|
||||
base_x, base_y,
|
||||
magnitude, state)
|
||||
magnitude, state,
|
||||
total_pressure, dynamic_thresh)
|
||||
|
||||
|
||||
# ===================== 角度计算核心 =====================
|
||||
@@ -577,22 +560,20 @@ def compute_PZT_angle(Px: float, Py: float) -> tuple[float, float]:
|
||||
def get_pzt_angle(adc_data):
|
||||
if len(adc_data) != 84:
|
||||
raise ValueError("ADC数据长度必须为84")
|
||||
baseline_subtracted = subtract_baseline(adc_data)
|
||||
result = compute_pressure_direction(baseline_subtracted)
|
||||
result = compute_pressure_direction(adc_data)
|
||||
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])
|
||||
total_press = result[12]
|
||||
threshold = result[13]
|
||||
pzt_angle, _ = compute_PZT_angle(dx, dy)
|
||||
return pzt_angle, magnitude, state, cop_x, cop_y, base_x, base_y
|
||||
return pzt_angle, magnitude, state, cop_x, cop_y, base_x, base_y, total_press, threshold
|
||||
|
||||
|
||||
# ===================== 重置基线（校准用） =====================
|
||||
def reset_baseline():
|
||||
global first_frame
|
||||
with first_frame_lock:
|
||||
first_frame = None
|
||||
reset_cop_state()
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user