update serial and spatial force components
This commit is contained in:
Binary file not shown.
@@ -906,8 +906,361 @@ class PressureDirectionEstimator:
|
|||||||
|
|
||||||
return angle, mag
|
return angle, mag
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class LocalForceResult:
|
||||||
|
angle: float
|
||||||
|
magnitude: float
|
||||||
|
planar_x: float
|
||||||
|
planar_y: float
|
||||||
|
confidence: float
|
||||||
|
contact_active: bool
|
||||||
|
reportable: bool
|
||||||
|
total_pressure: float
|
||||||
|
peak: float
|
||||||
|
cop_x: float
|
||||||
|
cop_y: float
|
||||||
|
threshold: float
|
||||||
|
|
||||||
_estimator = PressureDirectionEstimator()
|
|
||||||
|
class LocalTangentialForceEstimator:
|
||||||
|
CONTACT_ENTER_TOTAL_THRESHOLD = 520.0
|
||||||
|
CONTACT_ENTER_PEAK_THRESHOLD = 50.0
|
||||||
|
CONTACT_EXIT_TOTAL_THRESHOLD = 260.0
|
||||||
|
CONTACT_EXIT_PEAK_THRESHOLD = 28.0
|
||||||
|
CONTACT_ENTER_FRAMES_REQUIRED = 2
|
||||||
|
CONTACT_EXIT_FRAMES_REQUIRED = 8
|
||||||
|
|
||||||
|
BASELINE_IDLE_ALPHA = 0.035
|
||||||
|
BASELINE_BOOTSTRAP_ALPHA = 1.0
|
||||||
|
BASELINE_NOISE_FLOOR = 5.0
|
||||||
|
|
||||||
|
ACTIVE_CELL_MIN_VALUE = 18.0
|
||||||
|
ACTIVE_CELL_PEAK_RATIO = 0.14
|
||||||
|
MIN_ACTIVE_CELLS = 3
|
||||||
|
|
||||||
|
VECTOR_SMOOTHING_ALPHA = 0.16
|
||||||
|
REPORT_MAGNITUDE_ENTER = 0.12
|
||||||
|
REPORT_MAGNITUDE_EXIT = 0.045
|
||||||
|
REPORT_CONFIDENCE_ENTER = 0.14
|
||||||
|
REPORT_CONFIDENCE_EXIT = 0.06
|
||||||
|
REPORT_HOLD_FRAMES = 10
|
||||||
|
|
||||||
|
ASYMMETRY_WEIGHT = 1.1
|
||||||
|
DRIFT_WEIGHT = 0.65
|
||||||
|
MOTION_WEIGHT = 0.25
|
||||||
|
EDGE_ASYMMETRY_DAMPING = 0.35
|
||||||
|
EDGE_INWARD_ROLLING_BIAS = 0.55
|
||||||
|
EDGE_START_COP_THRESHOLD = 0.45
|
||||||
|
EDGE_START_BIAS_WEIGHT = 1.1
|
||||||
|
ROLLING_FRICTION_ALPHA = 0.68
|
||||||
|
ROLLING_FRICTION_MIN_MAGNITUDE = 0.05
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
self.reset_all()
|
||||||
|
|
||||||
|
def reset_all(self):
|
||||||
|
self.baseline_frame = None
|
||||||
|
self.reset_contact_state()
|
||||||
|
|
||||||
|
def reset_contact_state(self):
|
||||||
|
self.contact_active = False
|
||||||
|
self.contact_enter_counter = 0
|
||||||
|
self.contact_exit_counter = 0
|
||||||
|
self.anchor_cop_x = None
|
||||||
|
self.anchor_cop_y = None
|
||||||
|
self.last_cop_x = None
|
||||||
|
self.last_cop_y = None
|
||||||
|
self.edge_start_bias_x = 0.0
|
||||||
|
self.edge_start_bias_y = 0.0
|
||||||
|
self.smoothed_x = 0.0
|
||||||
|
self.smoothed_y = 0.0
|
||||||
|
self.report_active = False
|
||||||
|
self.report_hold_counter = 0
|
||||||
|
self.held_report = None
|
||||||
|
|
||||||
|
def update(self, adc_data, timestamp_ms: float) -> LocalForceResult:
|
||||||
|
raw = np.asarray(adc_data, dtype=np.float32).flatten()
|
||||||
|
if len(raw) != ADC_LEN:
|
||||||
|
raise ValueError(f"ADC data length must be {ADC_LEN}")
|
||||||
|
|
||||||
|
baseline_subtracted = self._subtract_baseline(raw)
|
||||||
|
if not self._update_contact_state(raw, baseline_subtracted):
|
||||||
|
return self._inactive_result(float(np.sum(baseline_subtracted)), float(np.max(baseline_subtracted, initial=0.0)))
|
||||||
|
|
||||||
|
stats = self._compute_contact_stats(baseline_subtracted)
|
||||||
|
if stats is None:
|
||||||
|
return self._stabilize_report(self._weak_contact_result(float(np.sum(baseline_subtracted)), float(np.max(baseline_subtracted, initial=0.0))))
|
||||||
|
|
||||||
|
if self.anchor_cop_x is None:
|
||||||
|
self.anchor_cop_x = stats["cop_x"]
|
||||||
|
self.anchor_cop_y = stats["cop_y"]
|
||||||
|
self.last_cop_x = stats["cop_x"]
|
||||||
|
self.last_cop_y = stats["cop_y"]
|
||||||
|
self.edge_start_bias_x, self.edge_start_bias_y = self._edge_start_bias(stats)
|
||||||
|
return self._stabilize_report(self._weak_contact_result(stats["total"], stats["peak"], stats["cop_x"], stats["cop_y"]))
|
||||||
|
|
||||||
|
anchor_x = self.anchor_cop_x
|
||||||
|
anchor_y = self.anchor_cop_y if self.anchor_cop_y is not None else stats["cop_y"]
|
||||||
|
last_x = self.last_cop_x if self.last_cop_x is not None else stats["cop_x"]
|
||||||
|
last_y = self.last_cop_y if self.last_cop_y is not None else stats["cop_y"]
|
||||||
|
|
||||||
|
drift_x = stats["cop_x"] - anchor_x
|
||||||
|
drift_y = stats["cop_y"] - anchor_y
|
||||||
|
motion_x = stats["cop_x"] - last_x
|
||||||
|
motion_y = stats["cop_y"] - last_y
|
||||||
|
|
||||||
|
kinematic_x = drift_x * self.DRIFT_WEIGHT + motion_x * self.MOTION_WEIGHT
|
||||||
|
kinematic_y = drift_y * self.DRIFT_WEIGHT + motion_y * self.MOTION_WEIGHT
|
||||||
|
asymmetry_x, asymmetry_y = self._damp_edge_asymmetry(
|
||||||
|
stats,
|
||||||
|
kinematic_x + self.edge_start_bias_x,
|
||||||
|
kinematic_y + self.edge_start_bias_y,
|
||||||
|
)
|
||||||
|
|
||||||
|
combined_x = asymmetry_x + kinematic_x + self.edge_start_bias_x
|
||||||
|
combined_y = asymmetry_y + kinematic_y + self.edge_start_bias_y
|
||||||
|
combined_x, combined_y = self._apply_rolling_friction(
|
||||||
|
self.smoothed_x,
|
||||||
|
self.smoothed_y,
|
||||||
|
combined_x,
|
||||||
|
combined_y,
|
||||||
|
)
|
||||||
|
|
||||||
|
self.smoothed_x += (combined_x - self.smoothed_x) * self.VECTOR_SMOOTHING_ALPHA
|
||||||
|
self.smoothed_y += (combined_y - self.smoothed_y) * self.VECTOR_SMOOTHING_ALPHA
|
||||||
|
self.last_cop_x = stats["cop_x"]
|
||||||
|
self.last_cop_y = stats["cop_y"]
|
||||||
|
|
||||||
|
planar_x = self.smoothed_x
|
||||||
|
planar_y = -self.smoothed_y
|
||||||
|
angle, magnitude = self.compute_vector_angle(planar_x, planar_y)
|
||||||
|
|
||||||
|
active_span_rows = (stats["max_row"] - stats["min_row"] + 1) / SENSOR_ROWS
|
||||||
|
active_span_cols = (stats["max_col"] - stats["min_col"] + 1) / SENSOR_COLS
|
||||||
|
activity = min(max(stats["active_cells"] / ADC_LEN, 0.0), 1.0)
|
||||||
|
span = min(max((active_span_rows + active_span_cols) * 0.5, 0.0), 1.0)
|
||||||
|
pressure_ratio = min(max(stats["active_total"] / max(stats["total"], 1.0), 0.0), 1.0)
|
||||||
|
peak_ratio = min(max(stats["peak"] / (stats["total"] / stats["active_cells"] + 1.0), 0.0), 1.0)
|
||||||
|
confidence = min(max(activity * 0.35 + span * 0.2 + pressure_ratio * 0.3 + peak_ratio * 0.15, 0.0), 1.0)
|
||||||
|
|
||||||
|
return self._stabilize_report(LocalForceResult(
|
||||||
|
angle=angle,
|
||||||
|
magnitude=magnitude,
|
||||||
|
planar_x=planar_x,
|
||||||
|
planar_y=planar_y,
|
||||||
|
confidence=confidence,
|
||||||
|
contact_active=True,
|
||||||
|
reportable=False,
|
||||||
|
total_pressure=stats["total"],
|
||||||
|
peak=stats["peak"],
|
||||||
|
cop_x=stats["cop_x"],
|
||||||
|
cop_y=stats["cop_y"],
|
||||||
|
threshold=self.CONTACT_ENTER_TOTAL_THRESHOLD,
|
||||||
|
))
|
||||||
|
|
||||||
|
def _update_idle_baseline(self, raw_frame, alpha: float):
|
||||||
|
if self.baseline_frame is None:
|
||||||
|
self.baseline_frame = np.array(raw_frame, dtype=np.float32).copy()
|
||||||
|
return
|
||||||
|
self.baseline_frame += (raw_frame - self.baseline_frame) * alpha
|
||||||
|
|
||||||
|
def _subtract_baseline(self, raw_frame):
|
||||||
|
if self.baseline_frame is None:
|
||||||
|
self._update_idle_baseline(raw_frame, self.BASELINE_BOOTSTRAP_ALPHA)
|
||||||
|
diff = raw_frame - self.baseline_frame - self.BASELINE_NOISE_FLOOR
|
||||||
|
return np.clip(diff, 0, None)
|
||||||
|
|
||||||
|
def _pressure_metrics(self, frame):
|
||||||
|
return float(np.sum(frame)), float(np.max(frame, initial=0.0))
|
||||||
|
|
||||||
|
def _update_contact_state(self, raw_frame, frame) -> bool:
|
||||||
|
total, peak = self._pressure_metrics(frame)
|
||||||
|
enter = total >= self.CONTACT_ENTER_TOTAL_THRESHOLD and peak >= self.CONTACT_ENTER_PEAK_THRESHOLD
|
||||||
|
exit_frame = total <= self.CONTACT_EXIT_TOTAL_THRESHOLD or peak <= self.CONTACT_EXIT_PEAK_THRESHOLD
|
||||||
|
|
||||||
|
if self.contact_active:
|
||||||
|
if exit_frame:
|
||||||
|
self.contact_exit_counter += 1
|
||||||
|
if self.contact_exit_counter >= self.CONTACT_EXIT_FRAMES_REQUIRED:
|
||||||
|
self._update_idle_baseline(raw_frame, self.BASELINE_IDLE_ALPHA)
|
||||||
|
self.reset_contact_state()
|
||||||
|
return False
|
||||||
|
else:
|
||||||
|
self.contact_exit_counter = 0
|
||||||
|
return True
|
||||||
|
|
||||||
|
if enter:
|
||||||
|
self.contact_enter_counter += 1
|
||||||
|
if self.contact_enter_counter >= self.CONTACT_ENTER_FRAMES_REQUIRED:
|
||||||
|
self.contact_active = True
|
||||||
|
self.contact_enter_counter = 0
|
||||||
|
self.contact_exit_counter = 0
|
||||||
|
return True
|
||||||
|
return False
|
||||||
|
|
||||||
|
self.contact_enter_counter = 0
|
||||||
|
self._update_idle_baseline(raw_frame, self.BASELINE_IDLE_ALPHA)
|
||||||
|
return False
|
||||||
|
|
||||||
|
def _compute_contact_stats(self, frame):
|
||||||
|
total, peak = self._pressure_metrics(frame)
|
||||||
|
if total <= 0.0 or peak <= 0.0:
|
||||||
|
return None
|
||||||
|
|
||||||
|
active_threshold = max(peak * self.ACTIVE_CELL_PEAK_RATIO, self.ACTIVE_CELL_MIN_VALUE)
|
||||||
|
frame2d = np.asarray(frame, dtype=np.float32).reshape(SENSOR_ROWS, SENSOR_COLS)
|
||||||
|
active_mask = frame2d >= active_threshold
|
||||||
|
active_cells = int(np.count_nonzero(active_mask))
|
||||||
|
if active_cells < self.MIN_ACTIVE_CELLS:
|
||||||
|
return None
|
||||||
|
|
||||||
|
active_values = frame2d[active_mask]
|
||||||
|
active_total = float(np.sum(active_values))
|
||||||
|
if active_total <= 0.0:
|
||||||
|
return None
|
||||||
|
|
||||||
|
rows, cols = np.nonzero(active_mask)
|
||||||
|
cop_x = float(np.sum(active_values * cols) / active_total)
|
||||||
|
cop_y = float(np.sum(active_values * rows) / active_total)
|
||||||
|
min_row, max_row = int(np.min(rows)), int(np.max(rows))
|
||||||
|
min_col, max_col = int(np.min(cols)), int(np.max(cols))
|
||||||
|
bbox_center_x = (min_col + max_col) * 0.5
|
||||||
|
bbox_center_y = (min_row + max_row) * 0.5
|
||||||
|
half_width = max(max_col - min_col, 1) * 0.5
|
||||||
|
half_height = max(max_row - min_row, 1) * 0.5
|
||||||
|
asymmetry_x = float(np.sum(active_values * ((cols - bbox_center_x) / half_width)) / active_total)
|
||||||
|
asymmetry_y = float(np.sum(active_values * ((rows - bbox_center_y) / half_height)) / active_total)
|
||||||
|
|
||||||
|
return {
|
||||||
|
"total": total,
|
||||||
|
"peak": peak,
|
||||||
|
"active_total": active_total,
|
||||||
|
"active_cells": active_cells,
|
||||||
|
"min_row": min_row,
|
||||||
|
"max_row": max_row,
|
||||||
|
"min_col": min_col,
|
||||||
|
"max_col": max_col,
|
||||||
|
"cop_x": cop_x,
|
||||||
|
"cop_y": cop_y,
|
||||||
|
"asymmetry_x": asymmetry_x,
|
||||||
|
"asymmetry_y": asymmetry_y,
|
||||||
|
}
|
||||||
|
|
||||||
|
def _contact_touches_edge(self, stats) -> bool:
|
||||||
|
return (
|
||||||
|
stats["min_row"] == 0
|
||||||
|
or stats["max_row"] == SENSOR_ROWS - 1
|
||||||
|
or stats["min_col"] == 0
|
||||||
|
or stats["max_col"] == SENSOR_COLS - 1
|
||||||
|
)
|
||||||
|
|
||||||
|
def _damp_edge_asymmetry(self, stats, kinematic_x: float, kinematic_y: float):
|
||||||
|
asymmetry_x = stats["asymmetry_x"] * self.ASYMMETRY_WEIGHT
|
||||||
|
asymmetry_y = stats["asymmetry_y"] * self.ASYMMETRY_WEIGHT
|
||||||
|
|
||||||
|
if stats["min_col"] == 0 and asymmetry_x < 0.0:
|
||||||
|
asymmetry_x = -asymmetry_x * self.EDGE_INWARD_ROLLING_BIAS
|
||||||
|
if stats["max_col"] == SENSOR_COLS - 1 and asymmetry_x > 0.0:
|
||||||
|
asymmetry_x = -asymmetry_x * self.EDGE_INWARD_ROLLING_BIAS
|
||||||
|
if stats["min_row"] == 0 and asymmetry_y < 0.0:
|
||||||
|
asymmetry_y = -asymmetry_y * self.EDGE_INWARD_ROLLING_BIAS
|
||||||
|
if stats["max_row"] == SENSOR_ROWS - 1 and asymmetry_y > 0.0:
|
||||||
|
asymmetry_y = -asymmetry_y * self.EDGE_INWARD_ROLLING_BIAS
|
||||||
|
|
||||||
|
opposing_dot = asymmetry_x * kinematic_x + asymmetry_y * kinematic_y
|
||||||
|
kinematic_mag = float(np.hypot(kinematic_x, kinematic_y))
|
||||||
|
if self._contact_touches_edge(stats) and opposing_dot < 0.0 and kinematic_mag >= self.ROLLING_FRICTION_MIN_MAGNITUDE:
|
||||||
|
asymmetry_x *= self.EDGE_ASYMMETRY_DAMPING
|
||||||
|
asymmetry_y *= self.EDGE_ASYMMETRY_DAMPING
|
||||||
|
|
||||||
|
return asymmetry_x, asymmetry_y
|
||||||
|
|
||||||
|
def _edge_start_bias(self, stats):
|
||||||
|
center_x = (SENSOR_COLS - 1) * 0.5
|
||||||
|
center_y = (SENSOR_ROWS - 1) * 0.5
|
||||||
|
normalized_x = min(max((stats["cop_x"] - center_x) / max(center_x, 1.0), -1.0), 1.0)
|
||||||
|
normalized_y = min(max((stats["cop_y"] - center_y) / max(center_y, 1.0), -1.0), 1.0)
|
||||||
|
bias_x = self._edge_start_axis_bias(normalized_x) if stats["min_col"] == 0 or stats["max_col"] == SENSOR_COLS - 1 else 0.0
|
||||||
|
bias_y = self._edge_start_axis_bias(normalized_y) if stats["min_row"] == 0 or stats["max_row"] == SENSOR_ROWS - 1 else 0.0
|
||||||
|
return bias_x, bias_y
|
||||||
|
|
||||||
|
def _edge_start_axis_bias(self, normalized_axis: float) -> float:
|
||||||
|
distance = abs(normalized_axis)
|
||||||
|
if distance <= self.EDGE_START_COP_THRESHOLD:
|
||||||
|
return 0.0
|
||||||
|
strength = min(max((distance - self.EDGE_START_COP_THRESHOLD) / (1.0 - self.EDGE_START_COP_THRESHOLD), 0.0), 1.0)
|
||||||
|
return -np.sign(normalized_axis) * strength * self.EDGE_START_BIAS_WEIGHT
|
||||||
|
|
||||||
|
def _apply_rolling_friction(self, previous_x: float, previous_y: float, current_x: float, current_y: float):
|
||||||
|
previous_mag = float(np.hypot(previous_x, previous_y))
|
||||||
|
current_mag = float(np.hypot(current_x, current_y))
|
||||||
|
if previous_mag < self.ROLLING_FRICTION_MIN_MAGNITUDE or current_mag < self.ROLLING_FRICTION_MIN_MAGNITUDE:
|
||||||
|
return current_x, current_y
|
||||||
|
|
||||||
|
dot = previous_x * current_x + previous_y * current_y
|
||||||
|
if dot >= 0.0:
|
||||||
|
return current_x, current_y
|
||||||
|
|
||||||
|
mixed_x = current_x * (1.0 - self.ROLLING_FRICTION_ALPHA) + previous_x * self.ROLLING_FRICTION_ALPHA
|
||||||
|
mixed_y = current_y * (1.0 - self.ROLLING_FRICTION_ALPHA) + previous_y * self.ROLLING_FRICTION_ALPHA
|
||||||
|
if mixed_x * previous_x + mixed_y * previous_y >= 0.0:
|
||||||
|
return mixed_x, mixed_y
|
||||||
|
|
||||||
|
keep_mag = min(previous_mag, current_mag) * 0.5
|
||||||
|
return previous_x / previous_mag * keep_mag, previous_y / previous_mag * keep_mag
|
||||||
|
|
||||||
|
def _inactive_result(self, total_pressure=0.0, peak=0.0):
|
||||||
|
return LocalForceResult(0.0, 0.0, 0.0, 0.0, 0.0, False, False, total_pressure, peak, 0.0, 0.0, self.CONTACT_ENTER_TOTAL_THRESHOLD)
|
||||||
|
|
||||||
|
def _weak_contact_result(self, total_pressure=0.0, peak=0.0, cop_x=0.0, cop_y=0.0):
|
||||||
|
return LocalForceResult(0.0, 0.0, 0.0, 0.0, 0.0, True, False, total_pressure, peak, cop_x, cop_y, self.CONTACT_ENTER_TOTAL_THRESHOLD)
|
||||||
|
|
||||||
|
def _store_report(self, result: LocalForceResult):
|
||||||
|
result.reportable = True
|
||||||
|
self.report_active = True
|
||||||
|
self.report_hold_counter = 0
|
||||||
|
self.held_report = result
|
||||||
|
return result
|
||||||
|
|
||||||
|
def _hold_or_drop_report(self):
|
||||||
|
if self.report_active and self.report_hold_counter < self.REPORT_HOLD_FRAMES and self.held_report is not None:
|
||||||
|
self.report_hold_counter += 1
|
||||||
|
held = self.held_report
|
||||||
|
held.reportable = True
|
||||||
|
return held
|
||||||
|
self.report_active = False
|
||||||
|
self.report_hold_counter = 0
|
||||||
|
self.held_report = None
|
||||||
|
return self._weak_contact_result()
|
||||||
|
|
||||||
|
def _stabilize_report(self, result: LocalForceResult):
|
||||||
|
if not result.contact_active:
|
||||||
|
self.report_active = False
|
||||||
|
self.report_hold_counter = 0
|
||||||
|
self.held_report = None
|
||||||
|
return result
|
||||||
|
|
||||||
|
can_enter = result.magnitude >= self.REPORT_MAGNITUDE_ENTER and result.confidence >= self.REPORT_CONFIDENCE_ENTER
|
||||||
|
can_stay = result.magnitude >= self.REPORT_MAGNITUDE_EXIT and result.confidence >= self.REPORT_CONFIDENCE_EXIT
|
||||||
|
if self.report_active:
|
||||||
|
if can_stay:
|
||||||
|
return self._store_report(result)
|
||||||
|
return self._hold_or_drop_report()
|
||||||
|
if can_enter:
|
||||||
|
return self._store_report(result)
|
||||||
|
return result
|
||||||
|
|
||||||
|
def compute_vector_angle(self, x: float, y: float) -> Tuple[float, float]:
|
||||||
|
magnitude = float(np.hypot(x, y))
|
||||||
|
if magnitude <= np.finfo(np.float32).eps:
|
||||||
|
return 0.0, 0.0
|
||||||
|
angle = float(np.degrees(np.arctan2(y, x)))
|
||||||
|
if angle < 0.0:
|
||||||
|
angle += 360.0
|
||||||
|
return angle, magnitude
|
||||||
|
|
||||||
|
|
||||||
|
_estimator = LocalTangentialForceEstimator()
|
||||||
|
|
||||||
|
|
||||||
def reset_cop_state():
|
def reset_cop_state():
|
||||||
@@ -924,16 +1277,16 @@ def compute_pressure_direction(adc_data, timestamp_ms: float):
|
|||||||
return (
|
return (
|
||||||
result.cop_x,
|
result.cop_x,
|
||||||
result.cop_y,
|
result.cop_y,
|
||||||
result.row_min,
|
0,
|
||||||
result.row_max,
|
SENSOR_ROWS - 1,
|
||||||
result.col_min,
|
0,
|
||||||
result.col_max,
|
SENSOR_COLS - 1,
|
||||||
result.dx,
|
result.planar_x,
|
||||||
result.dy,
|
result.planar_y,
|
||||||
result.base_x,
|
0.0,
|
||||||
result.base_y,
|
0.0,
|
||||||
result.magnitude,
|
result.magnitude,
|
||||||
result.state,
|
1 if result.reportable else 0,
|
||||||
result.total_pressure,
|
result.total_pressure,
|
||||||
result.threshold,
|
result.threshold,
|
||||||
)
|
)
|
||||||
@@ -956,11 +1309,11 @@ def get_pzt_angle(adc_data, timestamp_ms: float):
|
|||||||
return (
|
return (
|
||||||
result.angle,
|
result.angle,
|
||||||
result.magnitude,
|
result.magnitude,
|
||||||
result.state,
|
1 if result.reportable else 0,
|
||||||
result.cop_x,
|
result.cop_x,
|
||||||
result.cop_y,
|
result.cop_y,
|
||||||
result.base_x,
|
0.0,
|
||||||
result.base_y,
|
0.0,
|
||||||
result.total_pressure,
|
result.total_pressure,
|
||||||
result.threshold,
|
result.threshold,
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -29,7 +29,6 @@
|
|||||||
export let summary: HudSummary;
|
export let summary: HudSummary;
|
||||||
export let pressureMatrix: number[] | null = null;
|
export let pressureMatrix: number[] | null = null;
|
||||||
export let spatialForce: HudSpatialForce | null = null;
|
export let spatialForce: HudSpatialForce | null = null;
|
||||||
export let devkitSpatialForce: HudSpatialForce | null = null;
|
|
||||||
export let showConfigPanel = false;
|
export let showConfigPanel = false;
|
||||||
export let configPanelTitle = "";
|
export let configPanelTitle = "";
|
||||||
export let configPanelHint = "";
|
export let configPanelHint = "";
|
||||||
@@ -278,41 +277,25 @@
|
|||||||
</div>
|
</div>
|
||||||
{/each}
|
{/each}
|
||||||
|
|
||||||
<div
|
|
||||||
class="panel-motion-shell"
|
|
||||||
in:fly={{ x: 180, duration: 340, opacity: 0.08, easing: cubicOut }}
|
|
||||||
out:fly={{ x: 180, duration: 280, opacity: 0.06, easing: cubicIn }}
|
|
||||||
>
|
|
||||||
<SpatialForcePanel
|
|
||||||
{spatialForce}
|
|
||||||
{locale}
|
|
||||||
side="right"
|
|
||||||
panelIndex={rightPanels.length}
|
|
||||||
panelCode="ALG"
|
|
||||||
panelTitle={locale === "zh-CN" ? "本地切向力" : "Local Tangential"}
|
|
||||||
badgeLabel={locale === "zh-CN" ? "算法" : "ALGO"}
|
|
||||||
/>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
<div
|
{#if spatialForce}
|
||||||
class="panel-motion-shell"
|
<div
|
||||||
in:fly={{ x: 180, duration: 340, opacity: 0.08, easing: cubicOut }}
|
class="panel-motion-shell"
|
||||||
out:fly={{ x: 180, duration: 280, opacity: 0.06, easing: cubicIn }}
|
in:fly={{ x: 180, duration: 340, opacity: 0.08, easing: cubicOut }}
|
||||||
>
|
out:fly={{ x: 180, duration: 280, opacity: 0.06, easing: cubicIn }}
|
||||||
<SpatialForcePanel
|
>
|
||||||
spatialForce={devkitSpatialForce}
|
<SpatialForcePanel
|
||||||
{locale}
|
{spatialForce}
|
||||||
side="right"
|
{locale}
|
||||||
panelIndex={rightPanels.length + 1}
|
side="right"
|
||||||
panelCode="DKT"
|
panelIndex={rightPanels.length}
|
||||||
panelTitle={locale === "zh-CN" ? "DevKit 切向力" : "DevKit Tangential"}
|
panelCode="3D"
|
||||||
badgeLabel="DEVKIT"
|
panelTitle={locale === "zh-CN" ? "三维力" : "3D Force"}
|
||||||
badgeTone="lime"
|
badgeLabel=""
|
||||||
showMetrics={false}
|
badgeTone="lime"
|
||||||
requireMagnitude={false}
|
/>
|
||||||
compactMetaText={locale === "zh-CN" ? "等待 DevKit 角度流" : "Waiting for DevKit angle"}
|
</div>
|
||||||
/>
|
{/if}
|
||||||
</div>
|
|
||||||
|
|
||||||
{#if summaryCurveVisible && summarySide === "right"}
|
{#if summaryCurveVisible && summarySide === "right"}
|
||||||
<div
|
<div
|
||||||
|
|||||||
@@ -9,17 +9,7 @@
|
|||||||
export let panelTitle = "";
|
export let panelTitle = "";
|
||||||
export let badgeLabel = "";
|
export let badgeLabel = "";
|
||||||
export let badgeTone: "cyan" | "lime" | "orange" = "cyan";
|
export let badgeTone: "cyan" | "lime" | "orange" = "cyan";
|
||||||
export let showMetrics = true;
|
|
||||||
export let requireMagnitude = true;
|
export let requireMagnitude = true;
|
||||||
export let compactMetaText = "";
|
|
||||||
|
|
||||||
function formatValue(value: number | null, digits = 1): string {
|
|
||||||
if (value === null || !Number.isFinite(value)) {
|
|
||||||
return "--";
|
|
||||||
}
|
|
||||||
|
|
||||||
return value.toFixed(digits);
|
|
||||||
}
|
|
||||||
|
|
||||||
function normalizeAngle(value: number): number {
|
function normalizeAngle(value: number): number {
|
||||||
return ((value % 360) + 360) % 360;
|
return ((value % 360) + 360) % 360;
|
||||||
@@ -83,7 +73,7 @@
|
|||||||
$: i18n =
|
$: i18n =
|
||||||
locale === "zh-CN"
|
locale === "zh-CN"
|
||||||
? {
|
? {
|
||||||
title: "切向力方向",
|
title: "三维力",
|
||||||
waiting: "等待数据",
|
waiting: "等待数据",
|
||||||
angle: "ANGLE",
|
angle: "ANGLE",
|
||||||
heading: "方向角",
|
heading: "方向角",
|
||||||
@@ -91,7 +81,7 @@
|
|||||||
confidence: "置信度"
|
confidence: "置信度"
|
||||||
}
|
}
|
||||||
: {
|
: {
|
||||||
title: "Tangential Direction",
|
title: "3D Force",
|
||||||
waiting: "Waiting",
|
waiting: "Waiting",
|
||||||
angle: "ANGLE",
|
angle: "ANGLE",
|
||||||
heading: "Heading",
|
heading: "Heading",
|
||||||
@@ -100,8 +90,6 @@
|
|||||||
};
|
};
|
||||||
$: resolvedTitle = panelTitle || i18n.title;
|
$: resolvedTitle = panelTitle || i18n.title;
|
||||||
$: resolvedBadgeLabel = badgeLabel || i18n.angle;
|
$: resolvedBadgeLabel = badgeLabel || i18n.angle;
|
||||||
$: resolvedCompactMetaText =
|
|
||||||
compactMetaText || (locale === "zh-CN" ? "仅使用角度流" : "Angle stream only");
|
|
||||||
|
|
||||||
$: hasData =
|
$: hasData =
|
||||||
spatialForce !== null &&
|
spatialForce !== null &&
|
||||||
@@ -109,35 +97,34 @@
|
|||||||
(!requireMagnitude || Number.isFinite(spatialForce.magnitude));
|
(!requireMagnitude || Number.isFinite(spatialForce.magnitude));
|
||||||
$: angleDeg = hasData ? normalizeAngle(spatialForce?.angleDeg ?? 0) : 0;
|
$: angleDeg = hasData ? normalizeAngle(spatialForce?.angleDeg ?? 0) : 0;
|
||||||
$: updateVisualAngle(angleDeg, hasData);
|
$: updateVisualAngle(angleDeg, hasData);
|
||||||
$: magnitude = hasData ? spatialForce?.magnitude ?? 0 : null;
|
|
||||||
$: confidence = hasData ? (spatialForce?.confidence ?? 0) * 100 : null;
|
|
||||||
</script>
|
</script>
|
||||||
|
|
||||||
<article
|
{#if hasData}
|
||||||
class="signal-panel spatial-panel side-{side}"
|
<article
|
||||||
class:is-empty={!hasData}
|
class="signal-panel spatial-panel side-{side}"
|
||||||
aria-hidden={false}
|
aria-label={resolvedTitle}
|
||||||
style="--panel-index: {panelIndex};"
|
style="--panel-index: {panelIndex};"
|
||||||
>
|
>
|
||||||
<header class="panel-head">
|
<header class="panel-head">
|
||||||
<div class="head-text">
|
<div class="head-text">
|
||||||
<p class="panel-code">{panelCode}</p>
|
<p class="panel-code">{panelCode}</p>
|
||||||
<p class="panel-title">{resolvedTitle}</p>
|
<p class="panel-title">{resolvedTitle}</p>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
<div class="icon-layer" aria-hidden="true">
|
{#if resolvedBadgeLabel}
|
||||||
<span class={`icon-chip tone-${badgeTone}`}>{resolvedBadgeLabel}</span>
|
<div class="icon-layer" aria-hidden="true">
|
||||||
</div>
|
<span class={`icon-chip tone-${badgeTone}`}>{resolvedBadgeLabel}</span>
|
||||||
</header>
|
</div>
|
||||||
|
{/if}
|
||||||
|
</header>
|
||||||
|
|
||||||
<div class="panel-body">
|
<div class="panel-body">
|
||||||
<div class="compass-stage">
|
<div class="compass-stage">
|
||||||
<div class="compass-core">
|
<div class="compass-core">
|
||||||
<div class="compass-ring compass-ring-outer"></div>
|
<div class="compass-ring compass-ring-outer"></div>
|
||||||
<div class="compass-ring compass-ring-inner"></div>
|
<div class="compass-ring compass-ring-inner"></div>
|
||||||
<div class="compass-axis axis-horizontal"></div>
|
<div class="compass-axis axis-horizontal"></div>
|
||||||
<div class="compass-axis axis-vertical"></div>
|
<div class="compass-axis axis-vertical"></div>
|
||||||
{#if hasData}
|
|
||||||
<div
|
<div
|
||||||
class="compass-vector"
|
class="compass-vector"
|
||||||
class:is-snap={snapVector}
|
class:is-snap={snapVector}
|
||||||
@@ -146,44 +133,29 @@
|
|||||||
<span class="vector-shaft"></span>
|
<span class="vector-shaft"></span>
|
||||||
<span class="vector-head"></span>
|
<span class="vector-head"></span>
|
||||||
</div>
|
</div>
|
||||||
{/if}
|
<div class="compass-center"></div>
|
||||||
<div class="compass-center"></div>
|
<span class="compass-label label-top">90</span>
|
||||||
<span class="compass-label label-top">90</span>
|
<span class="compass-label label-right">0</span>
|
||||||
<span class="compass-label label-right">0</span>
|
<span class="compass-label label-bottom">270</span>
|
||||||
<span class="compass-label label-bottom">270</span>
|
<span class="compass-label label-left">180</span>
|
||||||
<span class="compass-label label-left">180</span>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
{#if !hasData}
|
|
||||||
<div class="empty-state">
|
|
||||||
<span>{i18n.waiting}</span>
|
|
||||||
</div>
|
</div>
|
||||||
{/if}
|
</div>
|
||||||
</div>
|
</div>
|
||||||
|
</article>
|
||||||
<div class="angle-stage">
|
{/if}
|
||||||
<p class="angle-label">{i18n.heading}</p>
|
|
||||||
{#if showMetrics}
|
|
||||||
<p class="angle-meta">{i18n.strength}: {formatValue(magnitude, 2)}</p>
|
|
||||||
<p class="angle-meta">{i18n.confidence}: {hasData ? `${formatValue(confidence, 0)}%` : "--"}</p>
|
|
||||||
{:else}
|
|
||||||
<p class="angle-meta">{resolvedCompactMetaText}</p>
|
|
||||||
<p class="angle-meta">{hasData ? (locale === "zh-CN" ? "实时对比中" : "Live comparison") : "--"}</p>
|
|
||||||
{/if}
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
</article>
|
|
||||||
|
|
||||||
<style>
|
<style>
|
||||||
.signal-panel {
|
.signal-panel {
|
||||||
--offset-x: 12%;
|
--offset-x: 12%;
|
||||||
--enter-ms: 1800ms;
|
--enter-ms: 1800ms;
|
||||||
--fade-ms: 1000ms;
|
--fade-ms: 1000ms;
|
||||||
overflow: hidden;
|
overflow: hidden;
|
||||||
inline-size: min(100%, clamp(34rem, 44vw, 44rem));
|
inline-size: var(--rail-width, min(100%, clamp(34rem, 44vw, 44rem)));
|
||||||
|
max-inline-size: 100%;
|
||||||
|
box-sizing: border-box;
|
||||||
|
flex: 0 0 var(--rail-width, auto);
|
||||||
justify-self: start;
|
justify-self: start;
|
||||||
display: grid;
|
display: grid;
|
||||||
grid-template-rows: auto 1fr;
|
grid-template-rows: auto 1fr auto;
|
||||||
gap: 0.68rem;
|
gap: 0.68rem;
|
||||||
padding: 0.88rem 0.96rem 1rem;
|
padding: 0.88rem 0.96rem 1rem;
|
||||||
border: 1px solid rgb(var(--hud-border-strong-rgb) / 0.42);
|
border: 1px solid rgb(var(--hud-border-strong-rgb) / 0.42);
|
||||||
@@ -218,6 +190,16 @@
|
|||||||
opacity: 0.82;
|
opacity: 0.82;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
.spatial-panel::after {
|
||||||
|
content: "";
|
||||||
|
display: block;
|
||||||
|
block-size: 1rem;
|
||||||
|
}
|
||||||
|
|
||||||
|
.spatial-panel {
|
||||||
|
margin-block-end: clamp(0.8rem, 1.8vh, 1.4rem);
|
||||||
|
}
|
||||||
|
|
||||||
.panel-head {
|
.panel-head {
|
||||||
display: flex;
|
display: flex;
|
||||||
justify-content: space-between;
|
justify-content: space-between;
|
||||||
@@ -277,10 +259,10 @@
|
|||||||
|
|
||||||
.panel-body {
|
.panel-body {
|
||||||
display: grid;
|
display: grid;
|
||||||
grid-template-columns: minmax(0, 1.1fr) minmax(10rem, 0.9fr);
|
grid-template-columns: minmax(0, 1fr);
|
||||||
gap: 0.72rem;
|
gap: 0.72rem;
|
||||||
block-size: clamp(12rem, 15.5vw, 15rem);
|
block-size: clamp(12rem, 15.5vw, 15rem);
|
||||||
min-block-size: clamp(12rem, 15.5vw, 15rem);
|
min-block-size: 5rem;
|
||||||
}
|
}
|
||||||
|
|
||||||
.compass-stage {
|
.compass-stage {
|
||||||
@@ -433,76 +415,49 @@
|
|||||||
background: linear-gradient(180deg, rgb(var(--hud-surface-deep-rgb) / 0.06), rgb(var(--hud-surface-deep-rgb) / 0.18));
|
background: linear-gradient(180deg, rgb(var(--hud-surface-deep-rgb) / 0.06), rgb(var(--hud-surface-deep-rgb) / 0.18));
|
||||||
}
|
}
|
||||||
|
|
||||||
.angle-stage {
|
|
||||||
border: 1px solid rgb(var(--hud-border-rgb) / 0.26);
|
|
||||||
border-radius: 0.62rem;
|
|
||||||
padding: 0.9rem 0.85rem;
|
|
||||||
block-size: 100%;
|
|
||||||
min-block-size: 0;
|
|
||||||
overflow: hidden;
|
|
||||||
background:
|
|
||||||
linear-gradient(180deg, rgb(var(--hud-surface-rgb) / 0.72), rgb(var(--hud-surface-deep-rgb) / 0.84)),
|
|
||||||
radial-gradient(circle at 50% 0, rgb(var(--hud-glow-rgb) / 0.05), transparent 58%);
|
|
||||||
display: grid;
|
|
||||||
grid-template-rows: auto auto auto;
|
|
||||||
align-content: center;
|
|
||||||
justify-items: start;
|
|
||||||
gap: 0.36rem;
|
|
||||||
}
|
|
||||||
|
|
||||||
.angle-label {
|
|
||||||
margin: 0;
|
|
||||||
color: rgb(var(--hud-text-dim-rgb) / 0.82);
|
|
||||||
font-size: 0.68rem;
|
|
||||||
letter-spacing: 0.08em;
|
|
||||||
text-transform: uppercase;
|
|
||||||
}
|
|
||||||
|
|
||||||
.angle-meta {
|
|
||||||
margin: 0;
|
|
||||||
inline-size: 10rem;
|
|
||||||
min-block-size: 1rem;
|
|
||||||
white-space: nowrap;
|
|
||||||
overflow: hidden;
|
|
||||||
text-overflow: ellipsis;
|
|
||||||
color: rgb(var(--hud-text-dim-rgb) / 0.84);
|
|
||||||
font-size: 0.68rem;
|
|
||||||
font-variant-numeric: tabular-nums;
|
|
||||||
letter-spacing: 0.05em;
|
|
||||||
}
|
|
||||||
|
|
||||||
@media (max-width: 1180px) {
|
@media (max-width: 1180px) {
|
||||||
.signal-panel {
|
.signal-panel {
|
||||||
inline-size: min(100%, clamp(28rem, 40vw, 38rem));
|
inline-size: var(--rail-width, min(100%, clamp(28rem, 40vw, 38rem)));
|
||||||
|
}
|
||||||
|
|
||||||
|
.panel-body {
|
||||||
|
block-size: clamp(10rem, 13vw, 12rem);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@media (max-height: 900px) {
|
@media (max-height: 900px) {
|
||||||
.signal-panel {
|
.signal-panel {
|
||||||
inline-size: min(100%, clamp(28rem, 38vw, 36rem));
|
inline-size: var(--rail-width, min(100%, clamp(28rem, 38vw, 36rem)));
|
||||||
padding: 0.7rem 0.76rem 0.8rem;
|
padding: 0.7rem 0.76rem 0.8rem;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
.panel-body {
|
||||||
|
block-size: clamp(9.8rem, 12vw, 11.8rem);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@media (max-height: 760px) {
|
@media (max-height: 760px) {
|
||||||
.signal-panel {
|
.signal-panel {
|
||||||
inline-size: min(100%, clamp(24rem, 34vw, 30rem));
|
inline-size: var(--rail-width, min(100%, clamp(24rem, 34vw, 30rem)));
|
||||||
padding: 0.62rem 0.68rem 0.72rem;
|
padding: 0.62rem 0.68rem 0.72rem;
|
||||||
gap: 0.48rem;
|
gap: 0.48rem;
|
||||||
}
|
}
|
||||||
|
|
||||||
.panel-body {
|
.panel-body {
|
||||||
block-size: clamp(9rem, 10vw, 10.8rem);
|
block-size: clamp(8rem, 9.5vw, 9.8rem);
|
||||||
min-block-size: clamp(9rem, 10vw, 10.8rem);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@media (max-height: 680px) {
|
@media (max-height: 680px) {
|
||||||
.signal-panel {
|
.signal-panel {
|
||||||
inline-size: min(100%, clamp(20rem, 28vw, 26rem));
|
inline-size: var(--rail-width, min(100%, clamp(20rem, 28vw, 26rem)));
|
||||||
padding: 0.52rem 0.58rem 0.6rem;
|
padding: 0.52rem 0.58rem 0.6rem;
|
||||||
gap: 0.36rem;
|
gap: 0.36rem;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
.panel-body {
|
||||||
|
block-size: clamp(6.5rem, 8vw, 7.5rem);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@media (max-width: 900px) {
|
@media (max-width: 900px) {
|
||||||
|
|||||||
@@ -1092,8 +1092,7 @@
|
|||||||
hasSignalData =
|
hasSignalData =
|
||||||
signalPanels.length > 0 ||
|
signalPanels.length > 0 ||
|
||||||
packet.summary.points.length > 0 ||
|
packet.summary.points.length > 0 ||
|
||||||
spatialForce !== null ||
|
spatialForce !== null;
|
||||||
devkitSpatialForce !== null;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
function clearHudPanels(): void {
|
function clearHudPanels(): void {
|
||||||
@@ -1784,22 +1783,19 @@
|
|||||||
}
|
}
|
||||||
|
|
||||||
const angleDeg = Number(event.payload.angle);
|
const angleDeg = Number(event.payload.angle);
|
||||||
if (!Number.isFinite(angleDeg)) {
|
const magnitude = Number(event.payload.magnitude);
|
||||||
|
const isReportable = event.payload.state > 0 && Number.isFinite(magnitude) && magnitude > 0;
|
||||||
|
if (!Number.isFinite(angleDeg) || !isReportable) {
|
||||||
clearDevkitSpatialForce();
|
clearDevkitSpatialForce();
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
devkitSpatialForce = {
|
devkitSpatialForce = {
|
||||||
angleDeg,
|
angleDeg,
|
||||||
magnitude: Number.isFinite(event.payload.magnitude) ? event.payload.magnitude : 0,
|
magnitude,
|
||||||
confidence: event.payload.state
|
confidence: 0
|
||||||
};
|
};
|
||||||
scheduleDevkitSpatialForceClear();
|
scheduleDevkitSpatialForceClear();
|
||||||
hasSignalData =
|
|
||||||
signalPanels.length > 0 ||
|
|
||||||
summary.points.length > 0 ||
|
|
||||||
spatialForce !== null ||
|
|
||||||
devkitSpatialForce !== null;
|
|
||||||
})
|
})
|
||||||
.then((unlisten) => {
|
.then((unlisten) => {
|
||||||
if (disposed) {
|
if (disposed) {
|
||||||
@@ -1929,7 +1925,6 @@
|
|||||||
rightPanels={rightSignalPanels}
|
rightPanels={rightSignalPanels}
|
||||||
{pressureMatrix}
|
{pressureMatrix}
|
||||||
{spatialForce}
|
{spatialForce}
|
||||||
{devkitSpatialForce}
|
|
||||||
showConfigPanel={false}
|
showConfigPanel={false}
|
||||||
{summary}
|
{summary}
|
||||||
on:replaytoggle={handleReplayToggle}
|
on:replaytoggle={handleReplayToggle}
|
||||||
|
|||||||
Reference in New Issue
Block a user