feat: update CAN integration and T2G seat firmware

This commit is contained in:
vee
2026-08-01 15:55:21 +08:00
parent 8cda026ac2
commit 4aded5c21d
1443 changed files with 1007122 additions and 4978 deletions
-1
View File
@@ -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",
}
-4
View File
@@ -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"])
+646 -45
View File
@@ -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),
},
)
+127 -13
View File
@@ -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 = {
+3 -15
View File
@@ -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(