优化无线/有线CAN，以及控制指令解析
This commit is contained in:
minghanchen
@@ -9,6 +9,11 @@ from typing import Any
|
||||
|
||||
import yaml
|
||||
|
||||
from cognitive_agent.generated.can_semantic_protocol import (
|
||||
CAN0_SUPPORTED_CAPABILITIES,
|
||||
command_contract as shared_protocol_contract,
|
||||
)
|
||||
|
||||
|
||||
STATE_VALUES = {"on": 1, "off": 2}
|
||||
|
||||
@@ -16,7 +21,7 @@ STATE_VALUES = {"on": 1, "off": 2}
|
||||
class T2GSeatCompiler:
|
||||
"""Deterministic compiler for the HIL-verified T2G seat bench protocol."""
|
||||
|
||||
version = "2.1.0"
|
||||
version = "2.2.0"
|
||||
|
||||
def __init__(self, mapping_path: Path, calibration_path: Path):
|
||||
self.mapping = yaml.safe_load(mapping_path.read_text(encoding="utf-8"))
|
||||
@@ -29,6 +34,10 @@ class T2GSeatCompiler:
|
||||
for item in self.mapping["spec"]["mappings"]
|
||||
}
|
||||
|
||||
@staticmethod
|
||||
def _protocol_contract(rule_id: str, key: str) -> dict[str, Any]:
|
||||
return shared_protocol_contract(rule_id, key)
|
||||
|
||||
def compile(
|
||||
self,
|
||||
function_config: dict[str, Any],
|
||||
@@ -128,6 +137,9 @@ class T2GSeatCompiler:
|
||||
"operation_ref": operation_ref,
|
||||
"bus": "seat_can",
|
||||
"message": "T2G_Seat_Thermal_Mode_3CE",
|
||||
"protocol": self._protocol_contract(
|
||||
"can0.seat.thermal", "thermal_mode"
|
||||
),
|
||||
"signals": {"Driver_Seat_Thermal_Mode": 1},
|
||||
"send_offset_ms": 0,
|
||||
}
|
||||
@@ -138,6 +150,9 @@ class T2GSeatCompiler:
|
||||
"operation_ref": operation_ref,
|
||||
"bus": "seat_can",
|
||||
"message": "T2G_Seat_Thermal_Control_29E",
|
||||
"protocol": self._protocol_contract(
|
||||
"can0.seat.thermal", "thermal_control"
|
||||
),
|
||||
"signals": {
|
||||
"Driver_Seat_Heating_Gear": int(desired["heating"]["level"]),
|
||||
"Driver_Seat_Heating_On": STATE_VALUES[desired["heating"]["state"]],
|
||||
@@ -232,6 +247,15 @@ class T2GSeatCompiler:
|
||||
),
|
||||
"bus": "seat_can",
|
||||
"message": "T2G_Seat_Move_Command_300",
|
||||
"protocol": self._protocol_contract(
|
||||
(
|
||||
"can0.seat.relative"
|
||||
if (operation.get("motion") or {}).get("mode")
|
||||
in {"relative", "timed"}
|
||||
else "can0.seat.position"
|
||||
),
|
||||
"move",
|
||||
),
|
||||
"signals": {
|
||||
"Direction": 0,
|
||||
"Motor_Selector": int(axis["motor_selector"]),
|
||||
@@ -292,6 +316,9 @@ class T2GSeatCompiler:
|
||||
"operation_ref": operation["id"],
|
||||
"bus": "seat_can",
|
||||
"message": "T2G_Seat_Move_Command_300",
|
||||
"protocol": self._protocol_contract(
|
||||
"can0.seat.dynamic", "dynamic"
|
||||
),
|
||||
"signals": {
|
||||
"Direction": 0,
|
||||
"Motor_Selector": 0,
|
||||
@@ -357,6 +384,7 @@ class T2GSeatCompiler:
|
||||
),
|
||||
"bus": "seat_can",
|
||||
"message": "T2G_Seat_Move_Command_300",
|
||||
"protocol": self._protocol_contract("can0.seat.relative", "move"),
|
||||
"signals": {
|
||||
"Direction": direction,
|
||||
"Motor_Selector": int(axis["motor_selector"]),
|
||||
@@ -426,6 +454,7 @@ class T2GSeatCompiler:
|
||||
),
|
||||
"bus": "seat_can",
|
||||
"message": "T2G_Seat_Move_Command_300",
|
||||
"protocol": self._protocol_contract("can0.seat.relative", "move"),
|
||||
"signals": {
|
||||
"Direction": direction,
|
||||
"Motor_Selector": int(axis["motor_selector"]),
|
||||
@@ -469,6 +498,10 @@ class T2GSeatCompiler:
|
||||
|
||||
def _require_supported(self, operation: dict[str, Any]) -> None:
|
||||
key = (operation["capability"], operation["target"]["zone"])
|
||||
if key[0] not in CAN0_SUPPORTED_CAPABILITIES:
|
||||
raise ValueError(
|
||||
f"shared CAN0 protocol does not support capability={key[0]}"
|
||||
)
|
||||
if key not in self.supported:
|
||||
raise ValueError(
|
||||
"T2G hardware mapping does not support "
|
||||
|
||||
@@ -9,6 +9,9 @@ from typing import Any, Callable
|
||||
|
||||
from cognitive_agent.infrastructure.can.gateway import CanBus, CanFrame
|
||||
from cognitive_agent.infrastructure.firmware import T2GFirmwareGuard
|
||||
from cognitive_agent.generated.can_semantic_protocol import (
|
||||
command_contract as shared_protocol_contract,
|
||||
)
|
||||
|
||||
from .executor import MOTION_CAPABILITY_TO_AXIS
|
||||
from .matrix import SeatCanMatrix
|
||||
@@ -114,6 +117,7 @@ class HardwareSeatExecutor:
|
||||
synchronize_position_before_first_motion: bool = False,
|
||||
position_sync_state: dict[str, bool] | None = None,
|
||||
require_thermal_feedback: bool = True,
|
||||
feedback_required: bool = True,
|
||||
):
|
||||
self.matrix = matrix
|
||||
self.bus_factory = bus_factory
|
||||
@@ -136,7 +140,19 @@ class HardwareSeatExecutor:
|
||||
else {"synchronized": False}
|
||||
)
|
||||
self.require_thermal_feedback = bool(require_thermal_feedback)
|
||||
# A locally attached CAN adapter can synchronously enforce the complete
|
||||
# 0x301/0x302/0x306/0x4FA feedback contract. The wireless head-unit
|
||||
# relay currently publishes CAN0 state asynchronously, so its HTTP
|
||||
# execution path must stop after the approved frame has entered MQTT.
|
||||
# Keep this opt-out explicit and default-on so wired HIL retains its
|
||||
# strict physical verification behavior.
|
||||
self.feedback_required = bool(feedback_required)
|
||||
self._current_samples: dict[int, list[dict[str, Any]]] = {}
|
||||
# Evidence for the currently executing *requested* command. Safety
|
||||
# prerequisites and best-effort STOP frames are deliberately excluded,
|
||||
# so a feedback timeout can be reported as submitted-but-unverified
|
||||
# without mistaking a prerequisite frame for the requested operation.
|
||||
self._active_command_frame: CanFrame | None = None
|
||||
|
||||
@classmethod
|
||||
def _next_sequence(cls) -> int:
|
||||
@@ -145,6 +161,49 @@ class HardwareSeatExecutor:
|
||||
cls._sequence = 1
|
||||
return cls._sequence
|
||||
|
||||
@staticmethod
|
||||
def _operation_debug_command(operation: dict[str, Any]) -> str:
|
||||
"""Return the CAN0 semantic command attached to MQTT debug metadata."""
|
||||
|
||||
capability = str(operation["capability"])
|
||||
parameters = operation.get("parameters", {})
|
||||
axes = {
|
||||
"seat.set_longitudinal_position": "horizontal",
|
||||
"seat.set_cushion_tilt": "basin",
|
||||
"seat.set_backrest_recline": "backrest",
|
||||
"seat.set_legrest_height": "legrest",
|
||||
}
|
||||
if capability in axes:
|
||||
axis = axes[capability]
|
||||
motion = operation.get("motion", {})
|
||||
relative_directions = {
|
||||
"horizontal": {"increase": "backward", "decrease": "forward"},
|
||||
"basin": {"increase": "down", "decrease": "up"},
|
||||
"backrest": {"increase": "backward", "decrease": "forward"},
|
||||
"legrest": {"increase": "up", "decrease": "down"},
|
||||
}
|
||||
direction = relative_directions[axis].get(
|
||||
str(motion.get("direction", ""))
|
||||
)
|
||||
delta = motion.get("delta_position_pct")
|
||||
if motion.get("mode") == "relative" and direction and delta is not None:
|
||||
return f"seat passenger {axis} relative {direction} {int(delta)}"
|
||||
return f"seat passenger {axis} position {parameters['position_pct']}"
|
||||
if capability in {"seat.set_ventilation", "seat.set_heating"}:
|
||||
feature = "vent" if capability.endswith("ventilation") else "heat"
|
||||
if parameters.get("state") == "off":
|
||||
return f"seat passenger {feature} off"
|
||||
level = {1: "low", 2: "medium", 3: "high"}.get(
|
||||
int(parameters.get("level", 1)), "low"
|
||||
)
|
||||
return f"seat passenger {feature} {level}"
|
||||
if capability == "seat.start_dynamic_mode":
|
||||
mode = "rocking-horse" if int(parameters["mode_id"]) == 1 else "cradle"
|
||||
return f"seat passenger dynamic {mode}"
|
||||
if capability == "seat.stop_dynamic_mode":
|
||||
return "seat passenger dynamic stop"
|
||||
return capability
|
||||
|
||||
def execute(
|
||||
self,
|
||||
command_plan: dict[str, Any],
|
||||
@@ -169,6 +228,8 @@ class HardwareSeatExecutor:
|
||||
position_sync_deferred_to_commands = False
|
||||
position_sync_trigger = "process_first_motion"
|
||||
physical_state: dict[str, Any] | None = None
|
||||
motion_publication_start: int | None = None
|
||||
thermal_publication_start: int | None = None
|
||||
self._current_samples = {}
|
||||
|
||||
with self._adapter_lock:
|
||||
@@ -200,7 +261,22 @@ class HardwareSeatExecutor:
|
||||
else:
|
||||
bus = self.bus_factory()
|
||||
if has_motion:
|
||||
self._stop_all(bus, require_ack=True)
|
||||
motion_publication_start = len(
|
||||
getattr(bus, "published_messages", ())
|
||||
)
|
||||
set_debug_command = getattr(bus, "set_debug_command", None)
|
||||
if callable(set_debug_command):
|
||||
set_debug_command(
|
||||
"; ".join(
|
||||
self._operation_debug_command(operation)
|
||||
for operation in operations.values()
|
||||
)
|
||||
)
|
||||
# STOP_ALL is the firmware-defined sequence barrier. Keep
|
||||
# publishing it for a remote relay, but do not block the
|
||||
# requested command on a synchronous 0x301 that is delivered
|
||||
# later through the MQTT state stream.
|
||||
self._stop_all(bus, require_ack=self.feedback_required)
|
||||
motion_armed = True
|
||||
position_sync_required = (
|
||||
self.synchronize_position_before_first_motion
|
||||
@@ -231,14 +307,37 @@ class HardwareSeatExecutor:
|
||||
)
|
||||
previous_offset = 0
|
||||
for command in commands:
|
||||
self._active_command_frame = None
|
||||
offset = int(command.get("send_offset_ms", 0))
|
||||
if offset > previous_offset:
|
||||
time.sleep((offset - previous_offset) / 1000)
|
||||
previous_offset = offset
|
||||
operation_ids = command["operation_ref"].split(",")
|
||||
referenced = [operations[item] for item in operation_ids]
|
||||
publication_start = (
|
||||
thermal_publication_start
|
||||
if thermal_publication_start is not None
|
||||
else motion_publication_start
|
||||
if motion_publication_start is not None
|
||||
and command["message"] == "T2G_Seat_Move_Command_300"
|
||||
else len(getattr(bus, "published_messages", ()))
|
||||
)
|
||||
set_debug_command = getattr(bus, "set_debug_command", None)
|
||||
if callable(set_debug_command):
|
||||
set_debug_command(
|
||||
"; ".join(
|
||||
self._operation_debug_command(item)
|
||||
for item in referenced
|
||||
)
|
||||
)
|
||||
if command["message"] == "T2G_Seat_Thermal_Mode_3CE":
|
||||
frame = self._send_encoded(bus, command["message"], command["signals"])
|
||||
frame = self._send_encoded(
|
||||
bus,
|
||||
command["message"],
|
||||
command["signals"],
|
||||
self._resolved_protocol(command),
|
||||
)
|
||||
self._active_command_frame = frame
|
||||
results.append(
|
||||
{
|
||||
"ref": command["id"],
|
||||
@@ -252,19 +351,56 @@ class HardwareSeatExecutor:
|
||||
},
|
||||
}
|
||||
)
|
||||
thermal_publication_start = publication_start
|
||||
continue
|
||||
elif not self.feedback_required:
|
||||
command_results = self._submit_without_feedback(
|
||||
bus, command, referenced
|
||||
)
|
||||
elif command["message"] == "T2G_Seat_Thermal_Control_29E":
|
||||
results.extend(self._execute_thermal(bus, command, referenced))
|
||||
command_results = self._execute_thermal(
|
||||
bus, command, referenced
|
||||
)
|
||||
thermal_publication_start = None
|
||||
elif command["message"] == "T2G_Seat_Move_Command_300":
|
||||
if int(command["signals"]["Opcode"]) in {0x0E, 0x0F}:
|
||||
results.extend(
|
||||
self._execute_dynamic_mode(bus, command, referenced)
|
||||
command_results = self._execute_dynamic_mode(
|
||||
bus, command, referenced
|
||||
)
|
||||
else:
|
||||
results.extend(self._execute_motion(bus, command, referenced))
|
||||
command_results = self._execute_motion(
|
||||
bus, command, referenced
|
||||
)
|
||||
motion_publication_start = None
|
||||
else:
|
||||
raise HardwareExecutionError(
|
||||
f"hardware executor refuses unknown message {command['message']}"
|
||||
)
|
||||
mqtt_messages = list(
|
||||
getattr(bus, "published_messages", ())[publication_start:]
|
||||
)
|
||||
if mqtt_messages:
|
||||
for item in command_results:
|
||||
telemetry = item.setdefault("telemetry", {})
|
||||
telemetry["mqtt_messages"] = mqtt_messages
|
||||
if not self.feedback_required:
|
||||
delivery_status = (
|
||||
"published"
|
||||
if all(
|
||||
message.get("delivery_status") == "published"
|
||||
for message in mqtt_messages
|
||||
)
|
||||
else "queued"
|
||||
)
|
||||
telemetry["delivery_status"] = delivery_status
|
||||
item["message"] = (
|
||||
"MQTT CAN0 白名单控制帧已由 Broker 确认；"
|
||||
"副驾座椅效果以异步状态回读为准。"
|
||||
if delivery_status == "published"
|
||||
else "MQTT CAN0 白名单控制帧已进入客户端发布队列；"
|
||||
"副驾座椅效果以异步状态回读为准。"
|
||||
)
|
||||
results.extend(command_results)
|
||||
if position_sync_deferred_to_commands:
|
||||
self.position_sync_state["synchronized"] = True
|
||||
results.append(
|
||||
@@ -287,36 +423,129 @@ class HardwareSeatExecutor:
|
||||
}
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001 - normalize transport and protocol failures
|
||||
if bus is not None and motion_armed:
|
||||
submitted_frame = self._active_command_frame
|
||||
feedback_timed_out = (
|
||||
submitted_frame is not None
|
||||
and isinstance(exc, HardwareExecutionError)
|
||||
and str(exc).startswith("timed out after ")
|
||||
)
|
||||
if bus is not None and motion_armed and self.feedback_required:
|
||||
self._best_effort_stop(bus)
|
||||
operation_ref = self._first_unreported_operation(operations, results)
|
||||
mqtt_messages = list(getattr(bus, "published_messages", ())) if bus else []
|
||||
delivery_status = "submitted"
|
||||
if mqtt_messages:
|
||||
delivery_status = (
|
||||
"published"
|
||||
if all(
|
||||
item.get("delivery_status") == "published"
|
||||
for item in mqtt_messages
|
||||
)
|
||||
else "queued"
|
||||
)
|
||||
results.append(
|
||||
{
|
||||
"ref": operation_ref,
|
||||
"domain": "seat",
|
||||
"status": "failed",
|
||||
"message": str(exc),
|
||||
"status": "succeeded" if feedback_timed_out else "failed",
|
||||
"message": (
|
||||
"CAN0 控制帧已提交，但反馈窗口内未完成车辆状态确认；"
|
||||
"编排提交成功，实体效果仍未验证。"
|
||||
if feedback_timed_out
|
||||
else str(exc)
|
||||
),
|
||||
"telemetry": {
|
||||
**getattr(exc, "telemetry", {}),
|
||||
"error_type": type(exc).__name__,
|
||||
"stop_all_attempted": bool(bus is not None and motion_armed),
|
||||
**(
|
||||
{
|
||||
"command_frame": submitted_frame.public(),
|
||||
"delivery_status": delivery_status,
|
||||
"verification_status": "submitted_unverified",
|
||||
"physical_effect_confirmed": False,
|
||||
"feedback_error": str(exc),
|
||||
**(
|
||||
{"mqtt_messages": mqtt_messages}
|
||||
if mqtt_messages
|
||||
else {}
|
||||
),
|
||||
}
|
||||
if feedback_timed_out
|
||||
else {
|
||||
**(
|
||||
{"command_frame": submitted_frame.public()}
|
||||
if submitted_frame is not None
|
||||
else {}
|
||||
),
|
||||
"verification_status": "failed_or_blocked",
|
||||
"physical_effect_confirmed": False,
|
||||
}
|
||||
),
|
||||
"simulated": False,
|
||||
},
|
||||
}
|
||||
)
|
||||
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
|
||||
if self.feedback_required:
|
||||
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)
|
||||
else:
|
||||
# MQTT state synchronization runs independently. Do not
|
||||
# add another bounded receive wait to the control request.
|
||||
physical_state = self._physical_state_from_results(results)
|
||||
bus.close()
|
||||
|
||||
succeeded = all(item["status"] == "succeeded" for item in results)
|
||||
for item in results:
|
||||
telemetry = item.setdefault("telemetry", {})
|
||||
telemetry.setdefault(
|
||||
"verification_status",
|
||||
(
|
||||
"verified_success"
|
||||
if item.get("status") == "succeeded"
|
||||
and (
|
||||
telemetry.get("feedback_frame") is not None
|
||||
or telemetry.get("hardware_feedback") is True
|
||||
or telemetry.get("firmware_verified") is True
|
||||
)
|
||||
else "submitted_unverified"
|
||||
if item.get("status") == "succeeded"
|
||||
and telemetry.get("command_frame") is not None
|
||||
else "verified_success"
|
||||
if item.get("status") == "succeeded"
|
||||
else "failed_or_blocked"
|
||||
),
|
||||
)
|
||||
telemetry.setdefault(
|
||||
"physical_effect_confirmed",
|
||||
telemetry["verification_status"] == "verified_success",
|
||||
)
|
||||
if telemetry.get("command_frame") is not None:
|
||||
telemetry.setdefault("delivery_status", "submitted")
|
||||
verification_status = (
|
||||
"failed_or_blocked"
|
||||
if any(
|
||||
item.get("telemetry", {}).get("verification_status")
|
||||
== "failed_or_blocked"
|
||||
for item in results
|
||||
)
|
||||
else "submitted_unverified"
|
||||
if any(
|
||||
item.get("telemetry", {}).get("verification_status")
|
||||
== "submitted_unverified"
|
||||
for item in results
|
||||
)
|
||||
else "verified_success"
|
||||
)
|
||||
succeeded = verification_status != "failed_or_blocked"
|
||||
request_id = function_config["metadata"]["request_id"]
|
||||
spec: dict[str, Any] = {
|
||||
"mode": "hil",
|
||||
"status": "succeeded" if succeeded else "failed",
|
||||
"verification_status": verification_status,
|
||||
"started_at": started_at,
|
||||
"finished_at": _now(),
|
||||
"results": results,
|
||||
@@ -336,6 +565,59 @@ class HardwareSeatExecutor:
|
||||
"spec": spec,
|
||||
}
|
||||
|
||||
def _submit_without_feedback(
|
||||
self,
|
||||
bus: CanBus,
|
||||
command: dict[str, Any],
|
||||
operations: list[dict[str, Any]],
|
||||
) -> list[dict[str, Any]]:
|
||||
"""Publish one approved CAN0 command without a synchronous state gate.
|
||||
|
||||
This mode is reserved for the wireless head-unit relay. It still uses
|
||||
the same matrix encoder and generated protocol contract as wired HIL,
|
||||
including a fresh firmware sequence for every 0x300 command. Only the
|
||||
evidence level changes: submission succeeds, while physical completion
|
||||
remains unverified until the independent MQTT state stream reports it.
|
||||
"""
|
||||
|
||||
signals = dict(command["signals"])
|
||||
if command["message"] == "T2G_Seat_Move_Command_300":
|
||||
signals["Sequence"] = self._next_sequence()
|
||||
elif command["message"] != "T2G_Seat_Thermal_Control_29E":
|
||||
raise HardwareExecutionError(
|
||||
f"wireless CAN0 executor refuses unknown message {command['message']}"
|
||||
)
|
||||
command_frame = self._send_encoded(
|
||||
bus,
|
||||
command["message"],
|
||||
signals,
|
||||
self._resolved_protocol(command),
|
||||
)
|
||||
self._active_command_frame = command_frame
|
||||
return [
|
||||
{
|
||||
"ref": operation["id"],
|
||||
"domain": "seat",
|
||||
"status": "succeeded",
|
||||
"message": (
|
||||
"CAN0 控制帧已提交到无线 MQTT；副驾座椅效果等待异步状态回读。"
|
||||
),
|
||||
"telemetry": {
|
||||
"requested_target": operation["target"]["instance"],
|
||||
"requested_parameters": operation.get("parameters", {}),
|
||||
"command_frame": command_frame.public(),
|
||||
"feedback_required": False,
|
||||
"feedback_observed": False,
|
||||
"delivery_status": "submitted",
|
||||
"verification_status": "submitted_unverified",
|
||||
"physical_effect_confirmed": False,
|
||||
"control_gate": "compiled_action_and_frame_whitelist",
|
||||
"simulated": False,
|
||||
},
|
||||
}
|
||||
for operation in operations
|
||||
]
|
||||
|
||||
def read_physical_state(self) -> dict[str, Any]:
|
||||
"""Read the passenger seat status without transmitting a CAN command."""
|
||||
|
||||
@@ -359,6 +641,7 @@ class HardwareSeatExecutor:
|
||||
results: list[dict[str, Any]],
|
||||
) -> dict[str, Any]:
|
||||
state = self._physical_state_from_results(results)
|
||||
observed_ids: set[int] = set()
|
||||
deadline = time.monotonic() + max(
|
||||
self.thermal_timeout_ms,
|
||||
self.ack_timeout_ms,
|
||||
@@ -371,10 +654,89 @@ class HardwareSeatExecutor:
|
||||
frame = bus.receive(remaining_ms)
|
||||
if frame is None:
|
||||
break
|
||||
if len(frame.data) >= 8 and frame.arbitration_id in {
|
||||
0x301,
|
||||
0x302,
|
||||
0x303,
|
||||
0x304,
|
||||
0x305,
|
||||
0x306,
|
||||
0x4FA,
|
||||
}:
|
||||
observed_ids.add(frame.arbitration_id)
|
||||
self._observe_auxiliary_frame(frame)
|
||||
if frame.arbitration_id == 0x302 and len(frame.data) >= 8:
|
||||
if frame.arbitration_id == 0x301 and len(frame.data) >= 8:
|
||||
values = self.matrix.decode_by_frame_id(0x301, frame.data)
|
||||
state["system"] = {
|
||||
"ack": int(values["Ack"]),
|
||||
"state": int(values["System_State"]),
|
||||
"reject_reason": int(values["Reject_Reason"]),
|
||||
"last_sequence": int(values["Last_Sequence"]),
|
||||
"last_opcode": int(values["Last_Opcode"]),
|
||||
"active_mask": int(values["Active_Mask"]),
|
||||
"safety_inhibit": bool(values["Safety_Inhibit"]),
|
||||
"calibration_busy": bool(values["Calibration_Busy"]),
|
||||
"fault": bool(values["Fault_Summary"]),
|
||||
"faulted_mask": int(values["Faulted_Mask"]),
|
||||
"position_valid_mask": int(values["Position_Valid_Mask"]),
|
||||
"firmware_revision": int(values["Firmware_Revision"]),
|
||||
"feedback_frame": frame.public(),
|
||||
}
|
||||
elif 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 == 0x303 and len(frame.data) >= 8:
|
||||
values = self.matrix.decode_by_frame_id(0x303, frame.data)
|
||||
motor = int(values["Motor_Sub_ID"])
|
||||
if motor in PHYSICAL_MOTOR_AXES:
|
||||
state.setdefault("diagnostics", {}).setdefault("motors", {})[
|
||||
PHYSICAL_MOTOR_AXES[motor]
|
||||
] = {
|
||||
"hall_count": int(values["Hall_Count"]),
|
||||
"max_hall_count": int(values["Max_Hall_Count"]),
|
||||
"hall_speed_half_pps": int(values["Hall_Speed_Half_Pps"]),
|
||||
"last_event": int(values["Last_Motor_Event"]),
|
||||
"fault_flags": int(values["Fault_Flags"]),
|
||||
"retry_count": int(values["Retry_Count"]),
|
||||
"feedback_frame": frame.public(),
|
||||
}
|
||||
elif frame.arbitration_id == 0x304 and len(frame.data) >= 8:
|
||||
values = self.matrix.decode_by_frame_id(0x304, frame.data)
|
||||
motor = int(values["Motor_Sub_ID"])
|
||||
if motor in PHYSICAL_MOTOR_AXES:
|
||||
motor_state = state.setdefault("diagnostics", {}).setdefault(
|
||||
"motors", {}
|
||||
).setdefault(PHYSICAL_MOTOR_AXES[motor], {})
|
||||
motor_state["current"] = {
|
||||
"sample_valid": bool(values["Current_Sample_Valid"]),
|
||||
"baseline_valid": bool(values["Current_Baseline_Valid"]),
|
||||
"high_current": bool(values["High_Current"]),
|
||||
"hall_activity_low": bool(values["Hall_Activity_Low"]),
|
||||
"calibrating": bool(values["Current_Calibrating"]),
|
||||
"raw": int(values["Current_Raw"]),
|
||||
"baseline_raw": int(values["Current_Baseline_Raw"]),
|
||||
"stall_delta_threshold_raw": int(
|
||||
values["Stall_Current_Delta_Threshold_Raw"]
|
||||
),
|
||||
"feedback_frame": frame.public(),
|
||||
}
|
||||
elif frame.arbitration_id == 0x305 and len(frame.data) >= 8:
|
||||
values = self.matrix.decode_by_frame_id(0x305, frame.data)
|
||||
state.setdefault("diagnostics", {})["thermal"] = {
|
||||
"fan_sample_valid": bool(values["Fan_Current_Sample_Valid"]),
|
||||
"heater_sample_valid": bool(
|
||||
values["Heater_Current_Sample_Valid"]
|
||||
),
|
||||
"fan_running": bool(values["Fan_Running"]),
|
||||
"heater_level": int(values["Heater_Level"]),
|
||||
"fan_current_raw": int(values["Fan_Current_Raw"]),
|
||||
"heater_current_raw": int(values["Heater_Current_Raw"]),
|
||||
"fan_adc_error_count": int(values["Fan_Adc_Error_Count"]),
|
||||
"heater_adc_error_count": int(
|
||||
values["Heater_Adc_Error_Count"]
|
||||
),
|
||||
"feedback_frame": frame.public(),
|
||||
}
|
||||
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)
|
||||
@@ -383,6 +745,11 @@ class HardwareSeatExecutor:
|
||||
self._record_dynamic_state(state, values, frame)
|
||||
state["captured_at"] = _now()
|
||||
state["source"] = "physical_can"
|
||||
state["channel"] = 0
|
||||
state["protocol_profile"] = "passenger-seat-native-can0-v1"
|
||||
state["observed_frame_ids"] = [
|
||||
f"0x{frame_id:03X}" for frame_id in sorted(observed_ids)
|
||||
]
|
||||
state["target"] = {
|
||||
"device": "seat",
|
||||
"zone": "front_right",
|
||||
@@ -532,10 +899,66 @@ class HardwareSeatExecutor:
|
||||
return next((item for item in operations if item not in reported), next(iter(operations)))
|
||||
|
||||
def _send_encoded(
|
||||
self, bus: CanBus, message: str, signals: dict[str, int]
|
||||
self,
|
||||
bus: CanBus,
|
||||
message: str,
|
||||
signals: dict[str, int],
|
||||
protocol: dict[str, Any] | None = None,
|
||||
) -> CanFrame:
|
||||
encoded = self.matrix.encode(message, signals)
|
||||
return bus.send(int(encoded["frame_id"], 16), bytes.fromhex(encoded["data_hex"]))
|
||||
frame_id = int(encoded["frame_id"], 16)
|
||||
payload = bytes.fromhex(encoded["data_hex"])
|
||||
if protocol is not None:
|
||||
expected_id = int(str(protocol["frame_id"]), 16)
|
||||
if frame_id != expected_id:
|
||||
raise HardwareExecutionError(
|
||||
f"shared protocol expects 0x{expected_id:03X}, matrix encoded 0x{frame_id:03X}"
|
||||
)
|
||||
if protocol["format"] != "classic_can" or protocol["brs"]:
|
||||
raise HardwareExecutionError(
|
||||
"CAN0 shared protocol requires classic CAN without BRS"
|
||||
)
|
||||
if len(payload) != int(protocol["dlc"]):
|
||||
raise HardwareExecutionError(
|
||||
"CAN0 encoded payload length disagrees with shared protocol"
|
||||
)
|
||||
return bus.send(frame_id, payload)
|
||||
|
||||
@staticmethod
|
||||
def _resolved_protocol(command: dict[str, Any]) -> dict[str, Any]:
|
||||
"""Return the generated contract, including for legacy in-memory plans."""
|
||||
|
||||
protocol = command.get("protocol")
|
||||
if isinstance(protocol, dict):
|
||||
return protocol
|
||||
message = str(command.get("message", ""))
|
||||
if message == "T2G_Seat_Thermal_Mode_3CE":
|
||||
rule_id, key = "can0.seat.thermal", "thermal_mode"
|
||||
elif message == "T2G_Seat_Thermal_Control_29E":
|
||||
rule_id, key = "can0.seat.thermal", "thermal_control"
|
||||
elif message == "T2G_Seat_Move_Command_300":
|
||||
opcode = int(command.get("signals", {}).get("Opcode", -1))
|
||||
if opcode in {0x0E, 0x0F}:
|
||||
rule_id, key = "can0.seat.dynamic", "dynamic"
|
||||
elif opcode == 0x03:
|
||||
rule_id, key = "can0.seat.relative", "move"
|
||||
else:
|
||||
rule_id, key = "can0.seat.position", "move"
|
||||
else:
|
||||
raise HardwareExecutionError(
|
||||
f"cannot resolve generated CAN0 protocol for message {message}"
|
||||
)
|
||||
return shared_protocol_contract(rule_id, key)
|
||||
|
||||
@classmethod
|
||||
def _protocol_feedback_ids(cls, command: dict[str, Any]) -> tuple[int, ...]:
|
||||
protocol = cls._resolved_protocol(command)
|
||||
values = protocol.get("feedback_ids")
|
||||
if not isinstance(values, list) or not values:
|
||||
raise HardwareExecutionError(
|
||||
"compiled CAN0 command has no generated feedback IDs"
|
||||
)
|
||||
return tuple(int(str(value), 16) for value in values)
|
||||
|
||||
def _receive_until(
|
||||
self,
|
||||
@@ -635,7 +1058,13 @@ class HardwareSeatExecutor:
|
||||
}
|
||||
|
||||
def _wait_ack(
|
||||
self, bus: CanBus, sequence: int, opcode: int, *, expected_ack: int = 1
|
||||
self,
|
||||
bus: CanBus,
|
||||
sequence: int,
|
||||
opcode: int,
|
||||
*,
|
||||
expected_ack: int = 1,
|
||||
arbitration_id: int = 0x301,
|
||||
) -> tuple[dict[str, int], CanFrame]:
|
||||
def matches(values: dict[str, int], _: CanFrame) -> bool:
|
||||
if values["Last_Sequence"] != sequence or values["Last_Opcode"] != opcode:
|
||||
@@ -655,7 +1084,9 @@ class HardwareSeatExecutor:
|
||||
)
|
||||
return True
|
||||
|
||||
return self._receive_until(bus, 0x301, self.ack_timeout_ms, matches)
|
||||
return self._receive_until(
|
||||
bus, arbitration_id, self.ack_timeout_ms, matches
|
||||
)
|
||||
|
||||
def _stop_all(self, bus: CanBus, *, require_ack: bool) -> tuple[CanFrame, CanFrame | None]:
|
||||
sequence = self._next_sequence()
|
||||
@@ -686,7 +1117,14 @@ class HardwareSeatExecutor:
|
||||
command: dict[str, Any],
|
||||
operations: list[dict[str, Any]],
|
||||
) -> list[dict[str, Any]]:
|
||||
command_frame = self._send_encoded(bus, command["message"], command["signals"])
|
||||
command_frame = self._send_encoded(
|
||||
bus,
|
||||
command["message"],
|
||||
command["signals"],
|
||||
self._resolved_protocol(command),
|
||||
)
|
||||
self._active_command_frame = command_frame
|
||||
(thermal_feedback_id,) = self._protocol_feedback_ids(command)
|
||||
expected: dict[str, int] = {}
|
||||
for operation in operations:
|
||||
feature = operation["capability"].removeprefix("seat.set_")
|
||||
@@ -703,7 +1141,7 @@ class HardwareSeatExecutor:
|
||||
return all(values[name] == value for name, value in expected.items())
|
||||
|
||||
values, feedback = self._receive_until(
|
||||
bus, 0x4FA, self.thermal_timeout_ms, matches
|
||||
bus, thermal_feedback_id, self.thermal_timeout_ms, matches
|
||||
)
|
||||
output = []
|
||||
for operation in operations:
|
||||
@@ -758,9 +1196,16 @@ class HardwareSeatExecutor:
|
||||
signals = dict(command["signals"])
|
||||
sequence = self._next_sequence()
|
||||
signals["Sequence"] = sequence
|
||||
command_frame = self._send_encoded(bus, command["message"], signals)
|
||||
ack_feedback_id, dynamic_feedback_id = self._protocol_feedback_ids(command)
|
||||
command_frame = self._send_encoded(
|
||||
bus, command["message"], signals, self._resolved_protocol(command)
|
||||
)
|
||||
self._active_command_frame = command_frame
|
||||
ack_values, ack_frame = self._wait_ack(
|
||||
bus, sequence, 0x0F if stopping else 0x0E
|
||||
bus,
|
||||
sequence,
|
||||
0x0F if stopping else 0x0E,
|
||||
arbitration_id=ack_feedback_id,
|
||||
)
|
||||
|
||||
completion = command["completion"]
|
||||
@@ -780,7 +1225,7 @@ class HardwareSeatExecutor:
|
||||
|
||||
status_values, status_frame = self._receive_until(
|
||||
bus,
|
||||
0x306,
|
||||
dynamic_feedback_id,
|
||||
int(completion["timeout_ms"]),
|
||||
matches,
|
||||
)
|
||||
@@ -916,8 +1361,14 @@ class HardwareSeatExecutor:
|
||||
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)
|
||||
ack_feedback_id, motor_feedback_id = self._protocol_feedback_ids(command)
|
||||
command_frame = self._send_encoded(
|
||||
bus, command["message"], signals, self._resolved_protocol(command)
|
||||
)
|
||||
self._active_command_frame = command_frame
|
||||
ack_values, ack_frame = self._wait_ack(
|
||||
bus, sequence, opcode, arbitration_id=ack_feedback_id
|
||||
)
|
||||
|
||||
def complete(values: dict[str, int], _: CanFrame) -> bool:
|
||||
if values["Motor_Sub_ID"] != motor or values["Motor_Sequence"] != sequence:
|
||||
@@ -941,7 +1392,7 @@ class HardwareSeatExecutor:
|
||||
|
||||
values, feedback = self._receive_until(
|
||||
bus,
|
||||
0x302,
|
||||
motor_feedback_id,
|
||||
int(command["completion"]["timeout_ms"]),
|
||||
complete,
|
||||
)
|
||||
@@ -1389,8 +1840,14 @@ class HardwareSeatExecutor:
|
||||
signals["Sequence"] = sequence
|
||||
self._reset_current_samples(motor)
|
||||
motion_started = time.perf_counter()
|
||||
command_frame = self._send_encoded(bus, command["message"], signals)
|
||||
ack_values, ack_frame = self._wait_ack(bus, sequence, 3)
|
||||
ack_feedback_id, motor_feedback_id = self._protocol_feedback_ids(command)
|
||||
command_frame = self._send_encoded(
|
||||
bus, command["message"], signals, self._resolved_protocol(command)
|
||||
)
|
||||
self._active_command_frame = command_frame
|
||||
ack_values, ack_frame = self._wait_ack(
|
||||
bus, sequence, 3, arbitration_id=ack_feedback_id
|
||||
)
|
||||
|
||||
def terminal(values: dict[str, int], _: CanFrame) -> bool:
|
||||
if values["Motor_Sub_ID"] != motor or values["Motor_Sequence"] != sequence:
|
||||
@@ -1401,7 +1858,7 @@ class HardwareSeatExecutor:
|
||||
|
||||
values, feedback = self._receive_until(
|
||||
bus,
|
||||
0x302,
|
||||
motor_feedback_id,
|
||||
int(command["completion"]["timeout_ms"]),
|
||||
terminal,
|
||||
)
|
||||
|
||||
@@ -152,7 +152,7 @@ LEVELS = {
|
||||
MOTION_AXIS_ALIASES = {
|
||||
"longitudinal": ("水平", "前后", "纵向", "滑轨"),
|
||||
# Internal capability name is retained for compatibility. On the
|
||||
# seat-ota CAN0 firmware this board axis is the physical seat-height axis.
|
||||
# Native CAN0 firmware axis 1 is the physical seat-height axis.
|
||||
"cushion_tilt": (
|
||||
"座椅高度",
|
||||
"高度",
|
||||
|
||||
@@ -930,6 +930,52 @@ class T2GHardwareTests(unittest.TestCase):
|
||||
|
||||
def test_passive_snapshot_reads_all_passenger_axes_and_thermal_state(self) -> None:
|
||||
bus = ReactiveFakeBus(self.matrix)
|
||||
bus.received.extend(
|
||||
[
|
||||
bus._frame(
|
||||
"T2G_Seat_Move_System_Status_301",
|
||||
{
|
||||
"Ack": 1,
|
||||
"System_State": 2,
|
||||
"Last_Sequence": 9,
|
||||
"Last_Opcode": 1,
|
||||
"Position_Valid_Mask": 0x0F,
|
||||
"Firmware_Revision": 1,
|
||||
},
|
||||
),
|
||||
bus._frame(
|
||||
"T2G_Seat_Move_Diagnostic_303",
|
||||
{
|
||||
"Motor_Sub_ID": 0,
|
||||
"Hall_Count": 120,
|
||||
"Max_Hall_Count": 480,
|
||||
"Hall_Speed_Half_Pps": 4,
|
||||
},
|
||||
),
|
||||
bus._frame(
|
||||
"T2G_Seat_Move_Current_Diagnostic_304",
|
||||
{
|
||||
"Motor_Sub_ID": 0,
|
||||
"Current_Sample_Valid": 1,
|
||||
"Current_Baseline_Valid": 1,
|
||||
"Current_Raw": 240,
|
||||
"Current_Baseline_Raw": 100,
|
||||
"Stall_Current_Delta_Threshold_Raw": 500,
|
||||
},
|
||||
),
|
||||
bus._frame(
|
||||
"T2G_Seat_Thermal_Current_Diagnostic_305",
|
||||
{
|
||||
"Fan_Current_Sample_Valid": 1,
|
||||
"Heater_Current_Sample_Valid": 1,
|
||||
"Fan_Running": 1,
|
||||
"Heater_Level": 2,
|
||||
"Fan_Current_Raw": 300,
|
||||
"Heater_Current_Raw": 450,
|
||||
},
|
||||
),
|
||||
]
|
||||
)
|
||||
for motor, value_x100 in enumerate((1200, 3400, 5600, 7800)):
|
||||
bus.received.append(
|
||||
bus._frame(
|
||||
@@ -982,6 +1028,26 @@ class T2GHardwareTests(unittest.TestCase):
|
||||
self.assertEqual(physical["ventilation"]["state"], "off")
|
||||
self.assertEqual(physical["heating"]["state"], "on")
|
||||
self.assertEqual(physical["heating"]["level"], 2)
|
||||
self.assertEqual(physical["channel"], 0)
|
||||
self.assertEqual(
|
||||
physical["protocol_profile"], "passenger-seat-native-can0-v1"
|
||||
)
|
||||
self.assertEqual(physical["system"]["position_valid_mask"], 0x0F)
|
||||
self.assertEqual(
|
||||
physical["diagnostics"]["motors"]["longitudinal"]["hall_count"],
|
||||
120,
|
||||
)
|
||||
self.assertEqual(
|
||||
physical["diagnostics"]["motors"]["longitudinal"]["current"]["raw"],
|
||||
240,
|
||||
)
|
||||
self.assertEqual(
|
||||
physical["diagnostics"]["thermal"]["heater_current_raw"], 450
|
||||
)
|
||||
self.assertEqual(
|
||||
physical["observed_frame_ids"],
|
||||
["0x301", "0x302", "0x303", "0x304", "0x305", "0x4FA"],
|
||||
)
|
||||
self.assertTrue(bus.closed)
|
||||
|
||||
def test_executor_sends_firmware_valid_stop_all_before_four_axis_zero(self) -> None:
|
||||
@@ -1516,11 +1582,18 @@ class T2GHardwareTests(unittest.TestCase):
|
||||
self.matrix, lambda: bus, ack_timeout_ms=20
|
||||
).execute(plan, config)
|
||||
|
||||
self.assertEqual(report["spec"]["status"], "failed")
|
||||
self.assertEqual([item.data[0] for item in bus.sent], [9, 1, 9])
|
||||
self.assertTrue(
|
||||
report["spec"]["results"][-1]["telemetry"]["stop_all_attempted"]
|
||||
self.assertEqual(report["spec"]["status"], "succeeded")
|
||||
self.assertEqual(
|
||||
report["spec"]["verification_status"],
|
||||
"submitted_unverified",
|
||||
)
|
||||
self.assertEqual([item.data[0] for item in bus.sent], [9, 1, 9])
|
||||
telemetry = report["spec"]["results"][-1]["telemetry"]
|
||||
self.assertTrue(telemetry["stop_all_attempted"])
|
||||
self.assertEqual(telemetry["delivery_status"], "submitted")
|
||||
self.assertEqual(telemetry["command_frame"]["frame_id"], "0x300")
|
||||
self.assertEqual(telemetry["verification_status"], "submitted_unverified")
|
||||
self.assertFalse(telemetry["physical_effect_confirmed"])
|
||||
|
||||
def test_executor_waits_for_real_thermal_status(self) -> None:
|
||||
config = function_config(
|
||||
@@ -1536,6 +1609,7 @@ class T2GHardwareTests(unittest.TestCase):
|
||||
telemetry = report["spec"]["results"][0]["telemetry"]
|
||||
self.assertEqual(telemetry["feedback_frame"]["frame_id"], "0x4FA")
|
||||
self.assertEqual(telemetry["feedback_value"], 3)
|
||||
self.assertEqual(telemetry["verification_status"], "verified_success")
|
||||
|
||||
def test_executor_preserves_can_adapter_open_failure(self) -> None:
|
||||
config = function_config(
|
||||
@@ -1553,6 +1627,9 @@ class T2GHardwareTests(unittest.TestCase):
|
||||
self.assertEqual(report["spec"]["status"], "failed")
|
||||
failure = report["spec"]["results"][-1]
|
||||
self.assertEqual(failure["telemetry"]["error_type"], "CanGatewayError")
|
||||
self.assertEqual(
|
||||
failure["telemetry"]["verification_status"], "failed_or_blocked"
|
||||
)
|
||||
self.assertIn("ZCAN_OpenDevice failed", failure["message"])
|
||||
self.assertFalse(failure["telemetry"]["stop_all_attempted"])
|
||||
|
||||
|
||||
Reference in New Issue
Block a user