feat: update CAN integration and T2G seat firmware
This commit is contained in:
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -19,7 +19,6 @@ MOTION_AXIS_BITS = {
|
||||
"seat.set_legrest_extension": 4,
|
||||
}
|
||||
MOTION_STATUS_MESSAGES = {
|
||||
"front_left": "Driver_Seat_Motion_Status_558",
|
||||
"front_right": "Passenger_Seat_Motion_Status_559",
|
||||
}
|
||||
|
||||
|
||||
@@ -9,14 +9,10 @@ from .matrix import SeatCanMatrix
|
||||
|
||||
|
||||
COMMAND_TO_FEEDBACK = {
|
||||
"Driver_Seat_Ventilation_S": "Pilot_Seat_Vtilat_Stat_S",
|
||||
"Driver_Seat_Heating_S": "Pilot_Seat_Heat_Stat_S",
|
||||
"Front_Passenger_Seat_Ventilation_S": "Copilot_Seat_Vtilat_Stat_4FA_S",
|
||||
"Front_Passenger_Seat_Heating_S": "Copilot_Seat_Heat_Stat_4FA_S",
|
||||
}
|
||||
OPERATION_TO_COMMAND = {
|
||||
("front_left", "seat.set_ventilation"): "Driver_Seat_Ventilation_S",
|
||||
("front_left", "seat.set_heating"): "Driver_Seat_Heating_S",
|
||||
("front_right", "seat.set_ventilation"): "Front_Passenger_Seat_Ventilation_S",
|
||||
("front_right", "seat.set_heating"): "Front_Passenger_Seat_Heating_S",
|
||||
}
|
||||
|
||||
@@ -7,7 +7,6 @@ from typing import Any
|
||||
|
||||
|
||||
TARGETS = {
|
||||
"driver": {"device": "seat", "zone": "front_left", "instance": "driver"},
|
||||
"passenger": {"device": "seat", "zone": "front_right", "instance": "passenger"},
|
||||
}
|
||||
CAPABILITIES = {
|
||||
|
||||
@@ -16,7 +16,7 @@ STATE_VALUES = {"on": 1, "off": 2}
|
||||
class T2GSeatCompiler:
|
||||
"""Deterministic compiler for the HIL-verified T2G seat bench protocol."""
|
||||
|
||||
version = "1.3.0"
|
||||
version = "2.0.0"
|
||||
|
||||
def __init__(self, mapping_path: Path, calibration_path: Path):
|
||||
self.mapping = yaml.safe_load(mapping_path.read_text(encoding="utf-8"))
|
||||
@@ -35,7 +35,7 @@ class T2GSeatCompiler:
|
||||
actuator_state: dict[str, Any] | None = None,
|
||||
) -> dict[str, Any]:
|
||||
operations = function_config["spec"]["operations"]
|
||||
self._validate_shared_physical_targets(operations)
|
||||
self._validate_passenger_targets(operations)
|
||||
thermal_operations = [
|
||||
item
|
||||
for item in operations
|
||||
@@ -86,7 +86,7 @@ class T2GSeatCompiler:
|
||||
operations: list[dict[str, Any]],
|
||||
actuator_state: dict[str, Any],
|
||||
) -> list[dict[str, Any]]:
|
||||
state = actuator_state.get("driver") or actuator_state.get("passenger", {})
|
||||
state = actuator_state.get("passenger", {})
|
||||
desired = {
|
||||
"ventilation": dict(state.get("ventilation", {"state": "off", "level": 0})),
|
||||
"heating": dict(state.get("heating", {"state": "off", "level": 0})),
|
||||
@@ -159,7 +159,10 @@ class T2GSeatCompiler:
|
||||
raise ValueError(
|
||||
f"T2G IAR firmware has no calibrated actuator for {capability}"
|
||||
) from exc
|
||||
maximum = int(axis["maximum_value_x100"])
|
||||
maximum = int(self.protocol["normalized_position_max"])
|
||||
absolute_timeout_ms = int(
|
||||
self.protocol["absolute_position_timeout_ms"]
|
||||
)
|
||||
position_pct = int(operation["parameters"]["position_pct"])
|
||||
target = round(maximum * position_pct / 100)
|
||||
tolerance = max(1, round(maximum / 100))
|
||||
@@ -195,20 +198,29 @@ class T2GSeatCompiler:
|
||||
"Motor_Selector": int(axis["motor_selector"]),
|
||||
"Opcode": int(self.protocol["absolute_position_opcode"]),
|
||||
"Param_A": target,
|
||||
"Param_B": int(self.protocol["default_duty_percent"]),
|
||||
# Opcode 1 Param_B is a timeout override, not duty.
|
||||
# The explicit calibrated value covers a full M0 stroke;
|
||||
# the legacy per-axis default can stop it at about 79%.
|
||||
"Param_B": absolute_timeout_ms,
|
||||
"Protocol_Version": int(self.protocol["version"]),
|
||||
# Allocated immediately before hardware transmission. Zero keeps
|
||||
# the compiled plan deterministic and is never put on the bus.
|
||||
"Sequence": 0,
|
||||
},
|
||||
"send_offset_ms": (command_offset + index) * 100,
|
||||
"send_offset_ms": (
|
||||
(command_offset + index)
|
||||
* int(self.protocol["inter_axis_settle_ms"])
|
||||
),
|
||||
"completion": {
|
||||
"message": "T2G_Seat_Move_Motor_Status_302",
|
||||
"signal": "Value_X100",
|
||||
"operator": "within",
|
||||
"value": target,
|
||||
"tolerance": tolerance,
|
||||
"timeout_ms": int(operation.get("timeout_ms", 8000)),
|
||||
"timeout_ms": max(
|
||||
int(operation.get("timeout_ms", 0)),
|
||||
absolute_timeout_ms + 3000,
|
||||
),
|
||||
},
|
||||
}
|
||||
)
|
||||
@@ -261,38 +273,36 @@ class T2GSeatCompiler:
|
||||
"Motor_Selector": int(axis["motor_selector"]),
|
||||
"Opcode": int(self.protocol["timed_duty_opcode"]),
|
||||
"Param_A": duration_ms,
|
||||
"Param_B": int(self.protocol["default_duty_percent"]),
|
||||
"Param_B": int(self.protocol["default_duty_percent"])
|
||||
* int(self.protocol["timed_duty_level_scale"]),
|
||||
"Protocol_Version": int(self.protocol["version"]),
|
||||
"Sequence": 0,
|
||||
},
|
||||
"send_offset_ms": command_index * 100,
|
||||
"send_offset_ms": (
|
||||
command_index * int(self.protocol["inter_axis_settle_ms"])
|
||||
),
|
||||
"completion": {
|
||||
"message": "T2G_Seat_Move_Motor_Status_302",
|
||||
"signal": "Task_State",
|
||||
"operator": "eq",
|
||||
"value": 8,
|
||||
"value": 6,
|
||||
"timeout_ms": timeout_ms,
|
||||
},
|
||||
}
|
||||
|
||||
@staticmethod
|
||||
def _validate_shared_physical_targets(
|
||||
def _validate_passenger_targets(
|
||||
operations: list[dict[str, Any]],
|
||||
) -> None:
|
||||
requested: dict[str, dict[str, Any]] = {}
|
||||
for operation in operations:
|
||||
capability = operation["capability"]
|
||||
request = {
|
||||
"parameters": dict(operation.get("parameters", {})),
|
||||
"motion": dict(operation["motion"]) if operation.get("motion") else None,
|
||||
}
|
||||
previous = requested.get(capability)
|
||||
if previous is not None and previous != request:
|
||||
target = operation.get("target", {})
|
||||
if (
|
||||
target.get("instance") != "passenger"
|
||||
or target.get("zone") != "front_right"
|
||||
):
|
||||
raise ValueError(
|
||||
"driver and passenger aliases request conflicting values for "
|
||||
f"the same physical capability {capability}"
|
||||
"T2G hardware accepts only passenger/front_right seat operations"
|
||||
)
|
||||
requested[capability] = request
|
||||
|
||||
def _require_supported(self, operation: dict[str, Any]) -> None:
|
||||
key = (operation["capability"], operation["target"]["zone"])
|
||||
|
||||
@@ -18,13 +18,15 @@ MOTOR_RESULT_NAMES = {
|
||||
0: "NONE",
|
||||
1: "RUNNING",
|
||||
2: "OK",
|
||||
3: "STOPPED",
|
||||
4: "STALL",
|
||||
5: "ENDPOINT",
|
||||
3: "STOPPED_BY_CMD",
|
||||
4: "ENDPOINT_REACHED",
|
||||
5: "STALL",
|
||||
6: "TIMEOUT",
|
||||
7: "DRIVER_FAULT",
|
||||
8: "INVALID",
|
||||
9: "CALIBRATION_FAILED",
|
||||
8: "HALL_FAULT",
|
||||
9: "INVALID_CMD",
|
||||
10: "INTERLOCKED",
|
||||
11: "CALIBRATION_FAILED",
|
||||
}
|
||||
|
||||
MOTOR_EVENT_NAMES = {
|
||||
@@ -39,6 +41,26 @@ MOTOR_EVENT_NAMES = {
|
||||
8: "CALIBRATION_FAILED",
|
||||
}
|
||||
|
||||
ENDPOINT_RECOVERY_PROBE_DURATION_MS = 500
|
||||
ENDPOINT_RECOVERY_PROBE_LEVEL_X10 = 500
|
||||
ENDPOINT_RECOVERY_MAX_STATIONARY_HALL_DELTA = 2
|
||||
ENDPOINT_RECOVERY_MAX_NEAR_HALL = 8
|
||||
ENDPOINT_RECOVERY_MAX_NEAR_HALL_PERCENT = 15
|
||||
|
||||
PHYSICAL_MOTOR_AXES = {
|
||||
0: "longitudinal",
|
||||
1: "cushion_tilt",
|
||||
2: "backrest_recline",
|
||||
3: "legrest_height",
|
||||
}
|
||||
PHYSICAL_AXIS_CAPABILITIES = {
|
||||
axis: capability
|
||||
for capability, axis in MOTION_CAPABILITY_TO_AXIS.items()
|
||||
if axis in PHYSICAL_MOTOR_AXES.values()
|
||||
}
|
||||
MOTION_ACTIVE_TASK_STATES = {1, 2, 3, 4, 5}
|
||||
MOTION_FAULT_RESULTS = {5, 6, 7, 8, 9, 10, 11}
|
||||
|
||||
|
||||
class HardwareExecutionError(RuntimeError):
|
||||
"""Raised for rejected, faulted or timed-out physical operations."""
|
||||
@@ -68,6 +90,10 @@ class HardwareSeatExecutor:
|
||||
thermal_timeout_ms: int = 2_000,
|
||||
ack_timeout_ms: int = 1_500,
|
||||
axis_maximum_x100: dict[str, int] | None = None,
|
||||
motor_max_hall: dict[int, int] | None = None,
|
||||
stop_all_motor_selector: int = 0,
|
||||
auto_home: bool = False,
|
||||
home_timeout_s: int = 120,
|
||||
firmware_guard: T2GFirmwareGuard | None = None,
|
||||
):
|
||||
self.matrix = matrix
|
||||
@@ -75,7 +101,14 @@ class HardwareSeatExecutor:
|
||||
self.thermal_timeout_ms = thermal_timeout_ms
|
||||
self.ack_timeout_ms = ack_timeout_ms
|
||||
self.axis_maximum_x100 = dict(axis_maximum_x100 or {})
|
||||
self.motor_max_hall = dict(motor_max_hall or {})
|
||||
if stop_all_motor_selector not in range(4):
|
||||
raise ValueError("stop_all_motor_selector must be between 0 and 3")
|
||||
self.stop_all_motor_selector = stop_all_motor_selector
|
||||
self.auto_home = auto_home
|
||||
self.home_timeout_s = home_timeout_s
|
||||
self.firmware_guard = firmware_guard
|
||||
self._current_samples: dict[int, list[dict[str, Any]]] = {}
|
||||
|
||||
@classmethod
|
||||
def _next_sequence(cls) -> int:
|
||||
@@ -100,6 +133,8 @@ class HardwareSeatExecutor:
|
||||
has_motion = any(item["message"] == "T2G_Seat_Move_Command_300" for item in commands)
|
||||
bus: CanBus | None = None
|
||||
motion_armed = False
|
||||
physical_state: dict[str, Any] | None = None
|
||||
self._current_samples = {}
|
||||
|
||||
with self._adapter_lock:
|
||||
try:
|
||||
@@ -113,7 +148,7 @@ class HardwareSeatExecutor:
|
||||
"message": (
|
||||
"repository Seat_Controller_demo was programmed and verified"
|
||||
if firmware_telemetry.get("programmed_this_request")
|
||||
else "repository Seat_Controller_demo identity was verified"
|
||||
else "repository Seat_Controller_demo protocol signature was verified"
|
||||
),
|
||||
"telemetry": {
|
||||
**firmware_telemetry,
|
||||
@@ -177,10 +212,23 @@ class HardwareSeatExecutor:
|
||||
)
|
||||
finally:
|
||||
if bus is not None:
|
||||
try:
|
||||
physical_state = self._capture_physical_state(bus, results)
|
||||
except Exception: # noqa: BLE001 - do not mask the command outcome
|
||||
physical_state = self._physical_state_from_results(results)
|
||||
bus.close()
|
||||
|
||||
succeeded = all(item["status"] == "succeeded" for item in results)
|
||||
request_id = function_config["metadata"]["request_id"]
|
||||
spec: dict[str, Any] = {
|
||||
"mode": "hil",
|
||||
"status": "succeeded" if succeeded else "failed",
|
||||
"started_at": started_at,
|
||||
"finished_at": _now(),
|
||||
"results": results,
|
||||
}
|
||||
if physical_state is not None:
|
||||
spec["physical_state"] = physical_state
|
||||
return {
|
||||
"apiVersion": "cognitive-agent/v1alpha1",
|
||||
"kind": "ExecutionReport",
|
||||
@@ -191,15 +239,166 @@ class HardwareSeatExecutor:
|
||||
"request_id": request_id,
|
||||
"labels": {"simulation": "false", "feedback": "physical_can"},
|
||||
},
|
||||
"spec": {
|
||||
"mode": "hil",
|
||||
"status": "succeeded" if succeeded else "failed",
|
||||
"started_at": started_at,
|
||||
"finished_at": _now(),
|
||||
"results": results,
|
||||
},
|
||||
"spec": spec,
|
||||
}
|
||||
|
||||
def read_physical_state(self) -> dict[str, Any]:
|
||||
"""Read the passenger seat status without transmitting a CAN command."""
|
||||
|
||||
with self._adapter_lock:
|
||||
bus = self.bus_factory()
|
||||
try:
|
||||
state = self._capture_physical_state(bus, [])
|
||||
finally:
|
||||
bus.close()
|
||||
if not state.get("position") and not any(
|
||||
state.get(feature) for feature in ("ventilation", "heating")
|
||||
):
|
||||
raise HardwareExecutionError(
|
||||
"no physical T2G seat status was received from CAN 0x302/0x4FA"
|
||||
)
|
||||
return state
|
||||
|
||||
def _capture_physical_state(
|
||||
self,
|
||||
bus: CanBus,
|
||||
results: list[dict[str, Any]],
|
||||
) -> dict[str, Any]:
|
||||
state = self._physical_state_from_results(results)
|
||||
deadline = time.monotonic() + max(
|
||||
self.thermal_timeout_ms,
|
||||
self.ack_timeout_ms,
|
||||
1_000,
|
||||
) / 1000
|
||||
while not self._physical_state_complete(state):
|
||||
remaining_ms = round((deadline - time.monotonic()) * 1000)
|
||||
if remaining_ms <= 0:
|
||||
break
|
||||
frame = bus.receive(remaining_ms)
|
||||
if frame is None:
|
||||
break
|
||||
self._observe_auxiliary_frame(frame)
|
||||
if frame.arbitration_id == 0x302 and len(frame.data) >= 8:
|
||||
values = self.matrix.decode_by_frame_id(0x302, frame.data)
|
||||
self._record_motion_state(state, values, frame)
|
||||
elif frame.arbitration_id == 0x4FA and len(frame.data) >= 8:
|
||||
values = self.matrix.decode_by_frame_id(0x4FA, frame.data)
|
||||
self._record_thermal_state(state, values, frame)
|
||||
state["captured_at"] = _now()
|
||||
state["source"] = "physical_can"
|
||||
state["target"] = {
|
||||
"device": "seat",
|
||||
"zone": "front_right",
|
||||
"instance": "passenger",
|
||||
}
|
||||
state["complete"] = self._physical_state_complete(state)
|
||||
state["missing"] = self._physical_state_missing(state)
|
||||
return state
|
||||
|
||||
def _physical_state_from_results(
|
||||
self, results: list[dict[str, Any]]
|
||||
) -> dict[str, Any]:
|
||||
state: dict[str, Any] = {"position": {}}
|
||||
for item in results:
|
||||
telemetry = item.get("telemetry", {})
|
||||
if item.get("status") != "succeeded":
|
||||
continue
|
||||
axis = telemetry.get("motion_axis")
|
||||
position = telemetry.get("effective_position_pct")
|
||||
if axis in PHYSICAL_AXIS_CAPABILITIES and position is not None:
|
||||
state["position"][str(axis)] = {
|
||||
"position_pct": int(position),
|
||||
"position_valid": bool(telemetry.get("position_valid", True)),
|
||||
"motion_state": "stopped",
|
||||
"fault": False,
|
||||
"feedback_frame": telemetry.get("feedback_frame"),
|
||||
}
|
||||
feature = str(telemetry.get("feedback_signal", ""))
|
||||
raw = telemetry.get("feedback_value")
|
||||
if raw is None:
|
||||
continue
|
||||
if feature == "Driver_Seat_Heating_Status":
|
||||
self._set_thermal_feature(state, "heating", int(raw))
|
||||
elif feature == "Driver_Seat_Ventilation_Status":
|
||||
self._set_thermal_feature(state, "ventilation", int(raw))
|
||||
return state
|
||||
|
||||
def _record_motion_state(
|
||||
self,
|
||||
state: dict[str, Any],
|
||||
values: dict[str, int],
|
||||
frame: CanFrame,
|
||||
) -> None:
|
||||
motor = int(values["Motor_Sub_ID"])
|
||||
axis = PHYSICAL_MOTOR_AXES.get(motor)
|
||||
if axis is None:
|
||||
return
|
||||
capability = PHYSICAL_AXIS_CAPABILITIES[axis]
|
||||
maximum = max(1, int(self.axis_maximum_x100.get(capability, 10_000)))
|
||||
task_state = int(values["Task_State"])
|
||||
result = int(values["Result"])
|
||||
position_pct = round(int(values["Value_X100"]) * 100 / maximum)
|
||||
state.setdefault("position", {})[axis] = {
|
||||
"position_pct": max(0, min(100, position_pct)),
|
||||
"position_valid": bool(values["Position_Valid"]),
|
||||
"motion_state": (
|
||||
"moving" if task_state in MOTION_ACTIVE_TASK_STATES else "stopped"
|
||||
),
|
||||
"fault": task_state == 7 or result in MOTION_FAULT_RESULTS,
|
||||
"task_state": task_state,
|
||||
"motor_result": result,
|
||||
"motor_selector": motor,
|
||||
"value_x100": int(values["Value_X100"]),
|
||||
"feedback_frame": frame.public(),
|
||||
}
|
||||
|
||||
def _record_thermal_state(
|
||||
self,
|
||||
state: dict[str, Any],
|
||||
values: dict[str, int],
|
||||
frame: CanFrame,
|
||||
) -> None:
|
||||
for feature, signal in (
|
||||
("ventilation", "Driver_Seat_Ventilation_Status"),
|
||||
("heating", "Driver_Seat_Heating_Status"),
|
||||
):
|
||||
raw = int(values[signal])
|
||||
if 1 <= raw <= 4:
|
||||
self._set_thermal_feature(state, feature, raw)
|
||||
state[feature]["feedback_frame"] = frame.public()
|
||||
|
||||
@staticmethod
|
||||
def _set_thermal_feature(
|
||||
state: dict[str, Any], feature: str, raw: int
|
||||
) -> None:
|
||||
state[feature] = {
|
||||
"state": "on" if raw >= 2 else "off",
|
||||
"level": max(0, min(3, raw - 1)),
|
||||
"feedback_value": raw,
|
||||
}
|
||||
|
||||
@staticmethod
|
||||
def _physical_state_complete(state: dict[str, Any]) -> bool:
|
||||
return (
|
||||
set(state.get("position", {})) == set(PHYSICAL_MOTOR_AXES.values())
|
||||
and isinstance(state.get("ventilation"), dict)
|
||||
and isinstance(state.get("heating"), dict)
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _physical_state_missing(state: dict[str, Any]) -> list[str]:
|
||||
missing = [
|
||||
axis
|
||||
for axis in PHYSICAL_MOTOR_AXES.values()
|
||||
if axis not in state.get("position", {})
|
||||
]
|
||||
missing.extend(
|
||||
feature
|
||||
for feature in ("ventilation", "heating")
|
||||
if not isinstance(state.get(feature), dict)
|
||||
)
|
||||
return missing
|
||||
|
||||
@staticmethod
|
||||
def _first_unreported_operation(
|
||||
operations: dict[str, dict[str, Any]], results: list[dict[str, Any]]
|
||||
@@ -232,27 +431,102 @@ class HardwareSeatExecutor:
|
||||
raise HardwareExecutionError(
|
||||
f"timed out after {timeout_ms} ms waiting for CAN 0x{arbitration_id:03X}"
|
||||
)
|
||||
self._observe_auxiliary_frame(frame)
|
||||
if frame.arbitration_id != arbitration_id or len(frame.data) < 8:
|
||||
continue
|
||||
values = self.matrix.decode_by_frame_id(arbitration_id, frame.data)
|
||||
if predicate(values, frame):
|
||||
return values, frame
|
||||
|
||||
def _observe_auxiliary_frame(self, frame: CanFrame) -> None:
|
||||
"""Retain current telemetry that would otherwise be skipped while waiting.
|
||||
|
||||
Motor 0x304 is cyclic and interleaved with the 0x301/0x302/0x303
|
||||
protocol frames. A STALL decision requires sustained high current and
|
||||
low Hall activity, but after STOP_ALL the public current has already
|
||||
fallen back to its idle baseline. Keeping the samples observed during
|
||||
the command preserves the evidence needed to distinguish a real loaded
|
||||
stall from a stale/incorrect event without changing motor behaviour.
|
||||
"""
|
||||
|
||||
if frame.arbitration_id != 0x304 or len(frame.data) < 8:
|
||||
return
|
||||
values = self.matrix.decode_by_frame_id(0x304, frame.data)
|
||||
motor = int(values["Motor_Sub_ID"])
|
||||
if motor not in PHYSICAL_MOTOR_AXES:
|
||||
return
|
||||
raw = int(values["Current_Raw"])
|
||||
baseline = int(values["Current_Baseline_Raw"])
|
||||
sample = {
|
||||
"current_raw": raw,
|
||||
"current_baseline_raw": baseline,
|
||||
"current_delta_raw": max(0, raw - baseline),
|
||||
"stall_current_delta_threshold_raw": int(
|
||||
values["Stall_Current_Delta_Threshold_Raw"]
|
||||
),
|
||||
"current_sample_valid": bool(values["Current_Sample_Valid"]),
|
||||
"current_baseline_valid": bool(values["Current_Baseline_Valid"]),
|
||||
"high_current": bool(values["High_Current"]),
|
||||
"hall_activity_low": bool(values["Hall_Activity_Low"]),
|
||||
"current_calibrating": bool(values["Current_Calibrating"]),
|
||||
"frame": frame.public(),
|
||||
}
|
||||
samples = self._current_samples.setdefault(motor, [])
|
||||
samples.append(sample)
|
||||
# A normal position command is at most tens of seconds. Bound the
|
||||
# retained diagnostics anyway so repeated state reads cannot grow it.
|
||||
if len(samples) > 512:
|
||||
del samples[:-512]
|
||||
|
||||
def _reset_current_samples(self, motor: int) -> None:
|
||||
self._current_samples[motor] = []
|
||||
|
||||
def _current_diagnostic_summary(self, motor: int) -> dict[str, Any]:
|
||||
samples = self._current_samples.get(motor, [])
|
||||
if not samples:
|
||||
return {
|
||||
"sample_count": 0,
|
||||
"high_current_seen": False,
|
||||
"low_hall_seen": False,
|
||||
"stall_overlap_seen": False,
|
||||
"latest": None,
|
||||
}
|
||||
valid = [item for item in samples if item["current_sample_valid"]]
|
||||
candidates = valid or samples
|
||||
peak = max(candidates, key=lambda item: item["current_delta_raw"])
|
||||
return {
|
||||
"sample_count": len(samples),
|
||||
"valid_sample_count": len(valid),
|
||||
"max_current_raw": max(item["current_raw"] for item in candidates),
|
||||
"max_current_delta_raw": peak["current_delta_raw"],
|
||||
"high_current_seen": any(item["high_current"] for item in candidates),
|
||||
"low_hall_seen": any(item["hall_activity_low"] for item in candidates),
|
||||
"stall_overlap_seen": any(
|
||||
item["high_current"] and item["hall_activity_low"]
|
||||
for item in candidates
|
||||
),
|
||||
"peak": peak,
|
||||
"latest": samples[-1],
|
||||
}
|
||||
|
||||
def _wait_ack(
|
||||
self, bus: CanBus, sequence: int, opcode: int
|
||||
self, bus: CanBus, sequence: int, opcode: int, *, expected_ack: int = 1
|
||||
) -> tuple[dict[str, int], CanFrame]:
|
||||
def matches(values: dict[str, int], _: CanFrame) -> bool:
|
||||
if values["Last_Sequence"] != sequence or values["Last_Opcode"] != opcode:
|
||||
return False
|
||||
if values["Ack"] != 1:
|
||||
if values["Ack"] != expected_ack:
|
||||
raise HardwareExecutionError(
|
||||
"T2G movement command rejected: "
|
||||
f"ack={values['Ack']}, reason={values['Reject_Reason']}, "
|
||||
f"safety_fault={values['Safety_Fault']}"
|
||||
f"system_state={values['System_State']}, "
|
||||
f"fault_summary={values['Fault_Summary']}"
|
||||
)
|
||||
if values["Safety_Inhibit"] or values["Safety_Fault"]:
|
||||
if values["Safety_Inhibit"] or values["Fault_Summary"]:
|
||||
raise HardwareExecutionError(
|
||||
f"T2G movement safety inhibit/fault: {values['Safety_Fault']}"
|
||||
"T2G movement safety inhibit/fault: "
|
||||
f"state={values['System_State']}, "
|
||||
f"faulted_mask=0x{values['Faulted_Mask']:X}"
|
||||
)
|
||||
return True
|
||||
|
||||
@@ -262,7 +536,7 @@ class HardwareSeatExecutor:
|
||||
sequence = self._next_sequence()
|
||||
signals = {
|
||||
"Opcode": 9,
|
||||
"Motor_Selector": 3,
|
||||
"Motor_Selector": self.stop_all_motor_selector,
|
||||
"Direction": 0,
|
||||
"Protocol_Version": 1,
|
||||
"Sequence": sequence,
|
||||
@@ -272,7 +546,7 @@ class HardwareSeatExecutor:
|
||||
sent = self._send_encoded(bus, "T2G_Seat_Move_Command_300", signals)
|
||||
if not require_ack:
|
||||
return sent, None
|
||||
_, ack = self._wait_ack(bus, sequence, 9)
|
||||
_, ack = self._wait_ack(bus, sequence, 9, expected_ack=4)
|
||||
return sent, ack
|
||||
|
||||
def _best_effort_stop(self, bus: CanBus) -> None:
|
||||
@@ -368,38 +642,61 @@ class HardwareSeatExecutor:
|
||||
bus, command, operations, signals, motor
|
||||
)
|
||||
|
||||
target = int(command["completion"]["value"])
|
||||
tolerance = int(command["completion"].get("tolerance", 0))
|
||||
axis_maximum = self.axis_maximum_x100.get(operation["capability"])
|
||||
preflight_values, preflight_frame = self._motor_status_snapshot(bus, motor)
|
||||
baseline: dict[str, int] | None = None
|
||||
baseline_frame: CanFrame | None = None
|
||||
baseline_error: str | None = None
|
||||
position_recovery: dict[str, Any] | None = None
|
||||
if not preflight_values["Position_Valid"]:
|
||||
raise HardwareExecutionError(
|
||||
f"T2G motor {motor} absolute-position command blocked before send: "
|
||||
"controller Position_Valid=0. Run the explicit endpoint-calibration "
|
||||
"maintenance procedure with the bench clear and emergency stop ready, "
|
||||
"or use a small relative movement request.",
|
||||
telemetry={
|
||||
"motor_selector": motor,
|
||||
"position_valid": False,
|
||||
"calibration_required": True,
|
||||
"preflight_frame": preflight_frame.public(),
|
||||
},
|
||||
)
|
||||
if not self.auto_home:
|
||||
raise HardwareExecutionError(
|
||||
f"T2G motor {motor} absolute-position command blocked before send: "
|
||||
"controller Position_Valid=0. Enable the separately gated known-range "
|
||||
"homing workflow only with the bench clear and emergency stop ready.",
|
||||
telemetry={
|
||||
"motor_selector": motor,
|
||||
"position_valid": False,
|
||||
"known_range_home_required": True,
|
||||
"preflight_frame": preflight_frame.public(),
|
||||
},
|
||||
)
|
||||
position_recovery = self._home_known_range(bus, motor)
|
||||
preflight_values, preflight_frame = self._motor_status_snapshot(bus, motor)
|
||||
if not preflight_values["Position_Valid"]:
|
||||
raise HardwareExecutionError(
|
||||
f"T2G motor {motor} remained position-invalid after known-range homing",
|
||||
telemetry={
|
||||
"motor_selector": motor,
|
||||
"position_recovery": position_recovery,
|
||||
"preflight_frame": preflight_frame.public(),
|
||||
},
|
||||
)
|
||||
|
||||
# Capture Hall telemetry after any required homing. It is required
|
||||
# for endpoint recovery at 0% and makes non-zero failures diagnosable.
|
||||
if opcode == 1:
|
||||
try:
|
||||
baseline, baseline_frame = self._motor_diagnostic_snapshot(bus, motor)
|
||||
except HardwareExecutionError as exc:
|
||||
baseline_error = str(exc)
|
||||
|
||||
sequence = self._next_sequence()
|
||||
signals["Sequence"] = sequence
|
||||
self._reset_current_samples(motor)
|
||||
command_frame = self._send_encoded(bus, command["message"], signals)
|
||||
ack_values, ack_frame = self._wait_ack(bus, sequence, opcode)
|
||||
target = int(command["completion"]["value"])
|
||||
tolerance = int(command["completion"].get("tolerance", 0))
|
||||
|
||||
def complete(values: dict[str, int], _: CanFrame) -> bool:
|
||||
if values["Motor_Sub_ID"] != motor or values["Motor_Sequence"] != sequence:
|
||||
return False
|
||||
task_state = values["Task_State"]
|
||||
result = values["Result"]
|
||||
if task_state == 9 or result not in {1, 2}:
|
||||
raise HardwareExecutionError(
|
||||
f"T2G motor {motor} failed: task_state={task_state}, result={result}"
|
||||
)
|
||||
if task_state != 8 or result != 2:
|
||||
if task_state == 7 or result not in {1, 2}:
|
||||
return True
|
||||
if task_state != 6 or result != 2:
|
||||
return False
|
||||
if not values["Position_Valid"]:
|
||||
raise HardwareExecutionError(
|
||||
@@ -418,8 +715,151 @@ class HardwareSeatExecutor:
|
||||
int(command["completion"]["timeout_ms"]),
|
||||
complete,
|
||||
)
|
||||
task_state = int(values["Task_State"])
|
||||
result = int(values["Result"])
|
||||
far_endpoint_tolerance_acceptance = bool(
|
||||
opcode == 1
|
||||
and axis_maximum is not None
|
||||
and target == axis_maximum
|
||||
and task_state == 7
|
||||
and result in {5, 6}
|
||||
and values["Position_Valid"]
|
||||
and abs(int(values["Value_X100"]) - target) <= tolerance
|
||||
)
|
||||
terminal_tolerance_acceptance: dict[str, Any] | None = None
|
||||
if task_state != 6 or result != 2:
|
||||
if far_endpoint_tolerance_acceptance:
|
||||
terminal_tolerance_acceptance = {
|
||||
"mode": "verified_far_endpoint_within_target_tolerance",
|
||||
"reported_motor_result": MOTOR_RESULT_NAMES.get(
|
||||
result, f"UNKNOWN_{result}"
|
||||
),
|
||||
"effective_value_x100": int(values["Value_X100"]),
|
||||
"requested_value_x100": target,
|
||||
"position_tolerance_x100": tolerance,
|
||||
}
|
||||
else:
|
||||
diagnostic: dict[str, int] | None = None
|
||||
diagnostic_frame: CanFrame | None = None
|
||||
diagnostic_error: str | None = None
|
||||
try:
|
||||
diagnostic, diagnostic_frame = self._motor_diagnostic_snapshot(
|
||||
bus, motor
|
||||
)
|
||||
except HardwareExecutionError as exc:
|
||||
diagnostic_error = str(exc)
|
||||
|
||||
hall_before = int(baseline["Hall_Count"]) if baseline else None
|
||||
hall_after = int(diagnostic["Hall_Count"]) if diagnostic else None
|
||||
hall_delta = (
|
||||
abs(hall_after - hall_before)
|
||||
if hall_before is not None and hall_after is not None
|
||||
else None
|
||||
)
|
||||
calibrated_max_hall = int(self.motor_max_hall.get(motor, 0))
|
||||
endpoint_recovery_max_near_hall = ENDPOINT_RECOVERY_MAX_NEAR_HALL
|
||||
if calibrated_max_hall > 0:
|
||||
endpoint_recovery_max_near_hall = max(
|
||||
ENDPOINT_RECOVERY_MAX_NEAR_HALL,
|
||||
(
|
||||
calibrated_max_hall
|
||||
* ENDPOINT_RECOVERY_MAX_NEAR_HALL_PERCENT
|
||||
+ 50
|
||||
)
|
||||
// 100,
|
||||
)
|
||||
endpoint_drift = bool(
|
||||
opcode == 1
|
||||
and target == 0
|
||||
and result in {5, 6}
|
||||
and values["Position_Valid"]
|
||||
and hall_after is not None
|
||||
and 0 <= hall_after <= endpoint_recovery_max_near_hall
|
||||
)
|
||||
if not endpoint_drift:
|
||||
result_name = MOTOR_RESULT_NAMES.get(result, f"UNKNOWN_{result}")
|
||||
raise HardwareExecutionError(
|
||||
f"T2G motor {motor} {result_name}: task_state={task_state}, "
|
||||
f"Hall={hall_before}->{hall_after}",
|
||||
telemetry={
|
||||
"motor_selector": motor,
|
||||
"motion_axis": MOTION_CAPABILITY_TO_AXIS[
|
||||
operation["capability"]
|
||||
],
|
||||
"task_state": task_state,
|
||||
"motor_result": result,
|
||||
"motor_result_name": result_name,
|
||||
"position_valid": bool(values["Position_Valid"]),
|
||||
"hall_before": hall_before,
|
||||
"hall_after": hall_after,
|
||||
"hall_delta": hall_delta,
|
||||
"endpoint_recovery_max_near_hall": (
|
||||
endpoint_recovery_max_near_hall
|
||||
),
|
||||
"command_frame": command_frame.public(),
|
||||
"ack_frame": ack_frame.public(),
|
||||
"feedback_frame": feedback.public(),
|
||||
"diagnostic_before_frame": (
|
||||
baseline_frame.public() if baseline_frame else None
|
||||
),
|
||||
"diagnostic_frame": (
|
||||
diagnostic_frame.public() if diagnostic_frame else None
|
||||
),
|
||||
"diagnostic_read_error": diagnostic_error or baseline_error,
|
||||
"current_diagnostic": self._current_diagnostic_summary(motor),
|
||||
},
|
||||
)
|
||||
|
||||
try:
|
||||
away_probe = self._probe_away_from_near_endpoint(
|
||||
bus,
|
||||
motor,
|
||||
diagnostic,
|
||||
diagnostic_frame,
|
||||
)
|
||||
except HardwareExecutionError as exc:
|
||||
raise HardwareExecutionError(
|
||||
f"T2G motor {motor} endpoint-drift recovery refused: {exc}",
|
||||
telemetry={
|
||||
"motor_selector": motor,
|
||||
"motion_axis": MOTION_CAPABILITY_TO_AXIS[
|
||||
operation["capability"]
|
||||
],
|
||||
"failure_category": "endpoint_recovery_probe_failed",
|
||||
"failed_absolute_command_frame": command_frame.public(),
|
||||
"failed_absolute_feedback_frame": feedback.public(),
|
||||
**exc.telemetry,
|
||||
},
|
||||
) from exc
|
||||
home = self._home_known_range(bus, motor)
|
||||
recovered_values, recovered_frame = self._motor_status_snapshot(bus, motor)
|
||||
if (
|
||||
not recovered_values["Position_Valid"]
|
||||
or recovered_values["Value_X100"] > tolerance
|
||||
):
|
||||
raise HardwareExecutionError(
|
||||
f"T2G motor {motor} endpoint re-home did not establish zero",
|
||||
telemetry={
|
||||
"motor_selector": motor,
|
||||
"away_probe": away_probe,
|
||||
"home": home,
|
||||
"recovered_status_frame": recovered_frame.public(),
|
||||
},
|
||||
)
|
||||
position_recovery = {
|
||||
"mode": "verified_endpoint_drift_rehome",
|
||||
"failed_absolute_hall_before": hall_before,
|
||||
"failed_absolute_hall_after": hall_after,
|
||||
"endpoint_recovery_max_near_hall": (
|
||||
endpoint_recovery_max_near_hall
|
||||
),
|
||||
"away_probe": away_probe,
|
||||
"home": home,
|
||||
}
|
||||
values = recovered_values
|
||||
feedback = recovered_frame
|
||||
axis = MOTION_CAPABILITY_TO_AXIS[operation["capability"]]
|
||||
maximum = self.axis_maximum_x100.get(operation["capability"])
|
||||
maximum = axis_maximum
|
||||
if maximum is None:
|
||||
requested_pct = int(operation["parameters"]["position_pct"])
|
||||
maximum = max(1, target * 100 // max(1, requested_pct))
|
||||
@@ -439,6 +879,8 @@ class HardwareSeatExecutor:
|
||||
"position_valid": bool(values["Position_Valid"]),
|
||||
"system_ack": ack_values["Ack"],
|
||||
"preflight_frame": preflight_frame.public(),
|
||||
"position_recovery": position_recovery,
|
||||
"terminal_tolerance_acceptance": terminal_tolerance_acceptance,
|
||||
"command_frame": command_frame.public(),
|
||||
"ack_frame": ack_frame.public(),
|
||||
"feedback_frame": feedback.public(),
|
||||
@@ -462,6 +904,83 @@ class HardwareSeatExecutor:
|
||||
for item in operations
|
||||
]
|
||||
|
||||
def _probe_away_from_near_endpoint(
|
||||
self,
|
||||
bus: CanBus,
|
||||
motor: int,
|
||||
baseline: dict[str, int] | None,
|
||||
baseline_frame: CanFrame | None,
|
||||
) -> dict[str, Any]:
|
||||
if baseline is None or baseline_frame is None:
|
||||
raise HardwareExecutionError(
|
||||
"endpoint recovery requires a physical Hall baseline"
|
||||
)
|
||||
sequence = self._next_sequence()
|
||||
command = self._send_encoded(
|
||||
bus,
|
||||
"T2G_Seat_Move_Command_300",
|
||||
{
|
||||
"Opcode": 3,
|
||||
"Motor_Selector": motor,
|
||||
"Direction": 1,
|
||||
"Protocol_Version": 1,
|
||||
"Sequence": sequence,
|
||||
"Param_A": ENDPOINT_RECOVERY_PROBE_DURATION_MS,
|
||||
"Param_B": ENDPOINT_RECOVERY_PROBE_LEVEL_X10,
|
||||
},
|
||||
)
|
||||
ack_values, ack_frame = self._wait_ack(bus, sequence, 3)
|
||||
|
||||
def terminal(values: dict[str, int], _: CanFrame) -> bool:
|
||||
return bool(
|
||||
values["Motor_Sub_ID"] == motor
|
||||
and values["Motor_Sequence"] == sequence
|
||||
and (
|
||||
values["Task_State"] in {6, 7}
|
||||
or values["Result"] not in {1, 2}
|
||||
)
|
||||
)
|
||||
|
||||
values, feedback = self._receive_until(bus, 0x302, 5_000, terminal)
|
||||
after, after_frame = self._motor_diagnostic_snapshot(bus, motor)
|
||||
hall_before = int(baseline["Hall_Count"])
|
||||
hall_after = int(after["Hall_Count"])
|
||||
hall_delta = hall_after - hall_before
|
||||
if (
|
||||
values["Task_State"] != 6
|
||||
or values["Result"] != 2
|
||||
or hall_delta <= ENDPOINT_RECOVERY_MAX_STATIONARY_HALL_DELTA
|
||||
):
|
||||
raise HardwareExecutionError(
|
||||
"opposite-direction probe did not prove free Hall movement",
|
||||
telemetry={
|
||||
"probe_task_state": int(values["Task_State"]),
|
||||
"probe_motor_result": int(values["Result"]),
|
||||
"probe_hall_before": hall_before,
|
||||
"probe_hall_after": hall_after,
|
||||
"probe_hall_delta": hall_delta,
|
||||
"probe_command_frame": command.public(),
|
||||
"probe_ack_frame": ack_frame.public(),
|
||||
"probe_feedback_frame": feedback.public(),
|
||||
"probe_diagnostic_before_frame": baseline_frame.public(),
|
||||
"probe_diagnostic_frame": after_frame.public(),
|
||||
},
|
||||
)
|
||||
return {
|
||||
"sequence": sequence,
|
||||
"duration_ms": ENDPOINT_RECOVERY_PROBE_DURATION_MS,
|
||||
"duty_percent": ENDPOINT_RECOVERY_PROBE_LEVEL_X10 // 10,
|
||||
"hall_before": hall_before,
|
||||
"hall_after": hall_after,
|
||||
"hall_delta": hall_delta,
|
||||
"system_ack": ack_values["Ack"],
|
||||
"command_frame": command.public(),
|
||||
"ack_frame": ack_frame.public(),
|
||||
"feedback_frame": feedback.public(),
|
||||
"diagnostic_before_frame": baseline_frame.public(),
|
||||
"diagnostic_frame": after_frame.public(),
|
||||
}
|
||||
|
||||
def _motor_status_snapshot(
|
||||
self, bus: CanBus, motor: int
|
||||
) -> tuple[dict[str, int], CanFrame]:
|
||||
@@ -482,6 +1001,86 @@ class HardwareSeatExecutor:
|
||||
lambda values, _: values["Motor_Sub_ID"] == motor,
|
||||
)
|
||||
|
||||
def _home_known_range(self, bus: CanBus, motor: int) -> dict[str, Any]:
|
||||
"""Home one invalid axis to its verified near endpoint without re-ranging."""
|
||||
|
||||
before, before_frame = self._motor_diagnostic_snapshot(bus, motor)
|
||||
expected_max = self.motor_max_hall.get(motor)
|
||||
if expected_max is None:
|
||||
raise HardwareExecutionError(
|
||||
f"T2G motor {motor} has no repository MaxHall calibration"
|
||||
)
|
||||
observed_max = int(before["Max_Hall_Count"])
|
||||
max_tolerance = max(4, round(expected_max * 0.05))
|
||||
if abs(observed_max - expected_max) > max_tolerance:
|
||||
raise HardwareExecutionError(
|
||||
f"T2G motor {motor} MaxHall mismatch blocks known-range homing: "
|
||||
f"observed={observed_max}, expected={expected_max}±{max_tolerance}"
|
||||
)
|
||||
|
||||
sequence = self._next_sequence()
|
||||
command = self._send_encoded(
|
||||
bus,
|
||||
"T2G_Seat_Move_Command_300",
|
||||
{
|
||||
"Opcode": 0x0B,
|
||||
"Motor_Selector": motor,
|
||||
"Direction": 0,
|
||||
"Protocol_Version": 1,
|
||||
"Sequence": sequence,
|
||||
"Param_A": 0x4B52,
|
||||
"Param_B": self.home_timeout_s,
|
||||
},
|
||||
)
|
||||
ack_values, ack_frame = self._wait_ack(bus, sequence, 0x0B)
|
||||
|
||||
def complete(values: dict[str, int], _: CanFrame) -> bool:
|
||||
if values["Motor_Sub_ID"] != motor or values["Motor_Sequence"] != sequence:
|
||||
return False
|
||||
if values["Task_State"] == 7 or values["Result"] not in {1, 2}:
|
||||
raise HardwareExecutionError(
|
||||
f"T2G motor {motor} known-range homing failed: "
|
||||
f"task_state={values['Task_State']}, result={values['Result']}"
|
||||
)
|
||||
return values["Task_State"] == 6 and values["Result"] == 2
|
||||
|
||||
status, status_frame = self._receive_until(
|
||||
bus,
|
||||
0x302,
|
||||
(self.home_timeout_s + 15) * 1000,
|
||||
complete,
|
||||
)
|
||||
after, after_frame = self._motor_diagnostic_snapshot(bus, motor)
|
||||
after_max = int(after["Max_Hall_Count"])
|
||||
if (
|
||||
not status["Position_Valid"]
|
||||
or not status["Near_Endpoint"]
|
||||
or int(status["Value_X100"]) > 100
|
||||
or int(after["Hall_Count"]) > 2
|
||||
or abs(after_max - expected_max) > max_tolerance
|
||||
):
|
||||
raise HardwareExecutionError(
|
||||
f"T2G motor {motor} known-range homing feedback was inconsistent",
|
||||
telemetry={
|
||||
"home_status_frame": status_frame.public(),
|
||||
"home_diagnostic_frame": after_frame.public(),
|
||||
},
|
||||
)
|
||||
return {
|
||||
"mode": "home_known_range",
|
||||
"motor_selector": motor,
|
||||
"sequence": sequence,
|
||||
"expected_max_hall": expected_max,
|
||||
"hall_before": int(before["Hall_Count"]),
|
||||
"hall_after": int(after["Hall_Count"]),
|
||||
"command_frame": command.public(),
|
||||
"ack_frame": ack_frame.public(),
|
||||
"status_frame": status_frame.public(),
|
||||
"diagnostic_before_frame": before_frame.public(),
|
||||
"diagnostic_after_frame": after_frame.public(),
|
||||
"system_ack": ack_values["Ack"],
|
||||
}
|
||||
|
||||
def _execute_timed_motion(
|
||||
self,
|
||||
bus: CanBus,
|
||||
@@ -493,6 +1092,7 @@ class HardwareSeatExecutor:
|
||||
baseline, baseline_frame = self._motor_diagnostic_snapshot(bus, motor)
|
||||
sequence = self._next_sequence()
|
||||
signals["Sequence"] = sequence
|
||||
self._reset_current_samples(motor)
|
||||
command_frame = self._send_encoded(bus, command["message"], signals)
|
||||
ack_values, ack_frame = self._wait_ack(bus, sequence, 3)
|
||||
|
||||
@@ -501,7 +1101,7 @@ class HardwareSeatExecutor:
|
||||
return False
|
||||
task_state = values["Task_State"]
|
||||
result = values["Result"]
|
||||
return task_state in {8, 9} or result not in {1, 2}
|
||||
return task_state in {6, 7} or result not in {1, 2}
|
||||
|
||||
values, feedback = self._receive_until(
|
||||
bus,
|
||||
@@ -543,8 +1143,8 @@ class HardwareSeatExecutor:
|
||||
else int(baseline["Max_Hall_Count"]),
|
||||
)
|
||||
|
||||
if task_state != 8 or result != 2:
|
||||
if result == 4 and hall_delta <= 2:
|
||||
if task_state != 6 or result != 2:
|
||||
if result == 5 and hall_delta <= 2:
|
||||
failure_category = "stall_no_hall_activity"
|
||||
diagnostic_hint = (
|
||||
"No sustained Hall activity was observed. The commanded direction may "
|
||||
@@ -553,13 +1153,13 @@ class HardwareSeatExecutor:
|
||||
"small command in the opposite direction to distinguish endpoint from "
|
||||
"a Hall wiring fault; do not run automatic full-range calibration."
|
||||
)
|
||||
elif result == 4:
|
||||
elif result == 5:
|
||||
failure_category = "stall_after_hall_activity"
|
||||
diagnostic_hint = (
|
||||
"Hall activity was observed before the stall; inspect the mechanism for "
|
||||
"an endpoint or obstruction in the commanded direction."
|
||||
)
|
||||
elif result == 5 or last_event in {2, 3}:
|
||||
elif result == 4 or last_event in {2, 3}:
|
||||
failure_category = "endpoint"
|
||||
diagnostic_hint = (
|
||||
"The controller reported an endpoint. Verify the requested direction "
|
||||
@@ -606,6 +1206,7 @@ class HardwareSeatExecutor:
|
||||
else None
|
||||
),
|
||||
"diagnostic_read_error": diagnostic_error,
|
||||
"current_diagnostic": self._current_diagnostic_summary(motor),
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
@@ -11,6 +11,11 @@ TARGET_PATTERN = re.compile(r"主副驾|前排|主驾|驾驶位|司机位|副驾
|
||||
FEATURE_PATTERN = re.compile(r"通风|加热")
|
||||
STATE_PATTERN = re.compile(r"关闭|关掉|停止|取消|打开|开启|启动|开")
|
||||
LEVEL_PATTERN = re.compile(r"([1-5一二三四五])\s*档")
|
||||
POSITION_PATTERN_TEMPLATE = (
|
||||
r"{alias}(?:位置)?(?:调到|移动到|移到|设为|设置为|调至|移动至|到|为)?"
|
||||
r"(?P<position>\d{{1,3}})(?:%|%)"
|
||||
)
|
||||
ZERO_POSITION_PATTERN = re.compile(r"归零|回零|复位|零位")
|
||||
VENTILATION_FEEDBACK_PATTERN = re.compile(r"(?<!不)(?<!加)热|(?<!不)闷|出汗")
|
||||
HEATING_FEEDBACK_PATTERN = re.compile(r"(?<!不)冷|(?<!不)凉")
|
||||
MILD_PATTERN = re.compile(r"有点|一点|稍微|微微")
|
||||
@@ -18,6 +23,7 @@ STRONG_PATTERN = re.compile(r"很|太|非常|特别|难受")
|
||||
AIRFLOW_CAUSE_PATTERN = re.compile(r"吹|风太大|风吹")
|
||||
FIRST_PERSON_PATTERN = re.compile(r"我|自己")
|
||||
VEHICLE_SCOPE_PATTERN = re.compile(r"车上|车内|车里|全车|整车|整个车")
|
||||
PASSENGER_ONLY_QUESTION = "当前所有座椅能力仅控制副驾,不支持主驾座椅目标。"
|
||||
|
||||
TARGETS = {
|
||||
"主驾": ["driver"],
|
||||
@@ -40,6 +46,12 @@ STATES = {
|
||||
"取消": "off",
|
||||
}
|
||||
LEVELS = {"一": 1, "二": 2, "三": 3, "四": 4, "五": 5}
|
||||
MOTION_AXIS_ALIASES = {
|
||||
"longitudinal": ("水平", "前后", "纵向"),
|
||||
"cushion_tilt": ("坐盆", "坐垫"),
|
||||
"backrest_recline": ("靠背",),
|
||||
"legrest_height": ("腿托", "腿部支撑"),
|
||||
}
|
||||
|
||||
|
||||
def _nearest_preceding_target(clause: str, position: int) -> str | None:
|
||||
@@ -107,8 +119,15 @@ def _feedback_targets(
|
||||
if last_target in {"driver", "passenger"}:
|
||||
return [last_target], "history"
|
||||
if FIRST_PERSON_PATTERN.search(text):
|
||||
return ["driver"], "speaker_default"
|
||||
return [], "missing"
|
||||
return ["passenger"], "passenger_default"
|
||||
return ["passenger"], "passenger_default"
|
||||
|
||||
|
||||
def _contains_unsupported_driver_target(text: str) -> bool:
|
||||
return any(
|
||||
"driver" in TARGETS[match.group(0)]
|
||||
for match in TARGET_PATTERN.finditer(text)
|
||||
)
|
||||
|
||||
|
||||
def _inferred_level(text: str) -> tuple[int, str]:
|
||||
@@ -162,6 +181,17 @@ def resolve_contextual_intent(
|
||||
if not ventilation_cues and not cold_cues:
|
||||
return result
|
||||
|
||||
if _contains_unsupported_driver_target(text):
|
||||
result["actions"] = []
|
||||
result["ambiguities"] = [
|
||||
{"field": "target", "question": PASSENGER_ONLY_QUESTION}
|
||||
]
|
||||
result["decision_summary"] = [
|
||||
"识别到主驾或包含主驾的座椅范围;当前系统不会将其重定向到副驾。"
|
||||
]
|
||||
result["confidence"] = max(float(result.get("confidence", 0)), 0.99)
|
||||
return result
|
||||
|
||||
if VEHICLE_SCOPE_PATTERN.search(text) and not TARGET_PATTERN.search(text):
|
||||
result["actions"] = []
|
||||
result["ambiguities"] = [
|
||||
@@ -197,8 +227,8 @@ def resolve_contextual_intent(
|
||||
summaries: list[str] = []
|
||||
if target_source == "history":
|
||||
summaries.append(f"本轮省略了座位,沿用上一轮的{labels[targets[0]]}目标。")
|
||||
elif target_source == "speaker_default":
|
||||
summaries.append("当前为座椅单域驾驶员交互,将第一人称“我”解析为主驾。")
|
||||
elif target_source == "passenger_default":
|
||||
summaries.append("当前座椅域仅控制副驾,本轮省略座位并安全限定为副驾。")
|
||||
|
||||
planned_actions: list[dict[str, Any]] = []
|
||||
desired_level, intensity = _inferred_level(text)
|
||||
@@ -342,8 +372,31 @@ def parse_local_intent(
|
||||
text = re.sub(r"\s+", "", utterance.strip())
|
||||
clauses = [part for part in re.split(r"[,,。;;!!??]", text) if part]
|
||||
actions: list[dict[str, Any]] = []
|
||||
motion_actions: list[dict[str, Any]] = []
|
||||
ambiguities: list[dict[str, str]] = []
|
||||
|
||||
mentioned_axes = [
|
||||
axis
|
||||
for axis, aliases in MOTION_AXIS_ALIASES.items()
|
||||
if any(alias in text for alias in aliases)
|
||||
]
|
||||
if ZERO_POSITION_PATTERN.search(text) and (mentioned_axes or "四轴" in text):
|
||||
reset_axes = list(MOTION_AXIS_ALIASES) if "四轴" in text else mentioned_axes
|
||||
if _contains_unsupported_driver_target(text):
|
||||
ambiguities.append(
|
||||
{"field": "target", "question": PASSENGER_ONLY_QUESTION}
|
||||
)
|
||||
else:
|
||||
motion_actions.extend(
|
||||
{
|
||||
"target": "passenger",
|
||||
"action_type": "position",
|
||||
"axis": axis,
|
||||
"position_pct": 0,
|
||||
}
|
||||
for axis in reset_axes
|
||||
)
|
||||
|
||||
for clause in clauses:
|
||||
level = _level_for_clause(clause)
|
||||
for feature_match in FEATURE_PATTERN.finditer(clause):
|
||||
@@ -351,8 +404,10 @@ def parse_local_intent(
|
||||
target_zh = _nearest_preceding_target(clause, feature_match.start())
|
||||
state = _state_for_feature(clause, feature_match.end())
|
||||
if target_zh is None:
|
||||
target_zh = "副驾"
|
||||
if "driver" in TARGETS[target_zh]:
|
||||
ambiguities.append(
|
||||
{"field": "target", "question": f"“{feature_zh}”需要作用于主驾还是副驾?"}
|
||||
{"field": "target", "question": PASSENGER_ONLY_QUESTION}
|
||||
)
|
||||
continue
|
||||
if state is None:
|
||||
@@ -369,6 +424,40 @@ def parse_local_intent(
|
||||
"requested_level": level,
|
||||
}
|
||||
)
|
||||
if ZERO_POSITION_PATTERN.search(text):
|
||||
continue
|
||||
for axis, aliases in MOTION_AXIS_ALIASES.items():
|
||||
for alias in aliases:
|
||||
match = re.search(
|
||||
POSITION_PATTERN_TEMPLATE.format(alias=re.escape(alias)),
|
||||
clause,
|
||||
)
|
||||
if match is None:
|
||||
continue
|
||||
target_zh = _nearest_preceding_target(clause, match.start()) or "副驾"
|
||||
if "driver" in TARGETS[target_zh]:
|
||||
ambiguities.append(
|
||||
{"field": "target", "question": PASSENGER_ONLY_QUESTION}
|
||||
)
|
||||
break
|
||||
position_pct = int(match.group("position"))
|
||||
if position_pct > 100:
|
||||
ambiguities.append(
|
||||
{
|
||||
"field": f"passenger.position.{axis}",
|
||||
"question": f"{alias}位置百分比必须在0到100之间。",
|
||||
}
|
||||
)
|
||||
break
|
||||
motion_actions.append(
|
||||
{
|
||||
"target": "passenger",
|
||||
"action_type": "position",
|
||||
"axis": axis,
|
||||
"position_pct": position_pct,
|
||||
}
|
||||
)
|
||||
break
|
||||
|
||||
merged: dict[tuple[str, str], dict[str, Any]] = {}
|
||||
for action in actions:
|
||||
@@ -382,7 +471,19 @@ def parse_local_intent(
|
||||
}
|
||||
)
|
||||
merged[key] = action
|
||||
actions = list(merged.values())
|
||||
merged_motion: dict[tuple[str, str], dict[str, Any]] = {}
|
||||
for action in motion_actions:
|
||||
key = (action["target"], action["axis"])
|
||||
previous = merged_motion.get(key)
|
||||
if previous and previous["position_pct"] != action["position_pct"]:
|
||||
ambiguities.append(
|
||||
{
|
||||
"field": f"{key[0]}.position.{key[1]}",
|
||||
"question": "同一个座椅轴出现多个位置目标,请确认最终位置。",
|
||||
}
|
||||
)
|
||||
merged_motion[key] = action
|
||||
actions = [*merged.values(), *merged_motion.values()]
|
||||
|
||||
parsed = resolve_contextual_intent(
|
||||
{
|
||||
@@ -401,11 +502,11 @@ def parse_local_intent(
|
||||
ambiguities.append(
|
||||
{
|
||||
"field": "capability",
|
||||
"question": "当前仅支持主/副驾座椅通风或加热的打开与关闭。",
|
||||
"question": "当前仅支持副驾座椅通风、加热、移动和姿态预设能力。",
|
||||
}
|
||||
)
|
||||
|
||||
summary = ["识别为座椅热舒适单域请求。", *parsed["decision_summary"]]
|
||||
summary = ["识别为副驾座椅功能单域请求。", *parsed["decision_summary"]]
|
||||
labels = {
|
||||
"driver": "主驾",
|
||||
"passenger": "副驾",
|
||||
@@ -414,11 +515,24 @@ def parse_local_intent(
|
||||
"on": "打开",
|
||||
"off": "关闭",
|
||||
}
|
||||
summary.extend(
|
||||
f"目标:{labels[action['target']]}座椅{labels[action['function']]}{labels[action['state']]}。"
|
||||
for action in actions
|
||||
)
|
||||
if any(action["requested_level"] is not None for action in actions):
|
||||
axis_labels = {
|
||||
"longitudinal": "水平",
|
||||
"cushion_tilt": "坐盆",
|
||||
"backrest_recline": "靠背",
|
||||
"legrest_height": "腿托",
|
||||
}
|
||||
for action in actions:
|
||||
if action.get("action_type") == "position":
|
||||
summary.append(
|
||||
f"目标:副驾座椅{axis_labels[action['axis']]}位置"
|
||||
f"{action['position_pct']}%。"
|
||||
)
|
||||
else:
|
||||
summary.append(
|
||||
f"目标:{labels[action['target']]}座椅"
|
||||
f"{labels[action['function']]}{labels[action['state']]}。"
|
||||
)
|
||||
if any(action.get("requested_level") is not None for action in actions):
|
||||
summary.append("目标档位已写入功能配置;现有 CAN 矩阵仅支持档位虚拟回读。")
|
||||
|
||||
response = {
|
||||
|
||||
@@ -5,7 +5,7 @@ from __future__ import annotations
|
||||
from typing import Any
|
||||
|
||||
|
||||
SUPPORTED_TARGETS = {"driver", "passenger"}
|
||||
SUPPORTED_TARGETS = {"passenger"}
|
||||
SUPPORTED_FUNCTIONS = {"ventilation", "heating"}
|
||||
SUPPORTED_STATES = {"on", "off"}
|
||||
SUPPORTED_MOTION_AXES = {
|
||||
@@ -17,7 +17,7 @@ SUPPORTED_MOTION_AXES = {
|
||||
}
|
||||
ANGLE_RANGES = {
|
||||
"backrest_recline": (95, 163),
|
||||
"legrest_height": (95, 173),
|
||||
"legrest_height": (95, 167),
|
||||
}
|
||||
|
||||
|
||||
@@ -71,7 +71,6 @@ def validate_semantics(
|
||||
seen: dict[tuple[str, str], str] = {}
|
||||
active_functions: dict[str, set[str]] = {}
|
||||
has_motion = False
|
||||
has_driver_motion = False
|
||||
for index, action in enumerate(parsed.get("actions", [])):
|
||||
path = f"actions[{index}]"
|
||||
if action.get("target") not in SUPPORTED_TARGETS:
|
||||
@@ -80,12 +79,11 @@ def validate_semantics(
|
||||
"severity": "error",
|
||||
"code": "UNSUPPORTED_TARGET",
|
||||
"path": f"{path}.target",
|
||||
"message": "当前只支持主驾和前排副驾。",
|
||||
"message": "当前所有座椅能力仅支持前排副驾。",
|
||||
}
|
||||
)
|
||||
if action.get("action_type") == "position":
|
||||
has_motion = True
|
||||
has_driver_motion = has_driver_motion or action.get("target") == "driver"
|
||||
axis = action.get("axis")
|
||||
position = action.get("position_pct")
|
||||
if axis not in SUPPORTED_MOTION_AXES:
|
||||
@@ -206,16 +204,6 @@ def validate_semantics(
|
||||
"message": "座椅位置控制和反馈 CAN 矩阵完全为演示虚构,仅用于验证模型与配置链路。",
|
||||
}
|
||||
)
|
||||
if has_driver_motion:
|
||||
issues.append(
|
||||
{
|
||||
"severity": "warning",
|
||||
"code": "DRIVER_MOTION_INTERLOCK_NOT_IMPLEMENTED",
|
||||
"path": "platform.reference_vehicle.vehicle_state",
|
||||
"message": "未接入车速、挡位、乘员和防夹状态,主驾移动只能虚拟执行。",
|
||||
}
|
||||
)
|
||||
|
||||
for target, functions in active_functions.items():
|
||||
if functions == {"ventilation", "heating"}:
|
||||
issues.append(
|
||||
|
||||
Reference in New Issue
Block a user