minghanchen
1734 lines
71 KiB
Python
1734 lines
71 KiB
Python
"""Trusted compilation and execution support for CAN_DEV vehicle controls."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import hashlib
|
|
import json
|
|
import os
|
|
import time
|
|
from dataclasses import dataclass
|
|
from datetime import UTC, datetime
|
|
from pathlib import Path
|
|
from typing import Any, Callable
|
|
|
|
import yaml
|
|
|
|
from cognitive_agent.application.native_can_feedback import (
|
|
decode_feedback,
|
|
feedback_matches,
|
|
)
|
|
from cognitive_agent.infrastructure.can.gateway import CanBus, CanFrame
|
|
|
|
|
|
def _now() -> str:
|
|
return datetime.now(UTC).isoformat().replace("+00:00", "Z")
|
|
|
|
|
|
def _set_bits(payload: bytearray, start: int, length: int, value: int) -> None:
|
|
if length <= 0 or start < 0 or start + length > len(payload) * 8:
|
|
raise ValueError("invalid bit range")
|
|
if value < 0 or value >= (1 << length):
|
|
raise ValueError(f"value {value} does not fit in {length} bits")
|
|
for offset in range(length):
|
|
bit = start + offset
|
|
mask = 1 << (bit % 8)
|
|
if value & (1 << offset):
|
|
payload[bit // 8] |= mask
|
|
else:
|
|
payload[bit // 8] &= ~mask
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class NativeFrameSpec:
|
|
frame_id: int
|
|
payload: bytes
|
|
is_fd: bool = False
|
|
bitrate_switch: bool = False
|
|
repeat_count: int = 1
|
|
period_ms: int = 0
|
|
feedback_ids: tuple[int, ...] = ()
|
|
expected_feedback: str = ""
|
|
special_handler: str = ""
|
|
special_parameters: dict[str, Any] | None = None
|
|
|
|
|
|
class NativeCanCompileError(ValueError):
|
|
"""Raised when a request is outside the CAN_DEV verified vocabulary."""
|
|
|
|
|
|
DRIVER_SEAT_CAPABILITIES = frozenset(
|
|
{
|
|
"seat.set_driver_ventilation_native",
|
|
"seat.set_driver_heating_native",
|
|
"seat.set_driver_massage_native",
|
|
"seat.set_driver_axis_native",
|
|
"seat.set_driver_position_native",
|
|
"seat.recall_driver_memory_native",
|
|
}
|
|
)
|
|
DRIVER_SEAT_TARGETS = frozenset({"driver", "front_left"})
|
|
UNSUPPORTED_CH1_DOOR_LOCK_CAPABILITIES = frozenset(
|
|
{"door.set_vehicle_lock"}
|
|
)
|
|
|
|
|
|
def _unsupported_ch1_door_lock_message(capability: str) -> str:
|
|
if capability == "door.set_vehicle_lock":
|
|
return (
|
|
"door.set_vehicle_lock is unsupported for physical execution on the "
|
|
"current CH1; the 0x496 smoke request caused no 0x407 vehicle-lock or "
|
|
"four-door transition, so CH1 door-lock support is observational only"
|
|
)
|
|
return f"{capability} is unsupported for physical execution on the current CH1"
|
|
|
|
|
|
class NativeCanRuntime:
|
|
"""Compile named capabilities into byte-exact CAN_DEV frames."""
|
|
|
|
compiler_version = "1.0.0"
|
|
mapping_version = "can-dev-verified-body-can-v5"
|
|
|
|
def __init__(self, repository_root: Path):
|
|
self.root = repository_root
|
|
calibration_path = (
|
|
repository_root
|
|
/ "platforms"
|
|
/ "t2g_cabin_bench"
|
|
/ "calibration"
|
|
/ "body_can.yaml"
|
|
)
|
|
self.calibration_path = calibration_path
|
|
self.calibration = yaml.safe_load(calibration_path.read_text(encoding="utf-8"))
|
|
self.capabilities: dict[str, dict[str, Any]] = {}
|
|
catalog_paths = [
|
|
*(repository_root / "domains").glob("*/config/capabilities.yaml"),
|
|
*(repository_root / "config" / "native_can_capabilities").glob("*.yaml"),
|
|
]
|
|
for path in sorted(catalog_paths):
|
|
catalog = yaml.safe_load(path.read_text(encoding="utf-8"))
|
|
for capability in catalog.get("spec", {}).get("capabilities", []):
|
|
self.capabilities[capability["name"]] = capability
|
|
|
|
@property
|
|
def supported_capabilities(self) -> frozenset[str]:
|
|
return frozenset(
|
|
{
|
|
"window.set_motion",
|
|
"window.set_position",
|
|
"climate.set_power",
|
|
"climate.set_ac",
|
|
"climate.set_fan_level",
|
|
"climate.set_outlet_mode",
|
|
"climate.set_temperature",
|
|
"climate.set_control_mode",
|
|
"climate.set_cycle_mode",
|
|
"door.set_rear_lock",
|
|
"outlet.set_position_percent",
|
|
"outlet.set_calibrated_position",
|
|
*DRIVER_SEAT_CAPABILITIES,
|
|
"interior_light.set_indoor_power",
|
|
"interior_light.set_reading_power",
|
|
"ambient_light.set_power",
|
|
"ambient_light.set_color_index",
|
|
"ambient_light.set_brightness_level",
|
|
"roof.set_shade_motion",
|
|
"roof.set_shade_position",
|
|
}
|
|
)
|
|
|
|
def supports(self, action: dict[str, Any]) -> bool:
|
|
capability = str(action.get("capability"))
|
|
if capability not in self.supported_capabilities:
|
|
return False
|
|
if capability in DRIVER_SEAT_CAPABILITIES:
|
|
target = str(action.get("target", action.get("target_scope", "")))
|
|
return target in DRIVER_SEAT_TARGETS
|
|
if capability == "interior_light.set_reading_power":
|
|
target = str(action.get("target", action.get("target_scope", "")))
|
|
return target == "all"
|
|
return True
|
|
|
|
def build_function_config(
|
|
self,
|
|
domain: str,
|
|
actions: list[dict[str, Any]],
|
|
request_id: str,
|
|
provider: str,
|
|
) -> dict[str, Any]:
|
|
operations = []
|
|
for index, action in enumerate(actions, start=1):
|
|
capability_name = str(action["capability"])
|
|
if capability_name not in self.supported_capabilities:
|
|
if capability_name in UNSUPPORTED_CH1_DOOR_LOCK_CAPABILITIES:
|
|
raise NativeCanCompileError(
|
|
_unsupported_ch1_door_lock_message(capability_name)
|
|
)
|
|
raise NativeCanCompileError(
|
|
f"CAN_DEV does not implement {capability_name}"
|
|
)
|
|
capability = self.capabilities[capability_name]
|
|
target = str(action.get("target", action.get("target_scope", "vehicle")))
|
|
if target not in capability.get("target_scopes", []):
|
|
raise NativeCanCompileError(
|
|
f"{capability_name} does not support target {target}"
|
|
)
|
|
suffix = capability_name.split(".", 1)[1]
|
|
operation_id = f"{target}_{suffix}_{index}"[:63]
|
|
operations.append(
|
|
{
|
|
"id": operation_id,
|
|
"capability": capability_name,
|
|
"target": {
|
|
"device": capability["target_devices"][0],
|
|
"zone": target,
|
|
},
|
|
"parameters": dict(action.get("parameters", {})),
|
|
"parse_source": str(
|
|
action.get("parse_source", "local_rule")
|
|
),
|
|
"timeout_ms": (
|
|
20_000
|
|
if capability_name
|
|
in {
|
|
"window.set_position",
|
|
"roof.set_shade_position",
|
|
"seat.set_driver_position_native",
|
|
"seat.recall_driver_memory_native",
|
|
}
|
|
else 4_000
|
|
),
|
|
"failure_policy": "abort",
|
|
}
|
|
)
|
|
return {
|
|
"apiVersion": "cognitive-agent/v1alpha1",
|
|
"kind": "FunctionConfig",
|
|
"metadata": {
|
|
"id": f"native-{domain}-{request_id}",
|
|
"name": f"{domain.replace('_', ' ').title()} native CAN function",
|
|
"version": "1.0.0",
|
|
"request_id": request_id,
|
|
"generated_at": _now(),
|
|
"labels": {
|
|
"provider": provider,
|
|
"execution_scope": "vehicle_can_channel_1",
|
|
"source_branch": "CAN_DEV",
|
|
},
|
|
},
|
|
"spec": {
|
|
"domain": domain,
|
|
"operations": operations,
|
|
"assumptions": [
|
|
"Only named, range-checked CAN_DEV capabilities may reach the body CAN channel.",
|
|
"The body CAN channel uses ISO CAN FD at 500 kbit/s arbitration and 2 Mbit/s data rate.",
|
|
],
|
|
},
|
|
}
|
|
|
|
def compile(self, function_config: dict[str, Any]) -> dict[str, Any]:
|
|
commands: list[dict[str, Any]] = []
|
|
for operation_index, operation in enumerate(
|
|
function_config["spec"]["operations"], start=1
|
|
):
|
|
specs = self._frame_specs(operation)
|
|
for frame_index, spec in enumerate(specs, start=1):
|
|
signals: dict[str, int | bool | str] = {
|
|
"frame_id": f"0x{spec.frame_id:03X}",
|
|
"data_hex": " ".join(f"{byte:02X}" for byte in spec.payload),
|
|
"is_fd": spec.is_fd,
|
|
"bitrate_switch": spec.bitrate_switch,
|
|
"feedback_ids": ",".join(
|
|
f"0x{frame_id:03X}" for frame_id in spec.feedback_ids
|
|
),
|
|
"expected_feedback": spec.expected_feedback,
|
|
"debug_command": self.operation_debug_command(operation),
|
|
}
|
|
if spec.special_handler:
|
|
signals["special_handler"] = spec.special_handler
|
|
signals["special_parameters"] = json.dumps(
|
|
spec.special_parameters or {},
|
|
ensure_ascii=False,
|
|
sort_keys=True,
|
|
)
|
|
command: dict[str, Any] = {
|
|
"id": f"native_{operation_index}_{frame_index}_control",
|
|
"operation_ref": operation["id"],
|
|
"bus": "body_can",
|
|
"message": f"Native_0x{spec.frame_id:03X}",
|
|
"signals": signals,
|
|
"send_offset_ms": (operation_index - 1) * 20,
|
|
}
|
|
if spec.repeat_count > 1:
|
|
command["repeat"] = {
|
|
"count": spec.repeat_count,
|
|
"period_ms": spec.period_ms,
|
|
}
|
|
commands.append(command)
|
|
|
|
canonical = json.dumps(
|
|
function_config, ensure_ascii=False, sort_keys=True
|
|
).encode("utf-8")
|
|
metadata = function_config["metadata"]
|
|
domain = function_config["spec"]["domain"]
|
|
return {
|
|
"apiVersion": "cognitive-agent/v1alpha1",
|
|
"kind": "CommandPlan",
|
|
"metadata": {
|
|
"id": f"plan-native-{domain}-{metadata['request_id']}",
|
|
"name": f"{domain.replace('_', ' ').title()} native CAN command plan",
|
|
"version": "1.0.0",
|
|
"request_id": metadata["request_id"],
|
|
"labels": {
|
|
"execution_scope": "vehicle_can_channel_1",
|
|
"source_branch": "CAN_DEV",
|
|
},
|
|
},
|
|
"spec": {
|
|
"domain": domain,
|
|
"platform": "t2g_cabin_bench",
|
|
"source": {
|
|
"config_id": metadata["id"],
|
|
"config_version": metadata["version"],
|
|
"config_sha256": hashlib.sha256(canonical).hexdigest(),
|
|
"compiler_version": self.compiler_version,
|
|
"mapping_version": self.mapping_version,
|
|
},
|
|
"commands": commands,
|
|
},
|
|
}
|
|
|
|
def execute_virtual(
|
|
self,
|
|
command_plan: dict[str, Any],
|
|
function_config: dict[str, Any],
|
|
) -> dict[str, Any]:
|
|
started_at = _now()
|
|
operations = {
|
|
operation["id"]: operation
|
|
for operation in function_config["spec"]["operations"]
|
|
}
|
|
results = []
|
|
for command in command_plan["spec"]["commands"]:
|
|
operation = operations[command["operation_ref"]]
|
|
results.append(
|
|
{
|
|
"ref": operation["id"],
|
|
"domain": function_config["spec"]["domain"],
|
|
"status": "succeeded",
|
|
"message": "已完成 CAN_DEV 帧编译；dry-run 未打开物理 CAN。",
|
|
"telemetry": {
|
|
"target": operation["target"]["zone"],
|
|
"requested_parameters": operation["parameters"],
|
|
"command_frame": self.command_frame_public(command),
|
|
"source_branch": "CAN_DEV",
|
|
"simulated": True,
|
|
},
|
|
}
|
|
)
|
|
return self._report(function_config, results, started_at, "dry_run")
|
|
|
|
@staticmethod
|
|
def command_frame_public(command: dict[str, Any]) -> dict[str, Any]:
|
|
signals = command["signals"]
|
|
public = {
|
|
"frame_id": signals["frame_id"],
|
|
"payload_size": len(bytes.fromhex(str(signals["data_hex"]))),
|
|
"data_hex": signals["data_hex"],
|
|
"channel": 1,
|
|
"frame_format": "can_fd" if signals["is_fd"] else "classic_can",
|
|
"bitrate_switch": bool(signals["bitrate_switch"]),
|
|
"repeat": command.get("repeat", {"count": 1, "period_ms": 0}),
|
|
}
|
|
if signals.get("special_handler") == "driver_seat_axis":
|
|
special = json.loads(str(signals["special_parameters"]))
|
|
public["safety"] = {
|
|
"forced_stop": True,
|
|
"stop_data_hex": special["stop_data_hex"],
|
|
"stop_repeat_count": special["stop_repeat_count"],
|
|
"stop_period_ms": special["stop_period_ms"],
|
|
}
|
|
elif signals.get("special_handler") == "driver_seat_memory_recall":
|
|
special = json.loads(str(signals["special_parameters"]))
|
|
public["sequence"] = {
|
|
"frame_count": len(special["frame_sequence"]),
|
|
"period_ms": special["period_ms"],
|
|
"data_hex": list(special["frame_sequence"]),
|
|
"requires_fresh_initialized_0x494": True,
|
|
}
|
|
return public
|
|
|
|
@staticmethod
|
|
def operation_debug_command(operation: dict[str, Any]) -> str:
|
|
"""Return the trusted canonical command represented by one operation."""
|
|
|
|
capability = str(operation["capability"])
|
|
parameters = operation.get("parameters", {})
|
|
target = str(operation.get("target", {}).get("zone", "vehicle")).replace(
|
|
"_", "-"
|
|
)
|
|
if capability == "window.set_motion":
|
|
return f"window {target} {parameters['motion']}"
|
|
if capability == "window.set_position":
|
|
return f"window {target} position {parameters['position_pct']}"
|
|
if capability == "climate.set_power":
|
|
return f"ac {parameters['state']}"
|
|
if capability == "climate.set_ac":
|
|
return f"ac compressor {parameters['state']}"
|
|
if capability == "climate.set_fan_level":
|
|
return f"ac fan {parameters['level']}"
|
|
if capability == "climate.set_outlet_mode":
|
|
return f"ac mode {str(parameters['mode']).replace('_', '-')}"
|
|
if capability == "climate.set_temperature":
|
|
return f"ac temp {target} {parameters['temperature_c']}"
|
|
if capability == "climate.set_control_mode":
|
|
return f"ac control {parameters['mode']}"
|
|
if capability == "climate.set_cycle_mode":
|
|
return f"ac cycle {parameters['mode']}"
|
|
if capability == "door.set_rear_lock":
|
|
return f"door rear {parameters['state']}"
|
|
if capability == "outlet.set_position_percent":
|
|
return (
|
|
f"outlet {target} percent {parameters['horizontal_pct']} "
|
|
f"{parameters['vertical_pct']}"
|
|
)
|
|
if capability == "outlet.set_calibrated_position":
|
|
return (
|
|
f"outlet {target} position {parameters['horizontal_value']} "
|
|
f"{parameters['vertical_value']}"
|
|
)
|
|
if capability in {
|
|
"seat.set_driver_heating_native",
|
|
"seat.set_driver_ventilation_native",
|
|
}:
|
|
feature = (
|
|
"heat"
|
|
if capability == "seat.set_driver_heating_native"
|
|
else "vent"
|
|
)
|
|
setting = "off"
|
|
if parameters["state"] == "on":
|
|
setting = {1: "high", 2: "low"}[int(parameters.get("level", 2))]
|
|
return f"seat driver {feature} {setting}"
|
|
if capability == "seat.set_driver_massage_native":
|
|
action = str(parameters["action"])
|
|
if action == "off":
|
|
return "seat driver massage off"
|
|
if action == "strength":
|
|
strength = {1: "weak", 2: "medium", 3: "strong"}[
|
|
int(parameters["strength"])
|
|
]
|
|
return f"seat driver massage strength {strength}"
|
|
return f"seat driver massage mode {parameters['mode']}"
|
|
if capability == "seat.set_driver_axis_native":
|
|
if parameters["direction"] == "stop":
|
|
return f"seat driver {parameters['axis']} stop"
|
|
return (
|
|
f"seat driver {parameters['axis']} {parameters['direction']} "
|
|
f"{parameters.get('duration_ms', 500)}"
|
|
)
|
|
if capability == "seat.set_driver_position_native":
|
|
return f"seat driver position {parameters['slot']}"
|
|
if capability == "seat.recall_driver_memory_native":
|
|
return f"seat driver memory recall {parameters['slot']}"
|
|
if capability == "interior_light.set_indoor_power":
|
|
return f"light indoor {parameters['state']}"
|
|
if capability == "interior_light.set_reading_power":
|
|
return f"light reading all {parameters['state']}"
|
|
if capability == "ambient_light.set_power":
|
|
return f"light ambient {parameters['state']}"
|
|
if capability == "ambient_light.set_color_index":
|
|
return f"light ambient color {parameters['color_index']}"
|
|
if capability == "ambient_light.set_brightness_level":
|
|
return f"light ambient brightness {parameters['brightness_level']}"
|
|
if capability == "roof.set_shade_motion":
|
|
return f"shade {parameters['motion']}"
|
|
if capability == "roof.set_shade_position":
|
|
return f"shade position {parameters['position_pct']}"
|
|
return capability
|
|
|
|
@staticmethod
|
|
def _report(
|
|
function_config: dict[str, Any],
|
|
results: list[dict[str, Any]],
|
|
started_at: str,
|
|
mode: str,
|
|
) -> dict[str, Any]:
|
|
succeeded = all(item["status"] == "succeeded" for item in results)
|
|
request_id = function_config["metadata"]["request_id"]
|
|
domain = function_config["spec"]["domain"]
|
|
return {
|
|
"apiVersion": "cognitive-agent/v1alpha1",
|
|
"kind": "ExecutionReport",
|
|
"metadata": {
|
|
"id": f"report-native-{domain}-{request_id}",
|
|
"name": f"{domain.replace('_', ' ').title()} native CAN execution report",
|
|
"version": "1.0.0",
|
|
"request_id": request_id,
|
|
"labels": {
|
|
"simulation": "false" if mode == "vehicle" else "true",
|
|
"source_branch": "CAN_DEV",
|
|
},
|
|
},
|
|
"spec": {
|
|
"mode": mode,
|
|
"status": "succeeded" if succeeded else "failed",
|
|
"started_at": started_at,
|
|
"finished_at": _now(),
|
|
"results": results,
|
|
},
|
|
}
|
|
|
|
def _frame_specs(self, operation: dict[str, Any]) -> list[NativeFrameSpec]:
|
|
capability = str(operation["capability"])
|
|
if capability in UNSUPPORTED_CH1_DOOR_LOCK_CAPABILITIES:
|
|
raise NativeCanCompileError(
|
|
_unsupported_ch1_door_lock_message(capability)
|
|
)
|
|
target = operation["target"]["zone"]
|
|
parameters = operation["parameters"]
|
|
|
|
if capability == "door.set_rear_lock":
|
|
self._reject_unexpected_parameters(parameters, {"state"})
|
|
state = self._enum(parameters, "state", {"lock": 1, "unlock": 2})
|
|
payload = bytearray(8)
|
|
payload[0] = 0x0A
|
|
_set_bits(payload, 14, 2, state)
|
|
return [
|
|
self._classic(
|
|
0x32B,
|
|
payload,
|
|
1,
|
|
0,
|
|
(0x407,),
|
|
f"rear_lock:{parameters['state']}",
|
|
)
|
|
]
|
|
|
|
if capability in {"climate.set_power", "climate.set_ac"}:
|
|
state = self._enum(parameters, "state", {"on": 1, "off": 2})
|
|
payload = bytearray(8)
|
|
_set_bits(payload, 28, 2, state)
|
|
return [self._classic(0x1DE, payload, 3, 100, (0x404,), f"climate_power:{parameters['state']}")]
|
|
if capability == "climate.set_fan_level":
|
|
level = self._integer(parameters, "level", 1, 7)
|
|
payload = bytearray(8)
|
|
_set_bits(payload, 4, 4, level)
|
|
return [self._classic(0x1DE, payload, 3, 100, (0x404,), f"climate_fan:{level}")]
|
|
if capability == "climate.set_outlet_mode":
|
|
mode = self._enum(
|
|
parameters,
|
|
"mode",
|
|
{
|
|
"face": 1,
|
|
"face_feet": 2,
|
|
"feet": 3,
|
|
"feet_defrost": 4,
|
|
"defrost": 5,
|
|
"face_feet_defrost": 6,
|
|
"face_defrost": 7,
|
|
},
|
|
)
|
|
payload = bytearray(8)
|
|
_set_bits(payload, 0, 4, mode)
|
|
return [self._classic(0x1DE, payload, 3, 100, (0x404,), f"climate_mode:{mode}")]
|
|
if capability == "climate.set_control_mode":
|
|
mode = self._enum(parameters, "mode", {"manual": 1, "auto": 2})
|
|
payload = bytearray(8)
|
|
_set_bits(payload, 16, 2, mode)
|
|
return [self._classic(0x1DE, payload, 3, 100, (0x404,), f"climate_control:{mode}")]
|
|
if capability == "climate.set_cycle_mode":
|
|
self._reject_unexpected_parameters(parameters, {"mode"})
|
|
mode = self._enum(parameters, "mode", {"internal": 1, "external": 2})
|
|
payload = bytearray(8)
|
|
_set_bits(payload, 19, 2, mode)
|
|
return [
|
|
self._classic(
|
|
0x1DE,
|
|
payload,
|
|
3,
|
|
100,
|
|
(0x404,),
|
|
f"climate_cycle:{parameters['mode']}",
|
|
)
|
|
]
|
|
if capability == "climate.set_temperature":
|
|
temperature = self._integer(parameters, "temperature_c", 18, 32)
|
|
starts = {
|
|
"driver": (32,),
|
|
"passenger": (40,),
|
|
"rear": (48,),
|
|
"rear_row": (48,),
|
|
"rear_left": (48,),
|
|
"rear_right": (48,),
|
|
"vehicle": (32, 40),
|
|
"front_row": (32, 40),
|
|
}
|
|
if target not in starts:
|
|
raise NativeCanCompileError(
|
|
f"climate.set_temperature does not support target {target}"
|
|
)
|
|
specs = []
|
|
for start in starts[target]:
|
|
payload = bytearray(8)
|
|
_set_bits(payload, start, 8, temperature)
|
|
label = "driver" if start == 32 else "passenger" if start == 40 else "rear"
|
|
specs.append(
|
|
self._classic(
|
|
0x1DE,
|
|
payload,
|
|
1 if start == 48 else 3,
|
|
100,
|
|
(0x404,),
|
|
f"climate_temperature:{label}:{temperature}",
|
|
)
|
|
)
|
|
return specs
|
|
|
|
if capability == "window.set_motion":
|
|
motion = self._enum(parameters, "motion", {"open": 1, "close": 2, "stop": 3})
|
|
payload = self._window_payload(target, motion)
|
|
return [self._fd(0x431, payload, 3, 100, (0x388,), f"window_motion:{target}:{parameters['motion']}")]
|
|
if capability == "window.set_position":
|
|
position = self._integer(parameters, "position_pct", 0, 100)
|
|
if target not in {"front_left", "rear_left", "front_right", "rear_right"}:
|
|
raise NativeCanCompileError("absolute window position requires one window")
|
|
return [
|
|
NativeFrameSpec(
|
|
frame_id=0x431,
|
|
payload=self._window_payload(target, 3),
|
|
is_fd=True,
|
|
bitrate_switch=True,
|
|
feedback_ids=(0x388,),
|
|
expected_feedback=f"window_position:{target}:{position}",
|
|
special_handler="window_position",
|
|
special_parameters={"target": target, "position_pct": position},
|
|
)
|
|
]
|
|
|
|
if capability in {
|
|
"seat.set_driver_heating_native",
|
|
"seat.set_driver_ventilation_native",
|
|
}:
|
|
self._require_driver_seat_target(target)
|
|
return [self._driver_seat_thermal_frame(capability, parameters)]
|
|
if capability == "seat.set_driver_massage_native":
|
|
self._require_driver_seat_target(target)
|
|
return [self._driver_seat_massage_frame(parameters)]
|
|
if capability == "seat.set_driver_axis_native":
|
|
self._require_driver_seat_target(target)
|
|
return [self._driver_seat_axis_frame(parameters)]
|
|
if capability in {
|
|
"seat.set_driver_position_native",
|
|
"seat.recall_driver_memory_native",
|
|
}:
|
|
self._require_driver_seat_target(target)
|
|
return [self._driver_seat_memory_frame(parameters)]
|
|
|
|
if capability in {"outlet.set_position_percent", "outlet.set_calibrated_position"}:
|
|
return [self._outlet_frame(capability, target, parameters)]
|
|
|
|
if capability == "interior_light.set_indoor_power":
|
|
return [self._all_reading_light_frame(parameters, "indoor_light")]
|
|
if capability == "interior_light.set_reading_power":
|
|
if target != "all":
|
|
raise NativeCanCompileError(
|
|
"only the all-reading-lights target is verified on CH1"
|
|
)
|
|
return [self._all_reading_light_frame(parameters, "indoor_light")]
|
|
|
|
if capability == "ambient_light.set_power":
|
|
self._reject_unexpected_parameters(parameters, {"state"})
|
|
state = str(parameters.get("state"))
|
|
self._enum(parameters, "state", {"off": 0, "on": 1})
|
|
payload = bytes(
|
|
(0x00, 0x36 if state == "on" else 0x31, 0x00, 0x75, 0x00, 0x80, 0x00, 0x00)
|
|
)
|
|
return [self._fd(0x3FC, payload, 3, 100, (0x42E,), f"ambient_power:{state}")]
|
|
if capability == "ambient_light.set_color_index":
|
|
self._reject_unexpected_parameters(parameters, {"color_index"})
|
|
color = self._integer(parameters, "color_index", 1, 127)
|
|
payload = bytes((color, 0x30, 0x00, 0x75, 0x00, 0x00, 0x00, 0x00))
|
|
return [self._fd(0x3FC, payload, 3, 100, (0x42E,), f"ambient_color:{color}")]
|
|
if capability == "ambient_light.set_brightness_level":
|
|
self._reject_unexpected_parameters(parameters, {"brightness_level"})
|
|
level = self._integer(parameters, "brightness_level", 1, 5)
|
|
payload = bytes((0x00, 0x31 + level, 0x00, 0x75, 0x00, 0x00, 0x00, 0x00))
|
|
return [self._fd(0x3FC, payload, 3, 100, (0x42E,), f"ambient_brightness:{level}")]
|
|
|
|
if capability == "roof.set_shade_motion":
|
|
motion = self._enum(parameters, "motion", {"open": 1, "close": 2, "half": 3, "stop": 4})
|
|
return [self._shade_preset_frame(motion)]
|
|
if capability == "roof.set_shade_position":
|
|
position = self._integer(parameters, "position_pct", 0, 100)
|
|
presets = {0: 2, 50: 3, 100: 1}
|
|
if position in presets:
|
|
return [self._shade_preset_frame(presets[position])]
|
|
return [self._shade_position_frame(position)]
|
|
|
|
raise NativeCanCompileError(f"CAN_DEV does not implement {capability}")
|
|
|
|
@staticmethod
|
|
def _classic(
|
|
frame_id: int,
|
|
payload: bytes | bytearray,
|
|
repeat: int = 1,
|
|
period_ms: int = 0,
|
|
feedback_ids: tuple[int, ...] = (),
|
|
expected: str = "",
|
|
) -> NativeFrameSpec:
|
|
return NativeFrameSpec(
|
|
frame_id,
|
|
bytes(payload),
|
|
repeat_count=repeat,
|
|
period_ms=period_ms,
|
|
feedback_ids=feedback_ids,
|
|
expected_feedback=expected,
|
|
)
|
|
|
|
@staticmethod
|
|
def _fd(
|
|
frame_id: int,
|
|
payload: bytes | bytearray,
|
|
repeat: int = 1,
|
|
period_ms: int = 0,
|
|
feedback_ids: tuple[int, ...] = (),
|
|
expected: str = "",
|
|
) -> NativeFrameSpec:
|
|
return NativeFrameSpec(
|
|
frame_id,
|
|
bytes(payload),
|
|
is_fd=True,
|
|
bitrate_switch=True,
|
|
repeat_count=repeat,
|
|
period_ms=period_ms,
|
|
feedback_ids=feedback_ids,
|
|
expected_feedback=expected,
|
|
)
|
|
|
|
@staticmethod
|
|
def _enum(parameters: dict[str, Any], name: str, values: dict[str, int]) -> int:
|
|
value = str(parameters.get(name))
|
|
try:
|
|
return values[value]
|
|
except KeyError as exc:
|
|
raise NativeCanCompileError(
|
|
f"{name} must be one of {', '.join(values)}"
|
|
) from exc
|
|
|
|
@staticmethod
|
|
def _integer(
|
|
parameters: dict[str, Any], name: str, minimum: int, maximum: int
|
|
) -> int:
|
|
value = parameters.get(name)
|
|
if isinstance(value, bool) or not isinstance(value, (int, float)):
|
|
raise NativeCanCompileError(f"{name} must be an integer")
|
|
integer = int(value)
|
|
if integer != value or not minimum <= integer <= maximum:
|
|
raise NativeCanCompileError(
|
|
f"{name} must be between {minimum} and {maximum}"
|
|
)
|
|
return integer
|
|
|
|
@staticmethod
|
|
def _window_payload(target: str, motion: int) -> bytes:
|
|
masks = {
|
|
"front_left": 0b0001,
|
|
"rear_left": 0b0010,
|
|
"front_right": 0b0100,
|
|
"rear_right": 0b1000,
|
|
"front": 0b0101,
|
|
"rear": 0b1010,
|
|
"all": 0b1111,
|
|
}
|
|
try:
|
|
mask = masks[target]
|
|
except KeyError as exc:
|
|
raise NativeCanCompileError(f"unsupported window target {target}") from exc
|
|
starts = (48, 56, 51, 59)
|
|
payload = bytearray(8)
|
|
for index, start in enumerate(starts):
|
|
if mask & (1 << index):
|
|
_set_bits(payload, start, 3, motion)
|
|
return bytes(payload)
|
|
|
|
def _outlet_frame(
|
|
self, capability: str, target: str, parameters: dict[str, Any]
|
|
) -> NativeFrameSpec:
|
|
ranges = self.calibration["air_outlet"]["ranges"]
|
|
if target not in ranges:
|
|
raise NativeCanCompileError(f"unsupported outlet target {target}")
|
|
if capability == "outlet.set_position_percent":
|
|
horizontal_pct = self._integer(parameters, "horizontal_pct", 0, 100)
|
|
vertical_pct = self._integer(parameters, "vertical_pct", 0, 100)
|
|
horizontal_range = ranges[target]["horizontal"]
|
|
vertical_range = ranges[target]["vertical"]
|
|
horizontal = horizontal_range[0] + (
|
|
(horizontal_range[1] - horizontal_range[0]) * horizontal_pct + 50
|
|
) // 100
|
|
vertical = vertical_range[0] + (
|
|
(vertical_range[1] - vertical_range[0]) * vertical_pct + 50
|
|
) // 100
|
|
else:
|
|
horizontal = self._integer(parameters, "horizontal_value", -128, 126)
|
|
vertical = self._integer(parameters, "vertical_value", -128, 126)
|
|
horizontal_range = ranges[target]["horizontal"]
|
|
vertical_range = ranges[target]["vertical"]
|
|
if not horizontal_range[0] <= horizontal <= horizontal_range[1]:
|
|
raise NativeCanCompileError("horizontal_value exceeds the calibrated range")
|
|
if not vertical_range[0] <= vertical <= vertical_range[1]:
|
|
raise NativeCanCompileError("vertical_value exceeds the calibrated range")
|
|
|
|
side, position = target.split("_", 1)
|
|
sub_id = 0x02 if side == "driver" else 0x03
|
|
starts = {
|
|
"left": (24, 8),
|
|
"right": (32, 16),
|
|
}
|
|
horizontal_start, vertical_start = starts[position]
|
|
payload = bytearray((sub_id, 0xFF, 0xFF, 0xFF, 0xFF, 0, 0, 0))
|
|
if sub_id == 0x02:
|
|
payload[6] = 0xFF
|
|
payload[7] = 0xFF
|
|
_set_bits(payload, horizontal_start, 8, horizontal + 128)
|
|
_set_bits(payload, vertical_start, 8, vertical + 128)
|
|
return self._fd(
|
|
0x4EF,
|
|
payload,
|
|
3,
|
|
100,
|
|
(0x2F5, 0x37B),
|
|
f"outlet:{target}:{horizontal}:{vertical}",
|
|
)
|
|
|
|
@staticmethod
|
|
def _require_driver_seat_target(target: str) -> None:
|
|
if target not in DRIVER_SEAT_TARGETS:
|
|
raise NativeCanCompileError(
|
|
"CH1 driver-seat controls only allow driver/front_left; "
|
|
f"refusing target {target}"
|
|
)
|
|
|
|
@staticmethod
|
|
def _reject_unexpected_parameters(
|
|
parameters: dict[str, Any], allowed: set[str]
|
|
) -> None:
|
|
unexpected = set(parameters) - allowed
|
|
if unexpected:
|
|
raise NativeCanCompileError(
|
|
f"unsupported parameter(s): {', '.join(sorted(unexpected))}"
|
|
)
|
|
|
|
def _driver_seat_thermal_frame(
|
|
self, capability: str, parameters: dict[str, Any]
|
|
) -> NativeFrameSpec:
|
|
self._reject_unexpected_parameters(parameters, {"state", "level"})
|
|
state = str(parameters.get("state"))
|
|
if state not in {"on", "off"}:
|
|
raise NativeCanCompileError("state must be on or off")
|
|
if state == "off":
|
|
if parameters.get("level") is not None:
|
|
raise NativeCanCompileError("level must be omitted when state is off")
|
|
control_value = 3
|
|
feedback_value = 1
|
|
else:
|
|
level = parameters.get("level", 2)
|
|
parameters_with_default = {**parameters, "level": level}
|
|
control_value = self._integer(parameters_with_default, "level", 1, 2)
|
|
feedback_value = 3 if control_value == 1 else 2
|
|
|
|
feature = (
|
|
"heating"
|
|
if capability == "seat.set_driver_heating_native"
|
|
else "ventilation"
|
|
)
|
|
payload = bytearray(8)
|
|
payload[1] = (
|
|
control_value << 4 if feature == "heating" else control_value
|
|
)
|
|
return self._fd(
|
|
0x431,
|
|
payload,
|
|
3,
|
|
100,
|
|
(0x4FA,),
|
|
f"driver_seat_{feature}:{feedback_value}",
|
|
)
|
|
|
|
def _driver_seat_massage_frame(
|
|
self, parameters: dict[str, Any]
|
|
) -> NativeFrameSpec:
|
|
action = str(parameters.get("action"))
|
|
payload = bytearray(8)
|
|
payload[0] = 0x12
|
|
if action == "off":
|
|
self._reject_unexpected_parameters(parameters, {"action"})
|
|
payload[3] = 1
|
|
expected = "driver_seat_massage_mode:1"
|
|
elif action == "strength":
|
|
self._reject_unexpected_parameters(parameters, {"action", "strength"})
|
|
strength = self._integer(parameters, "strength", 1, 3)
|
|
payload[1] = strength
|
|
expected = f"driver_seat_massage_strength:{8 + strength}"
|
|
elif action == "mode":
|
|
self._reject_unexpected_parameters(parameters, {"action", "mode"})
|
|
mode = self._enum(
|
|
parameters,
|
|
"mode",
|
|
{
|
|
"wave": 2,
|
|
"pulse": 3,
|
|
"stretch": 4,
|
|
"slack": 5,
|
|
"relax": 6,
|
|
},
|
|
)
|
|
payload[3] = mode
|
|
expected = f"driver_seat_massage_mode:{mode}"
|
|
else:
|
|
raise NativeCanCompileError("action must be off, strength, or mode")
|
|
return self._fd(0x4C1, payload, 3, 100, (0x4FA,), expected)
|
|
|
|
def _driver_seat_axis_frame(
|
|
self, parameters: dict[str, Any]
|
|
) -> NativeFrameSpec:
|
|
self._reject_unexpected_parameters(
|
|
parameters, {"axis", "direction", "duration_ms"}
|
|
)
|
|
axis = str(parameters.get("axis"))
|
|
starts = {
|
|
"horizontal": 8,
|
|
"height": 10,
|
|
"backrest": 12,
|
|
"basin": 14,
|
|
}
|
|
if axis not in starts:
|
|
raise NativeCanCompileError(
|
|
"axis must be horizontal, height, backrest, or basin"
|
|
)
|
|
direction = str(parameters.get("direction"))
|
|
allowed_directions = (
|
|
{"forward": 1, "backward": 2, "stop": 0}
|
|
if axis in {"horizontal", "backrest"}
|
|
else {"up": 1, "down": 2, "stop": 0}
|
|
)
|
|
if direction not in allowed_directions:
|
|
raise NativeCanCompileError(
|
|
f"direction {direction} is not valid for {axis}"
|
|
)
|
|
duration_parameters = {
|
|
**parameters,
|
|
"duration_ms": parameters.get("duration_ms", 500),
|
|
}
|
|
duration_ms = self._integer(
|
|
duration_parameters, "duration_ms", 100, 2000
|
|
)
|
|
|
|
stop_payload = bytearray(8)
|
|
stop_payload[0] = 0x05
|
|
payload = bytearray(stop_payload)
|
|
_set_bits(payload, starts[axis], 2, allowed_directions[direction])
|
|
expected = f"driver_seat_axis:{axis}:{direction}"
|
|
if direction == "stop":
|
|
return self._fd(
|
|
0x4C1, stop_payload, 3, 100, (0x494,), expected
|
|
)
|
|
|
|
repeat_count = (duration_ms + 99) // 100
|
|
return NativeFrameSpec(
|
|
frame_id=0x4C1,
|
|
payload=bytes(payload),
|
|
is_fd=True,
|
|
bitrate_switch=True,
|
|
repeat_count=repeat_count,
|
|
period_ms=100,
|
|
feedback_ids=(0x494,),
|
|
expected_feedback=expected,
|
|
special_handler="driver_seat_axis",
|
|
special_parameters={
|
|
"duration_ms": duration_ms,
|
|
"final_action_hold_ms": 100,
|
|
"stop_data_hex": " ".join(
|
|
f"{value:02X}" for value in stop_payload
|
|
),
|
|
"stop_repeat_count": 3,
|
|
"stop_period_ms": 100,
|
|
},
|
|
)
|
|
|
|
def _driver_seat_memory_frame(
|
|
self, parameters: dict[str, Any]
|
|
) -> NativeFrameSpec:
|
|
"""Compile one learned PAD memory slot without inventing payload bytes."""
|
|
|
|
self._reject_unexpected_parameters(parameters, {"slot"})
|
|
slot = self._integer(parameters, "slot", 1, 3)
|
|
configured = os.getenv(
|
|
"COGNITIVE_AGENT_DRIVER_SEAT_MEMORY_PATH", ""
|
|
).strip()
|
|
path = (
|
|
Path(configured).expanduser()
|
|
if configured
|
|
else Path("/home/orin/project511/project-code/canfd/driver_seat_memory.json")
|
|
)
|
|
try:
|
|
document = json.loads(path.read_text(encoding="utf-8"))
|
|
except (OSError, json.JSONDecodeError) as exc:
|
|
raise NativeCanCompileError(
|
|
f"cannot load learned driver-seat memory file {path}: {exc}"
|
|
) from exc
|
|
if document.get("version") != 1 or not isinstance(
|
|
document.get("positions"), dict
|
|
):
|
|
raise NativeCanCompileError(
|
|
"driver-seat memory file must use version 1 with positions"
|
|
)
|
|
learned = document["positions"].get(str(slot))
|
|
if not isinstance(learned, dict):
|
|
raise NativeCanCompileError(
|
|
f"driver-seat memory slot {slot} has not been learned from PAD"
|
|
)
|
|
|
|
def learned_payload(name: str, sub_id: int) -> bytes:
|
|
raw = learned.get(name)
|
|
try:
|
|
payload = bytes.fromhex(str(raw))
|
|
except ValueError as exc:
|
|
raise NativeCanCompileError(
|
|
f"driver-seat memory slot {slot} has invalid {name}"
|
|
) from exc
|
|
if len(payload) != 8 or payload[0] != sub_id:
|
|
raise NativeCanCompileError(
|
|
f"driver-seat memory slot {slot} {name} must be an 8-byte "
|
|
f"0x{sub_id:02X} learned frame"
|
|
)
|
|
return payload
|
|
|
|
payload_15 = learned_payload("payload_15", 0x15)
|
|
payload_16 = learned_payload("payload_16", 0x16)
|
|
sequence = [
|
|
value.hex(" ").upper()
|
|
for _ in range(10)
|
|
for value in (payload_16, payload_15)
|
|
]
|
|
return NativeFrameSpec(
|
|
frame_id=0x4C1,
|
|
payload=payload_16,
|
|
is_fd=True,
|
|
bitrate_switch=True,
|
|
feedback_ids=(0x494,),
|
|
expected_feedback=f"driver_seat_memory_status:{slot}",
|
|
special_handler="driver_seat_memory_recall",
|
|
special_parameters={
|
|
"slot": slot,
|
|
"fresh_status_timeout_ms": 2_500,
|
|
"post_status_timeout_ms": 1_200,
|
|
"period_ms": 50,
|
|
"frame_sequence": sequence,
|
|
},
|
|
)
|
|
|
|
def _all_reading_light_frame(
|
|
self, parameters: dict[str, Any], feedback_family: str
|
|
) -> NativeFrameSpec:
|
|
self._reject_unexpected_parameters(parameters, {"state"})
|
|
state = self._enum(parameters, "state", {"off": 1, "on": 2})
|
|
payload = bytearray((0, 0, 3, 0xFF, 0xFF, 0, 0, 0))
|
|
_set_bits(payload, 50, 2, state)
|
|
return self._fd(
|
|
0x4F5,
|
|
payload,
|
|
3,
|
|
100,
|
|
(0x42E,),
|
|
f"{feedback_family}:{parameters['state']}",
|
|
)
|
|
|
|
@staticmethod
|
|
def _shade_payload(motion: int) -> bytes:
|
|
payload = bytearray(8)
|
|
_set_bits(payload, 4, 4, motion)
|
|
return bytes(payload)
|
|
|
|
def _shade_preset_frame(self, motion: int) -> NativeFrameSpec:
|
|
target = {1: 100, 2: 0, 3: 50}.get(motion)
|
|
expected = "shade_stopped" if motion == 4 else f"shade_position:{target}"
|
|
return NativeFrameSpec(
|
|
frame_id=0x431,
|
|
payload=self._shade_payload(motion),
|
|
feedback_ids=(0x41A,),
|
|
expected_feedback=expected,
|
|
special_handler="shade_preset",
|
|
special_parameters={
|
|
"motion": motion,
|
|
"target_position_pct": target,
|
|
"motion_timeout_ms": 15_000,
|
|
"tolerance_pct": 2 if target in {0, 100} else 3,
|
|
},
|
|
)
|
|
|
|
def _shade_position_frame(self, position: int) -> NativeFrameSpec:
|
|
return NativeFrameSpec(
|
|
frame_id=0x431,
|
|
payload=self._shade_payload(4),
|
|
feedback_ids=(0x41A,),
|
|
expected_feedback=f"shade_position:{position}",
|
|
special_handler="shade_position",
|
|
special_parameters={
|
|
"position_pct": position,
|
|
"fresh_feedback_timeout_ms": 2_500,
|
|
"motion_timeout_ms": 15_000,
|
|
"tolerance_pct": 2,
|
|
"open_data_hex": "10 00 00 00 00 00 00 00",
|
|
"close_data_hex": "20 00 00 00 00 00 00 00",
|
|
"stop_data_hex": "40 00 00 00 00 00 00 00",
|
|
},
|
|
)
|
|
|
|
|
|
class NativeCanExecutor:
|
|
"""Submit compiled vehicle frames and capture post-command feedback."""
|
|
|
|
def __init__(
|
|
self,
|
|
bus_factory: Callable[[], CanBus],
|
|
*,
|
|
feedback_timeout_ms: int = 1_200,
|
|
feedback_required: bool = True,
|
|
):
|
|
self.bus_factory = bus_factory
|
|
self.feedback_timeout_ms = feedback_timeout_ms
|
|
self.feedback_required = feedback_required
|
|
|
|
def execute(
|
|
self,
|
|
runtime: NativeCanRuntime,
|
|
command_plan: dict[str, Any],
|
|
function_config: dict[str, Any],
|
|
) -> dict[str, Any]:
|
|
started_at = _now()
|
|
operations = {
|
|
operation["id"]: operation
|
|
for operation in function_config["spec"]["operations"]
|
|
}
|
|
results: list[dict[str, Any]] = []
|
|
bus = self.bus_factory()
|
|
try:
|
|
for command in command_plan["spec"]["commands"]:
|
|
operation = operations[command["operation_ref"]]
|
|
publication_start = len(
|
|
getattr(bus, "published_messages", ())
|
|
)
|
|
try:
|
|
set_debug_command = getattr(bus, "set_debug_command", None)
|
|
if callable(set_debug_command):
|
|
set_debug_command(command["signals"].get("debug_command", ""))
|
|
raw_feedback_ids = str(
|
|
command["signals"].get("feedback_ids", "")
|
|
).strip()
|
|
expected_feedback = str(
|
|
command["signals"].get("expected_feedback", "")
|
|
).strip()
|
|
if not raw_feedback_ids or not expected_feedback:
|
|
raise RuntimeError(
|
|
"physical CH1 execution requires an authoritative "
|
|
"feedback ID and a target-value match contract"
|
|
)
|
|
special_handler = command["signals"].get("special_handler")
|
|
feedback_required = self.feedback_required
|
|
if not feedback_required and special_handler in {
|
|
None,
|
|
"shade_preset",
|
|
}:
|
|
# Wireless MQTT uses the state topic as an asynchronous
|
|
# source of truth. Fixed frames and self-terminating
|
|
# shade presets must not hold the HTTP request open
|
|
# while waiting for the same state stream synchronously.
|
|
sent_frames = self._send_command(bus, command)
|
|
feedback = None
|
|
elif special_handler == "window_position":
|
|
sent_frames, feedback = self._execute_window_position(bus, command)
|
|
elif special_handler == "driver_seat_axis":
|
|
sent_frames, feedback = self._execute_driver_seat_axis(
|
|
bus,
|
|
command,
|
|
wait_for_feedback=feedback_required,
|
|
)
|
|
elif special_handler == "driver_seat_memory_recall":
|
|
sent_frames, feedback = self._execute_driver_seat_memory(
|
|
bus,
|
|
command,
|
|
wait_for_completion=feedback_required,
|
|
)
|
|
elif special_handler == "shade_preset":
|
|
sent_frames, feedback = self._execute_shade_preset(
|
|
bus, command
|
|
)
|
|
elif special_handler == "shade_position":
|
|
sent_frames, feedback = self._execute_shade_position(
|
|
bus, command
|
|
)
|
|
else:
|
|
sent_frames = self._send_command(bus, command)
|
|
feedback = self._wait_feedback(bus, command)
|
|
feedback_match = self._feedback_matches(command, feedback)
|
|
verified = feedback is not None and feedback_match is True
|
|
delivered = bool(sent_frames)
|
|
succeeded = verified or (delivered and not feedback_required)
|
|
physical_channel = (
|
|
feedback.channel
|
|
if feedback is not None
|
|
else sent_frames[-1].channel
|
|
if sent_frames
|
|
else None
|
|
)
|
|
mqtt_messages = list(
|
|
getattr(bus, "published_messages", ())[
|
|
publication_start:
|
|
]
|
|
)
|
|
delivery_status = "submitted"
|
|
if not feedback_required and delivered:
|
|
delivery_status = (
|
|
"published"
|
|
if mqtt_messages
|
|
and all(
|
|
message.get("delivery_status") == "published"
|
|
for message in mqtt_messages
|
|
)
|
|
else "queued"
|
|
)
|
|
results.append(
|
|
{
|
|
"ref": operation["id"],
|
|
"domain": function_config["spec"]["domain"],
|
|
"status": "succeeded" if succeeded else "failed",
|
|
"message": (
|
|
"MQTT 白名单控制帧已由 Broker 确认；车辆效果以状态话题回读为准。"
|
|
if not feedback_required and delivery_status == "published"
|
|
else "MQTT 白名单控制帧已进入客户端发布队列；车辆效果以状态话题回读为准。"
|
|
if not feedback_required
|
|
else "CAN 盒已提交车端控制帧并观察到匹配反馈。"
|
|
if feedback_match is True
|
|
else "CAN 盒已提交车端控制帧，但反馈字段与目标值不一致。"
|
|
if feedback_match is False
|
|
else "CAN 盒已提交车端控制帧并观察到状态帧，但该帧没有可证明动作完成的字段。"
|
|
if feedback is not None
|
|
else "CAN 盒已提交车端控制帧；当前反馈窗口未观察到对应状态帧。"
|
|
),
|
|
"telemetry": {
|
|
"target": operation["target"]["zone"],
|
|
"requested_parameters": operation["parameters"],
|
|
"command_frame": runtime.command_frame_public(command),
|
|
"sent_frames": [frame.public() for frame in sent_frames],
|
|
"feedback_frame": feedback.public() if feedback else None,
|
|
"feedback_observed": feedback is not None,
|
|
"feedback_match": feedback_match,
|
|
"feedback_state": self._feedback_state(feedback),
|
|
"feedback_required": feedback_required,
|
|
"delivery_status": delivery_status,
|
|
"physical_effect_confirmed": verified,
|
|
"control_gate": (
|
|
"compiled_action_and_frame_whitelist"
|
|
if not feedback_required
|
|
else "stationary_vehicle_and_feedback"
|
|
),
|
|
"channel": physical_channel,
|
|
"simulated": False,
|
|
**(
|
|
{"mqtt_messages": mqtt_messages}
|
|
if mqtt_messages
|
|
else {}
|
|
),
|
|
},
|
|
}
|
|
)
|
|
except Exception as exc: # noqa: BLE001 - normalize adapter faults
|
|
results.append(
|
|
{
|
|
"ref": operation["id"],
|
|
"domain": function_config["spec"]["domain"],
|
|
"status": "failed",
|
|
"message": str(exc),
|
|
"telemetry": {
|
|
"target": operation["target"]["zone"],
|
|
"requested_parameters": operation["parameters"],
|
|
"command_frame": runtime.command_frame_public(command),
|
|
"channel": 1,
|
|
"simulated": False,
|
|
},
|
|
}
|
|
)
|
|
break
|
|
finally:
|
|
bus.close()
|
|
return runtime._report(function_config, results, started_at, "vehicle")
|
|
|
|
@staticmethod
|
|
def _send_command(bus: CanBus, command: dict[str, Any]) -> list[CanFrame]:
|
|
signals = command["signals"]
|
|
frame_id = int(str(signals["frame_id"]), 0)
|
|
payload = bytes.fromhex(str(signals["data_hex"]))
|
|
repeat = command.get("repeat", {"count": 1, "period_ms": 0})
|
|
sent: list[CanFrame] = []
|
|
for index in range(int(repeat["count"])):
|
|
sent.append(
|
|
bus.send(
|
|
frame_id,
|
|
payload,
|
|
is_fd=bool(signals["is_fd"]),
|
|
bitrate_switch=bool(signals["bitrate_switch"]),
|
|
)
|
|
)
|
|
if index + 1 < int(repeat["count"]):
|
|
time.sleep(int(repeat["period_ms"]) / 1000)
|
|
return sent
|
|
|
|
def _execute_driver_seat_axis(
|
|
self,
|
|
bus: CanBus,
|
|
command: dict[str, Any],
|
|
*,
|
|
wait_for_feedback: bool = True,
|
|
) -> tuple[list[CanFrame], CanFrame | None]:
|
|
"""Send bounded motion and attempt all three stop frames in a finally path."""
|
|
|
|
signals = command["signals"]
|
|
special = json.loads(str(signals["special_parameters"]))
|
|
frame_id = int(str(signals["frame_id"]), 0)
|
|
action_payload = bytes.fromhex(str(signals["data_hex"]))
|
|
stop_payload = bytes.fromhex(str(special["stop_data_hex"]))
|
|
repeat = command.get("repeat", {"count": 1, "period_ms": 0})
|
|
sent: list[CanFrame] = []
|
|
action_error: Exception | None = None
|
|
stop_errors: list[Exception] = []
|
|
try:
|
|
for index in range(int(repeat["count"])):
|
|
sent.append(
|
|
bus.send(
|
|
frame_id,
|
|
action_payload,
|
|
is_fd=True,
|
|
bitrate_switch=True,
|
|
)
|
|
)
|
|
if index + 1 < int(repeat["count"]):
|
|
time.sleep(int(repeat["period_ms"]) / 1000)
|
|
time.sleep(int(special["final_action_hold_ms"]) / 1000)
|
|
except Exception as exc: # noqa: BLE001 - stop is mandatory after adapter faults
|
|
action_error = exc
|
|
finally:
|
|
for index in range(int(special["stop_repeat_count"])):
|
|
try:
|
|
sent.append(
|
|
bus.send(
|
|
frame_id,
|
|
stop_payload,
|
|
is_fd=True,
|
|
bitrate_switch=True,
|
|
)
|
|
)
|
|
except Exception as exc: # noqa: BLE001 - attempt remaining stop frames
|
|
stop_errors.append(exc)
|
|
if index + 1 < int(special["stop_repeat_count"]):
|
|
time.sleep(int(special["stop_period_ms"]) / 1000)
|
|
|
|
if action_error is not None or stop_errors:
|
|
details = []
|
|
if action_error is not None:
|
|
details.append(f"motion send failed: {action_error}")
|
|
if stop_errors:
|
|
details.append(
|
|
f"{len(stop_errors)}/3 forced-stop frame(s) failed"
|
|
)
|
|
raise RuntimeError("; ".join(details))
|
|
return sent, self._wait_feedback(bus, command) if wait_for_feedback else None
|
|
|
|
def _execute_driver_seat_memory(
|
|
self,
|
|
bus: CanBus,
|
|
command: dict[str, Any],
|
|
*,
|
|
wait_for_completion: bool = True,
|
|
) -> tuple[list[CanFrame], CanFrame | None]:
|
|
"""Require fresh initialization, then replay the learned 16/15 sequence."""
|
|
|
|
signals = command["signals"]
|
|
special = json.loads(str(signals["special_parameters"]))
|
|
before = self._wait_for_id(
|
|
bus, 0x494, int(special["fresh_status_timeout_ms"])
|
|
)
|
|
initialized = (
|
|
decode_feedback(before)
|
|
.get("actuator_state", {})
|
|
.get("driver", {})
|
|
.get("position_feedback", {})
|
|
.get("initialized", {})
|
|
if before is not None
|
|
else {}
|
|
)
|
|
axes = ("horizontal", "backrest", "height", "basin")
|
|
if not all(initialized.get(axis) is True for axis in axes):
|
|
raise RuntimeError(
|
|
"no fresh 0x494 proving all four driver-seat axes initialized; "
|
|
"refusing memory recall"
|
|
)
|
|
|
|
frame_id = int(str(signals["frame_id"]), 0)
|
|
sequence = [
|
|
bytes.fromhex(str(value)) for value in special["frame_sequence"]
|
|
]
|
|
if (
|
|
len(sequence) != 20
|
|
or any(len(payload) != 8 for payload in sequence)
|
|
or any(
|
|
payload[0] != (0x16 if index % 2 == 0 else 0x15)
|
|
for index, payload in enumerate(sequence)
|
|
)
|
|
):
|
|
raise RuntimeError("compiled driver-seat memory sequence is invalid")
|
|
sent: list[CanFrame] = []
|
|
for index, payload in enumerate(sequence):
|
|
sent.append(
|
|
bus.send(
|
|
frame_id,
|
|
payload,
|
|
is_fd=True,
|
|
bitrate_switch=True,
|
|
)
|
|
)
|
|
if index + 1 < len(sequence):
|
|
time.sleep(int(special["period_ms"]) / 1_000)
|
|
feedback = (
|
|
self._wait_for_id(
|
|
bus, 0x494, int(special["post_status_timeout_ms"])
|
|
)
|
|
if wait_for_completion
|
|
else None
|
|
)
|
|
return sent, feedback
|
|
|
|
def _execute_shade_preset(
|
|
self, bus: CanBus, command: dict[str, Any]
|
|
) -> tuple[list[CanFrame], CanFrame | None]:
|
|
"""Submit a preset and prove the shade is stopped at its target."""
|
|
|
|
special = json.loads(str(command["signals"]["special_parameters"]))
|
|
sent = self._send_command(bus, command)
|
|
feedback = self._wait_matching_feedback(
|
|
bus,
|
|
command,
|
|
timeout_ms=int(special["motion_timeout_ms"]),
|
|
)
|
|
if feedback is None:
|
|
return sent, None
|
|
roof = decode_feedback(feedback).get("domain_state", {}).get("roof", {})
|
|
target = special.get("target_position_pct")
|
|
if roof.get("motion_state") == "stopped" or target is None:
|
|
return sent, feedback
|
|
|
|
sent.append(bus.send(0x431, NativeCanRuntime._shade_payload(4)))
|
|
final = self._wait_for_shade_stopped(
|
|
bus,
|
|
target=int(target),
|
|
tolerance=int(special["tolerance_pct"]),
|
|
timeout_ms=self.feedback_timeout_ms,
|
|
)
|
|
if final is None:
|
|
raise RuntimeError(
|
|
"shade reached the preset position, but STOP state was not confirmed"
|
|
)
|
|
return sent, final
|
|
|
|
def _execute_shade_position(
|
|
self, bus: CanBus, command: dict[str, Any]
|
|
) -> tuple[list[CanFrame], CanFrame | None]:
|
|
"""Drive toward a non-preset position and always issue a STOP frame."""
|
|
|
|
special = json.loads(str(command["signals"]["special_parameters"]))
|
|
target = int(special["position_pct"])
|
|
tolerance = int(special["tolerance_pct"])
|
|
before = self._wait_for_id(
|
|
bus, 0x41A, int(special["fresh_feedback_timeout_ms"])
|
|
)
|
|
before_state = decode_feedback(before) if before is not None else {}
|
|
roof = before_state.get("domain_state", {}).get("roof", {})
|
|
current = roof.get("shade_position_pct")
|
|
if not isinstance(current, int) or not 0 <= current <= 100:
|
|
raise RuntimeError(
|
|
"no fresh valid 0x41A shade position; refusing blind motion"
|
|
)
|
|
if roof.get("thermal_protection") is True:
|
|
raise RuntimeError(
|
|
"0x41A reports shade thermal protection; refusing motion"
|
|
)
|
|
if abs(current - target) <= tolerance:
|
|
if roof.get("motion_state") == "stopped":
|
|
return [], before
|
|
stopped = bus.send(
|
|
0x431, bytes.fromhex(str(special["stop_data_hex"]))
|
|
)
|
|
final = self._wait_for_shade_stopped(
|
|
bus,
|
|
target=target,
|
|
tolerance=tolerance,
|
|
timeout_ms=self.feedback_timeout_ms,
|
|
)
|
|
if final is None:
|
|
raise RuntimeError(
|
|
"shade is at the target but STOP state was not confirmed"
|
|
)
|
|
return [stopped], final
|
|
|
|
moving_open = target > current
|
|
start_payload = bytes.fromhex(
|
|
str(
|
|
special[
|
|
"open_data_hex" if moving_open else "close_data_hex"
|
|
]
|
|
)
|
|
)
|
|
stop_payload = bytes.fromhex(str(special["stop_data_hex"]))
|
|
sent: list[CanFrame] = []
|
|
reached: CanFrame | None = None
|
|
action_error: Exception | None = None
|
|
stop_error: Exception | None = None
|
|
try:
|
|
sent.append(bus.send(0x431, start_payload))
|
|
deadline = time.monotonic() + int(special["motion_timeout_ms"]) / 1_000
|
|
while time.monotonic() < deadline:
|
|
remaining_ms = max(1, int((deadline - time.monotonic()) * 1_000))
|
|
frame = self._wait_for_id(bus, 0x41A, min(remaining_ms, 200))
|
|
if frame is None:
|
|
continue
|
|
state = decode_feedback(frame).get("domain_state", {}).get("roof", {})
|
|
position = state.get("shade_position_pct")
|
|
if not isinstance(position, int):
|
|
continue
|
|
if state.get("thermal_protection") is True:
|
|
raise RuntimeError("shade entered thermal protection during motion")
|
|
if (moving_open and position >= target) or (
|
|
not moving_open and position <= target
|
|
):
|
|
reached = frame
|
|
break
|
|
except Exception as exc: # noqa: BLE001 - STOP remains mandatory
|
|
action_error = exc
|
|
finally:
|
|
try:
|
|
sent.append(bus.send(0x431, stop_payload))
|
|
except Exception as exc: # noqa: BLE001 - normalize after action error
|
|
stop_error = exc
|
|
|
|
if action_error is not None or stop_error is not None:
|
|
details = []
|
|
if action_error is not None:
|
|
details.append(f"shade motion failed: {action_error}")
|
|
if stop_error is not None:
|
|
details.append(f"mandatory shade STOP failed: {stop_error}")
|
|
raise RuntimeError("; ".join(details))
|
|
if reached is None:
|
|
raise RuntimeError(
|
|
"shade did not cross the requested position before timeout; STOP sent"
|
|
)
|
|
|
|
# Confirm both the final position and that the mechanism reports STOP.
|
|
final = self._wait_for_shade_stopped(
|
|
bus,
|
|
target=target,
|
|
tolerance=tolerance,
|
|
timeout_ms=self.feedback_timeout_ms,
|
|
)
|
|
if final is None:
|
|
raise RuntimeError(
|
|
"shade crossed the target and STOP was sent, but no stopped 0x41A "
|
|
"within the target tolerance was observed"
|
|
)
|
|
return sent, final
|
|
|
|
def _wait_feedback(
|
|
self, bus: CanBus, command: dict[str, Any]
|
|
) -> CanFrame | None:
|
|
raw_ids = str(command["signals"].get("feedback_ids", ""))
|
|
feedback_ids = {
|
|
int(value, 0) for value in raw_ids.split(",") if value.strip()
|
|
}
|
|
if not feedback_ids:
|
|
return None
|
|
expected = str(command["signals"].get("expected_feedback", ""))
|
|
last_candidate: CanFrame | None = None
|
|
deadline = time.monotonic() + self.feedback_timeout_ms / 1000
|
|
while time.monotonic() < deadline:
|
|
remaining_ms = max(1, int((deadline - time.monotonic()) * 1000))
|
|
frame = bus.receive(min(remaining_ms, 100), include_fd=True)
|
|
if frame is None or frame.arbitration_id not in feedback_ids:
|
|
continue
|
|
last_candidate = frame
|
|
if feedback_matches(expected, frame) is True:
|
|
return frame
|
|
return last_candidate
|
|
|
|
@staticmethod
|
|
def _wait_matching_feedback(
|
|
bus: CanBus,
|
|
command: dict[str, Any],
|
|
*,
|
|
timeout_ms: int,
|
|
) -> CanFrame | None:
|
|
raw_ids = str(command["signals"].get("feedback_ids", ""))
|
|
feedback_ids = {
|
|
int(value, 0) for value in raw_ids.split(",") if value.strip()
|
|
}
|
|
expected = str(command["signals"].get("expected_feedback", ""))
|
|
deadline = time.monotonic() + max(1, timeout_ms) / 1_000
|
|
last_candidate: CanFrame | None = None
|
|
while time.monotonic() < deadline:
|
|
remaining_ms = max(1, int((deadline - time.monotonic()) * 1_000))
|
|
frame = bus.receive(min(remaining_ms, 100), include_fd=True)
|
|
if frame is None or frame.arbitration_id not in feedback_ids:
|
|
continue
|
|
last_candidate = frame
|
|
if feedback_matches(expected, frame) is True:
|
|
return frame
|
|
return last_candidate
|
|
|
|
@staticmethod
|
|
def _wait_for_shade_stopped(
|
|
bus: CanBus,
|
|
*,
|
|
target: int,
|
|
tolerance: int,
|
|
timeout_ms: int,
|
|
) -> CanFrame | None:
|
|
deadline = time.monotonic() + max(1, timeout_ms) / 1_000
|
|
while time.monotonic() < deadline:
|
|
remaining_ms = max(1, int((deadline - time.monotonic()) * 1_000))
|
|
frame = bus.receive(min(remaining_ms, 100), include_fd=True)
|
|
if frame is None or frame.arbitration_id != 0x41A:
|
|
continue
|
|
roof = decode_feedback(frame).get("domain_state", {}).get("roof", {})
|
|
position = roof.get("shade_position_pct")
|
|
if (
|
|
isinstance(position, int)
|
|
and abs(position - target) <= tolerance
|
|
and roof.get("motion_state") == "stopped"
|
|
):
|
|
return frame
|
|
return None
|
|
|
|
@staticmethod
|
|
def _feedback_state(frame: CanFrame | None) -> dict[str, Any] | None:
|
|
if frame is None:
|
|
return None
|
|
decoded = decode_feedback(frame)
|
|
source = f"0x{frame.arbitration_id:03X}"
|
|
if frame.arbitration_id == 0x4FA:
|
|
if len(frame.data) < 8:
|
|
return {"source": source, "valid": False}
|
|
thermal = {1: "off", 2: "low", 3: "high"}
|
|
massage_modes = {
|
|
1: "off",
|
|
2: "wave",
|
|
3: "pulse",
|
|
4: "stretch",
|
|
5: "slack",
|
|
6: "relax",
|
|
}
|
|
massage_strengths = {8: "off", 9: "weak", 10: "medium", 11: "strong"}
|
|
ventilation_raw = frame.data[0] & 0x07
|
|
heating_raw = (frame.data[0] >> 3) & 0x07
|
|
massage_mode_raw = frame.data[5] & 0x0F
|
|
massage_strength_raw = (frame.data[5] >> 4) & 0x0F
|
|
return {
|
|
**decoded,
|
|
"feedback_frame_id": source,
|
|
"valid": True,
|
|
"target": "driver",
|
|
"ventilation": thermal.get(
|
|
ventilation_raw, f"unknown({ventilation_raw})"
|
|
),
|
|
"heating": thermal.get(heating_raw, f"unknown({heating_raw})"),
|
|
"massage_mode": massage_modes.get(
|
|
massage_mode_raw, f"unknown({massage_mode_raw})"
|
|
),
|
|
"massage_strength": massage_strengths.get(
|
|
massage_strength_raw, f"unknown({massage_strength_raw})"
|
|
),
|
|
}
|
|
if frame.arbitration_id == 0x494:
|
|
if len(frame.data) < 8:
|
|
return {"source": source, "valid": False}
|
|
initialized = {
|
|
"horizontal": bool((frame.data[7] >> 2) & 1),
|
|
"backrest": bool((frame.data[7] >> 3) & 1),
|
|
"height": bool((frame.data[7] >> 4) & 1),
|
|
"basin": bool((frame.data[7] >> 5) & 1),
|
|
}
|
|
return {
|
|
**decoded,
|
|
"feedback_frame_id": source,
|
|
"valid": True,
|
|
"target": "driver",
|
|
"initialized": initialized,
|
|
"execution_confirmation": "status_frame_only",
|
|
"limitation": "0x494 has no absolute motor position",
|
|
}
|
|
return {**decoded, "feedback_frame_id": source, "valid": True}
|
|
|
|
def _execute_window_position(
|
|
self, bus: CanBus, command: dict[str, Any]
|
|
) -> tuple[list[CanFrame], CanFrame | None]:
|
|
parameters = json.loads(command["signals"]["special_parameters"])
|
|
target = str(parameters["target"])
|
|
goal = int(parameters["position_pct"])
|
|
indexes = {"front_left": 0, "rear_left": 1, "front_right": 2, "rear_right": 3}
|
|
index = indexes[target]
|
|
before = self._wait_for_id(bus, 0x388, 2_500)
|
|
if before is None or len(before.data) < 6 or before.data[2 + index] > 100:
|
|
raise RuntimeError("未收到有效且新鲜的 0x388 车窗位置，拒绝启动机构")
|
|
current = int(before.data[2 + index])
|
|
if abs(current - goal) <= 3:
|
|
return [], before
|
|
motion = 1 if goal > current else 2
|
|
start_payload = NativeCanRuntime._window_payload(target, motion)
|
|
stop_payload = NativeCanRuntime._window_payload(target, 3)
|
|
sent: list[CanFrame] = []
|
|
for attempt in range(3):
|
|
sent.append(bus.send(0x431, start_payload, is_fd=True, bitrate_switch=True))
|
|
if attempt < 2:
|
|
time.sleep(0.1)
|
|
deadline = time.monotonic() + 12
|
|
reached: CanFrame | None = None
|
|
try:
|
|
while time.monotonic() < deadline:
|
|
frame = self._wait_for_id(bus, 0x388, 200)
|
|
if frame is None or len(frame.data) < 6 or frame.data[2 + index] > 100:
|
|
continue
|
|
if abs(int(frame.data[2 + index]) - goal) <= 3:
|
|
reached = frame
|
|
break
|
|
finally:
|
|
for attempt in range(3):
|
|
sent.append(bus.send(0x431, stop_payload, is_fd=True, bitrate_switch=True))
|
|
if attempt < 2:
|
|
time.sleep(0.1)
|
|
if reached is None:
|
|
raise RuntimeError("车窗在 12 秒内未到达目标，已发送停止序列")
|
|
return sent, reached
|
|
|
|
@staticmethod
|
|
def _wait_for_id(bus: CanBus, frame_id: int, timeout_ms: int) -> CanFrame | None:
|
|
deadline = time.monotonic() + timeout_ms / 1000
|
|
while time.monotonic() < deadline:
|
|
remaining = max(1, int((deadline - time.monotonic()) * 1000))
|
|
frame = bus.receive(min(remaining, 100), include_fd=True)
|
|
if frame is not None and frame.arbitration_id == frame_id:
|
|
return frame
|
|
return None
|
|
|
|
@staticmethod
|
|
def _feedback_matches(
|
|
command: dict[str, Any], frame: CanFrame | None
|
|
) -> bool | None:
|
|
expected = str(command["signals"].get("expected_feedback", ""))
|
|
return feedback_matches(expected, frame)
|