vee
477 lines
19 KiB
Python
Executable File
477 lines
19 KiB
Python
Executable File
"""Compile seat functions to the CAN protocol implemented by the T2G IAR demos."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import hashlib
|
|
import json
|
|
from pathlib import Path
|
|
from typing import Any
|
|
|
|
import yaml
|
|
|
|
|
|
STATE_VALUES = {"on": 1, "off": 2}
|
|
|
|
|
|
class T2GSeatCompiler:
|
|
"""Deterministic compiler for the HIL-verified T2G seat bench protocol."""
|
|
|
|
version = "2.1.0"
|
|
|
|
def __init__(self, mapping_path: Path, calibration_path: Path):
|
|
self.mapping = yaml.safe_load(mapping_path.read_text(encoding="utf-8"))
|
|
self.calibration = yaml.safe_load(calibration_path.read_text(encoding="utf-8"))
|
|
self.mapping_version = self.mapping["metadata"]["version"]
|
|
self.axes = self.calibration["axes"]
|
|
self.protocol = self.calibration["protocol"]
|
|
self.supported = {
|
|
(item["capability"], item.get("target_zone"))
|
|
for item in self.mapping["spec"]["mappings"]
|
|
}
|
|
|
|
def compile(
|
|
self,
|
|
function_config: dict[str, Any],
|
|
actuator_state: dict[str, Any] | None = None,
|
|
) -> dict[str, Any]:
|
|
operations = function_config["spec"]["operations"]
|
|
self._validate_passenger_targets(operations)
|
|
thermal_operations = [
|
|
item
|
|
for item in operations
|
|
if item["capability"] in {"seat.set_ventilation", "seat.set_heating"}
|
|
]
|
|
dynamic_operations = [
|
|
item
|
|
for item in operations
|
|
if item["capability"] in {
|
|
"seat.start_dynamic_mode",
|
|
"seat.stop_dynamic_mode",
|
|
}
|
|
]
|
|
motion_operations = [
|
|
item
|
|
for item in operations
|
|
if item not in thermal_operations and item not in dynamic_operations
|
|
]
|
|
commands: list[dict[str, Any]] = []
|
|
|
|
if thermal_operations:
|
|
commands.extend(
|
|
self._compile_thermal(thermal_operations, actuator_state or {})
|
|
)
|
|
if dynamic_operations:
|
|
commands.extend(
|
|
self._compile_dynamic_mode(dynamic_operations, len(commands))
|
|
)
|
|
commands.extend(self._compile_motion(motion_operations, len(commands)))
|
|
|
|
canonical = json.dumps(
|
|
function_config, ensure_ascii=False, sort_keys=True
|
|
).encode("utf-8")
|
|
metadata = function_config["metadata"]
|
|
return {
|
|
"apiVersion": "cognitive-agent/v1alpha1",
|
|
"kind": "CommandPlan",
|
|
"metadata": {
|
|
"id": f"plan-t2g-{metadata['request_id']}",
|
|
"name": "T2G seat hardware CAN command plan",
|
|
"version": "1.0.0",
|
|
"request_id": metadata["request_id"],
|
|
"labels": {
|
|
"execution_scope": "hardware_candidate",
|
|
"protocol": "t2g_seat_v1",
|
|
},
|
|
},
|
|
"spec": {
|
|
"domain": "seat",
|
|
"platform": "t2g_seat_bench",
|
|
"source": {
|
|
"config_id": metadata["id"],
|
|
"config_version": metadata["version"],
|
|
"config_sha256": hashlib.sha256(canonical).hexdigest(),
|
|
"compiler_version": self.version,
|
|
"mapping_version": self.mapping_version,
|
|
},
|
|
"commands": commands,
|
|
},
|
|
}
|
|
|
|
def _compile_thermal(
|
|
self,
|
|
operations: list[dict[str, Any]],
|
|
actuator_state: dict[str, Any],
|
|
) -> list[dict[str, Any]]:
|
|
state = actuator_state.get("passenger", {})
|
|
desired = {
|
|
"ventilation": dict(state.get("ventilation", {"state": "off", "level": 0})),
|
|
"heating": dict(state.get("heating", {"state": "off", "level": 0})),
|
|
}
|
|
for operation in operations:
|
|
self._require_supported(operation)
|
|
feature = operation["capability"].removeprefix("seat.set_")
|
|
requested_state = str(operation["parameters"]["state"])
|
|
previous_level = int(desired[feature].get("level", 0))
|
|
level = (
|
|
int(operation["parameters"].get("level", previous_level or 1))
|
|
if requested_state == "on"
|
|
else 0
|
|
)
|
|
desired[feature] = {"state": requested_state, "level": level}
|
|
|
|
operation_ref = ",".join(item["id"] for item in operations)
|
|
commands: list[dict[str, Any]] = []
|
|
if desired["heating"]["state"] == "on":
|
|
commands.append(
|
|
{
|
|
"id": "t2g_seat_thermal_mode",
|
|
"operation_ref": operation_ref,
|
|
"bus": "seat_can",
|
|
"message": "T2G_Seat_Thermal_Mode_3CE",
|
|
"signals": {"Driver_Seat_Thermal_Mode": 1},
|
|
"send_offset_ms": 0,
|
|
}
|
|
)
|
|
commands.append(
|
|
{
|
|
"id": "t2g_seat_thermal_control",
|
|
"operation_ref": operation_ref,
|
|
"bus": "seat_can",
|
|
"message": "T2G_Seat_Thermal_Control_29E",
|
|
"signals": {
|
|
"Driver_Seat_Heating_Gear": int(desired["heating"]["level"]),
|
|
"Driver_Seat_Heating_On": STATE_VALUES[desired["heating"]["state"]],
|
|
"Driver_Seat_Ventilation_Gear": int(desired["ventilation"]["level"]),
|
|
"Driver_Seat_Ventilation_Mode": 1,
|
|
"Driver_Seat_Ventilation_On": STATE_VALUES[
|
|
desired["ventilation"]["state"]
|
|
],
|
|
},
|
|
"send_offset_ms": 20 if commands else 0,
|
|
}
|
|
)
|
|
return commands
|
|
|
|
def _compile_motion(
|
|
self, operations: list[dict[str, Any]], command_offset: int
|
|
) -> list[dict[str, Any]]:
|
|
grouped: list[list[dict[str, Any]]] = []
|
|
by_capability: dict[str, list[dict[str, Any]]] = {}
|
|
for operation in operations:
|
|
self._require_supported(operation)
|
|
capability = operation["capability"]
|
|
if capability not in by_capability:
|
|
by_capability[capability] = []
|
|
grouped.append(by_capability[capability])
|
|
by_capability[capability].append(operation)
|
|
|
|
commands = []
|
|
for index, physical_operations in enumerate(grouped):
|
|
operation = physical_operations[0]
|
|
capability = operation["capability"]
|
|
try:
|
|
axis = self.axes[capability]
|
|
except KeyError as exc:
|
|
raise ValueError(
|
|
f"T2G IAR firmware has no calibrated actuator for {capability}"
|
|
) from exc
|
|
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))
|
|
motion = operation.get("motion")
|
|
relative_fallback: dict[str, Any] | None = None
|
|
if motion is not None and motion.get("mode") == "timed":
|
|
commands.append(
|
|
self._compile_timed_motion(
|
|
operation,
|
|
physical_operations,
|
|
axis,
|
|
command_offset + index,
|
|
)
|
|
)
|
|
continue
|
|
if motion is not None and motion.get("mode") == "relative":
|
|
delta_pct = int(motion["delta_position_pct"])
|
|
maximum_delta = int(axis["max_uncalibrated_relative_pct"])
|
|
if delta_pct <= maximum_delta:
|
|
relative_fallback = self._compile_uncalibrated_relative(
|
|
operation,
|
|
physical_operations,
|
|
axis,
|
|
command_offset + index,
|
|
)
|
|
# Prefer the resolved closed-loop target whenever the board has
|
|
# a live calibrated position. The bounded timed-duty command
|
|
# remains available only as an uncalibrated fallback.
|
|
explicit_home = position_pct == 0
|
|
command_opcode = int(
|
|
self.protocol[
|
|
"home_known_range_opcode"
|
|
if explicit_home
|
|
else "absolute_position_opcode"
|
|
]
|
|
)
|
|
command_param_a = (
|
|
int(self.protocol["home_known_range_key"])
|
|
if explicit_home
|
|
else target
|
|
)
|
|
command_param_b = (
|
|
int(self.protocol["home_known_range_timeout_s"])
|
|
if explicit_home
|
|
else absolute_timeout_ms
|
|
)
|
|
command = {
|
|
"id": f"t2g_move_{operation['id']}",
|
|
"operation_ref": ",".join(
|
|
item["id"] for item in physical_operations
|
|
),
|
|
"bus": "seat_can",
|
|
"message": "T2G_Seat_Move_Command_300",
|
|
"signals": {
|
|
"Direction": 0,
|
|
"Motor_Selector": int(axis["motor_selector"]),
|
|
# An explicit 0% request is a physical zeroing request,
|
|
# not permission to trust a potentially drifted Hall
|
|
# counter. Opcode 0x0B proves the real near endpoint and
|
|
# rebuilds position zero while preserving MaxHall.
|
|
"Opcode": command_opcode,
|
|
"Param_A": command_param_a,
|
|
# Opcode 1 uses milliseconds; Opcode 0x0B uses seconds.
|
|
"Param_B": command_param_b,
|
|
"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)
|
|
* 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": max(
|
|
int(operation.get("timeout_ms", 0)),
|
|
(
|
|
command_param_b * 1000 + 5000
|
|
if explicit_home
|
|
else absolute_timeout_ms + 3000
|
|
),
|
|
),
|
|
},
|
|
}
|
|
if relative_fallback is not None:
|
|
command["relative_fallback"] = relative_fallback
|
|
commands.append(command)
|
|
return commands
|
|
|
|
def _compile_dynamic_mode(
|
|
self,
|
|
operations: list[dict[str, Any]],
|
|
command_offset: int,
|
|
) -> list[dict[str, Any]]:
|
|
if len(operations) != 1:
|
|
raise ValueError("one command may start or stop only one dynamic seat mode")
|
|
operation = operations[0]
|
|
self._require_supported(operation)
|
|
stopping = operation["capability"] == "seat.stop_dynamic_mode"
|
|
mode_id = 0 if stopping else int(operation["parameters"]["mode_id"])
|
|
if not stopping and mode_id not in {1, 2}:
|
|
raise ValueError("T2G dynamic mode_id must be 1 or 2")
|
|
return [
|
|
{
|
|
"id": f"t2g_dynamic_{operation['id']}",
|
|
"operation_ref": operation["id"],
|
|
"bus": "seat_can",
|
|
"message": "T2G_Seat_Move_Command_300",
|
|
"signals": {
|
|
"Direction": 0,
|
|
"Motor_Selector": 0,
|
|
"Opcode": int(
|
|
self.protocol[
|
|
"stop_dynamic_mode_opcode"
|
|
if stopping
|
|
else "start_dynamic_mode_opcode"
|
|
]
|
|
),
|
|
"Param_A": mode_id,
|
|
"Param_B": 0,
|
|
"Protocol_Version": int(self.protocol["version"]),
|
|
"Sequence": 0,
|
|
},
|
|
"send_offset_ms": (
|
|
command_offset * int(self.protocol["inter_axis_settle_ms"])
|
|
),
|
|
"completion": {
|
|
"message": "T2G_Seat_Dynamic_Mode_Status_306",
|
|
"signal": "Dynamic_State" if stopping else "Dynamic_Mode_ID",
|
|
"operator": "eq",
|
|
"value": 0 if stopping else mode_id,
|
|
"state_signal": "Dynamic_State",
|
|
"accepted_states": [0] if stopping else [1, 2],
|
|
"timeout_ms": max(int(operation.get("timeout_ms", 0)), 5000),
|
|
},
|
|
}
|
|
]
|
|
|
|
def _compile_timed_motion(
|
|
self,
|
|
operation: dict[str, Any],
|
|
physical_operations: list[dict[str, Any]],
|
|
axis: dict[str, Any],
|
|
command_index: int,
|
|
) -> dict[str, Any]:
|
|
motion = dict(operation["motion"])
|
|
duration_ms = int(motion["applied_duration_ms"])
|
|
minimum_duration = int(self.protocol["timed_duty_min_duration_ms"])
|
|
protocol_maximum = int(self.protocol["timed_duty_max_duration_ms"])
|
|
axis_maximum = int(
|
|
axis.get("max_user_timed_duration_ms", protocol_maximum)
|
|
)
|
|
if not minimum_duration <= duration_ms <= min(axis_maximum, protocol_maximum):
|
|
raise ValueError(
|
|
"timed seat movement duration was not clamped to the configured "
|
|
f"{minimum_duration}..{min(axis_maximum, protocol_maximum)} ms range"
|
|
)
|
|
if int(motion.get("max_duration_ms", axis_maximum)) != axis_maximum:
|
|
raise ValueError(
|
|
"timed seat movement semantic limit does not match hardware calibration"
|
|
)
|
|
direction_name = str(motion["direction"])
|
|
direction = int(self.protocol[f"{direction_name}_direction"])
|
|
timeout_ms = max(
|
|
int(operation.get("timeout_ms", 8_000)), duration_ms + 5_000
|
|
)
|
|
return {
|
|
"id": f"t2g_move_{operation['id']}",
|
|
"operation_ref": ",".join(
|
|
item["id"] for item in physical_operations
|
|
),
|
|
"bus": "seat_can",
|
|
"message": "T2G_Seat_Move_Command_300",
|
|
"signals": {
|
|
"Direction": direction,
|
|
"Motor_Selector": int(axis["motor_selector"]),
|
|
"Opcode": int(self.protocol["timed_duty_opcode"]),
|
|
"Param_A": duration_ms,
|
|
"Param_B": int(
|
|
axis.get(
|
|
"timed_duty_percent",
|
|
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 * int(self.protocol["inter_axis_settle_ms"])
|
|
),
|
|
"completion": {
|
|
"message": "T2G_Seat_Move_Motor_Status_302",
|
|
"signal": "Task_State",
|
|
"operator": "eq",
|
|
"value": 6,
|
|
"timeout_ms": timeout_ms,
|
|
},
|
|
}
|
|
|
|
def _compile_uncalibrated_relative(
|
|
self,
|
|
operation: dict[str, Any],
|
|
physical_operations: list[dict[str, Any]],
|
|
axis: dict[str, Any],
|
|
command_index: int,
|
|
) -> dict[str, Any]:
|
|
motion = dict(operation["motion"])
|
|
delta_pct = int(motion["delta_position_pct"])
|
|
maximum_delta = int(axis["max_uncalibrated_relative_pct"])
|
|
if delta_pct <= 0:
|
|
raise ValueError(
|
|
"relative seat movement resolved to zero travel; no motor command was generated"
|
|
)
|
|
if delta_pct > maximum_delta:
|
|
raise ValueError(
|
|
"internal compiler error: an oversized relative movement must be "
|
|
"compiled as a calibrated absolute target"
|
|
)
|
|
|
|
duration_ms = delta_pct * int(axis["timed_duty_ms_per_pct"])
|
|
duration_ms = max(
|
|
int(self.protocol["timed_duty_min_duration_ms"]), duration_ms
|
|
)
|
|
maximum_duration = int(self.protocol["timed_duty_max_duration_ms"])
|
|
if duration_ms > maximum_duration:
|
|
raise ValueError(
|
|
"uncalibrated relative seat movement would exceed the bounded "
|
|
f"{maximum_duration} ms timed-motion limit"
|
|
)
|
|
direction_name = str(motion["direction"])
|
|
direction = int(self.protocol[f"{direction_name}_direction"])
|
|
timeout_ms = max(
|
|
int(operation.get("timeout_ms", 8000)), duration_ms + 3000
|
|
)
|
|
return {
|
|
"id": f"t2g_move_{operation['id']}",
|
|
"operation_ref": ",".join(
|
|
item["id"] for item in physical_operations
|
|
),
|
|
"bus": "seat_can",
|
|
"message": "T2G_Seat_Move_Command_300",
|
|
"signals": {
|
|
"Direction": direction,
|
|
"Motor_Selector": int(axis["motor_selector"]),
|
|
"Opcode": int(self.protocol["timed_duty_opcode"]),
|
|
"Param_A": duration_ms,
|
|
"Param_B": int(
|
|
axis.get(
|
|
"timed_duty_percent",
|
|
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 * int(self.protocol["inter_axis_settle_ms"])
|
|
),
|
|
"completion": {
|
|
"message": "T2G_Seat_Move_Motor_Status_302",
|
|
"signal": "Task_State",
|
|
"operator": "eq",
|
|
"value": 6,
|
|
"timeout_ms": timeout_ms,
|
|
},
|
|
}
|
|
|
|
@staticmethod
|
|
def _validate_passenger_targets(
|
|
operations: list[dict[str, Any]],
|
|
) -> None:
|
|
for operation in operations:
|
|
target = operation.get("target", {})
|
|
if (
|
|
target.get("instance") != "passenger"
|
|
or target.get("zone") != "front_right"
|
|
):
|
|
raise ValueError(
|
|
"T2G hardware accepts only passenger/front_right seat operations"
|
|
)
|
|
|
|
def _require_supported(self, operation: dict[str, Any]) -> None:
|
|
key = (operation["capability"], operation["target"]["zone"])
|
|
if key not in self.supported:
|
|
raise ValueError(
|
|
"T2G hardware mapping does not support "
|
|
f"capability={key[0]}, zone={key[1]}"
|
|
)
|