feat: synchronize CH1 vehicle control and feedback
This commit is contained in:
vee
@@ -12,6 +12,10 @@ 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
|
||||
|
||||
|
||||
@@ -51,11 +55,39 @@ 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",
|
||||
}
|
||||
)
|
||||
DRIVER_SEAT_TARGETS = frozenset({"driver", "front_left"})
|
||||
UNSUPPORTED_CH1_DOOR_LOCK_CAPABILITIES = frozenset(
|
||||
{"door.set_vehicle_lock", "door.set_rear_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 (
|
||||
"door.set_rear_lock is unsupported for physical execution on the current "
|
||||
"CH1; 0x32B is second-row-only and is not a verified whole-vehicle lock "
|
||||
"control, so CH1 door-lock support is observational only"
|
||||
)
|
||||
|
||||
|
||||
class NativeCanRuntime:
|
||||
"""Compile named capabilities into byte-exact CAN_DEV frames."""
|
||||
|
||||
compiler_version = "1.0.0"
|
||||
mapping_version = "can-dev-6e7ed04"
|
||||
mapping_version = "can-dev-verified-body-can-v4"
|
||||
|
||||
def __init__(self, repository_root: Path):
|
||||
self.root = repository_root
|
||||
@@ -82,7 +114,6 @@ class NativeCanRuntime:
|
||||
def supported_capabilities(self) -> frozenset[str]:
|
||||
return frozenset(
|
||||
{
|
||||
"door.set_rear_lock",
|
||||
"window.set_motion",
|
||||
"window.set_position",
|
||||
"climate.set_power",
|
||||
@@ -93,21 +124,28 @@ class NativeCanRuntime:
|
||||
"climate.set_control_mode",
|
||||
"outlet.set_position_percent",
|
||||
"outlet.set_calibrated_position",
|
||||
"seat.set_heating",
|
||||
"seat.set_ventilation",
|
||||
*DRIVER_SEAT_CAPABILITIES,
|
||||
"interior_light.set_indoor_power",
|
||||
"interior_light.set_wall_power",
|
||||
"interior_light.set_reading_power",
|
||||
"ambient_light.set_power",
|
||||
"ambient_light.set_mode",
|
||||
"ambient_light.set_illumination",
|
||||
"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:
|
||||
return str(action.get("capability")) in self.supported_capabilities
|
||||
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,
|
||||
@@ -120,6 +158,10 @@ class NativeCanRuntime:
|
||||
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}"
|
||||
)
|
||||
@@ -140,7 +182,12 @@ class NativeCanRuntime:
|
||||
"zone": target,
|
||||
},
|
||||
"parameters": dict(action.get("parameters", {})),
|
||||
"timeout_ms": 15_000 if capability_name == "window.set_position" else 4_000,
|
||||
"timeout_ms": (
|
||||
20_000
|
||||
if capability_name
|
||||
in {"window.set_position", "roof.set_shade_position"}
|
||||
else 4_000
|
||||
),
|
||||
"failure_policy": "abort",
|
||||
}
|
||||
)
|
||||
@@ -273,7 +320,7 @@ class NativeCanRuntime:
|
||||
@staticmethod
|
||||
def command_frame_public(command: dict[str, Any]) -> dict[str, Any]:
|
||||
signals = command["signals"]
|
||||
return {
|
||||
public = {
|
||||
"frame_id": signals["frame_id"],
|
||||
"payload_size": len(bytes.fromhex(str(signals["data_hex"]))),
|
||||
"data_hex": signals["data_hex"],
|
||||
@@ -282,6 +329,15 @@ class NativeCanRuntime:
|
||||
"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"],
|
||||
}
|
||||
return public
|
||||
|
||||
@staticmethod
|
||||
def _report(
|
||||
@@ -316,7 +372,11 @@ class NativeCanRuntime:
|
||||
}
|
||||
|
||||
def _frame_specs(self, operation: dict[str, Any]) -> list[NativeFrameSpec]:
|
||||
capability = operation["capability"]
|
||||
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"]
|
||||
|
||||
@@ -385,13 +445,6 @@ class NativeCanRuntime:
|
||||
)
|
||||
return specs
|
||||
|
||||
if capability == "door.set_rear_lock":
|
||||
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, feedback_ids=(0x407,), expected=f"rear_lock:{parameters['state']}")]
|
||||
|
||||
if capability == "window.set_motion":
|
||||
motion = self._enum(parameters, "motion", {"open": 1, "close": 2, "stop": 3})
|
||||
payload = self._window_payload(target, motion)
|
||||
@@ -413,69 +466,59 @@ class NativeCanRuntime:
|
||||
)
|
||||
]
|
||||
|
||||
if capability in {"seat.set_heating", "seat.set_ventilation"}:
|
||||
return [self._body_seat_frame(capability, target, parameters)]
|
||||
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 {"outlet.set_position_percent", "outlet.set_calibrated_position"}:
|
||||
return [self._outlet_frame(capability, target, parameters)]
|
||||
|
||||
if capability == "interior_light.set_indoor_power":
|
||||
state = self._enum(parameters, "state", {"on": 1, "off": 2})
|
||||
payload = bytearray(8)
|
||||
payload[0] = 0x0A
|
||||
_set_bits(payload, 24, 3, state)
|
||||
return [self._classic(0x32B, payload, feedback_ids=(0x42E,), expected=f"indoor_light:{parameters['state']}")]
|
||||
if capability == "interior_light.set_wall_power":
|
||||
state = self._enum(parameters, "state", {"on": 1, "off": 2})
|
||||
payload = bytearray(8)
|
||||
payload[0] = 0x11
|
||||
_set_bits(payload, 20, 2, state)
|
||||
return [self._classic(0x32B, payload)]
|
||||
return [self._all_reading_light_frame(parameters, "indoor_light")]
|
||||
if capability == "interior_light.set_reading_power":
|
||||
return [self._reading_light_frame(target, parameters)]
|
||||
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":
|
||||
value = self._enum(parameters, "state", {"off": 6, "on": 7})
|
||||
payload = bytearray(8)
|
||||
payload[0] = 0x0A
|
||||
_set_bits(payload, 27, 5, value)
|
||||
return [self._classic(0x32B, payload, feedback_ids=(0x42E,), expected=f"ambient_power:{parameters['state']}")]
|
||||
if capability == "ambient_light.set_mode":
|
||||
value = self._enum(
|
||||
parameters,
|
||||
"mode",
|
||||
{"camping": 1, "rest": 2, "cinema": 3, "meditation": 4, "romantic": 5, "off": 6, "on": 7},
|
||||
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)
|
||||
)
|
||||
payload = bytearray(8)
|
||||
payload[0] = 0x0A
|
||||
_set_bits(payload, 27, 5, value)
|
||||
return [self._classic(0x32B, payload, feedback_ids=(0x42E,), expected=f"ambient_mode:{value}")]
|
||||
if capability == "ambient_light.set_illumination":
|
||||
color = str(parameters.get("color_hex", ""))
|
||||
if len(color) != 7 or not color.startswith("#"):
|
||||
raise NativeCanCompileError("color_hex must use #RRGGBB")
|
||||
try:
|
||||
red, green, blue = (int(color[index : index + 2], 16) for index in (1, 3, 5))
|
||||
except ValueError as exc:
|
||||
raise NativeCanCompileError("color_hex must use #RRGGBB") from exc
|
||||
if not all(1 <= value <= 255 for value in (red, green, blue)):
|
||||
raise NativeCanCompileError("CAN_DEV RGB components must be between 1 and 255")
|
||||
brightness = self._integer(parameters, "brightness_pct", 0, 100)
|
||||
payload = bytearray((0x2B, red, green, blue, 0, 0, 0, 0))
|
||||
_set_bits(payload, 32, 7, brightness)
|
||||
return [self._classic(0x32B, payload, feedback_ids=(0x42E,), expected="ambient_rgb")]
|
||||
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})
|
||||
payload = bytearray(8)
|
||||
_set_bits(payload, 4, 4, motion)
|
||||
return [self._classic(0x431, payload, feedback_ids=(0x41A,), expected=f"shade_motion:{motion}")]
|
||||
return [self._shade_preset_frame(motion)]
|
||||
if capability == "roof.set_shade_position":
|
||||
position = self._integer(parameters, "position_pct", 0, 100)
|
||||
payload = bytearray(8)
|
||||
payload[3] = 0xFF
|
||||
payload[4] = position
|
||||
return [self._classic(0x4F5, payload, feedback_ids=(0x41A,), expected=f"shade_position:{position}")]
|
||||
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}")
|
||||
|
||||
@@ -612,54 +655,217 @@ class NativeCanRuntime:
|
||||
f"outlet:{target}:{horizontal}:{vertical}",
|
||||
)
|
||||
|
||||
def _body_seat_frame(
|
||||
self, capability: str, target: str, parameters: dict[str, Any]
|
||||
@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:
|
||||
indexes = {"driver": 0, "passenger": 1, "left_rear": 2, "right_rear": 3}
|
||||
if target not in indexes:
|
||||
raise NativeCanCompileError(f"unsupported body seat target {target}")
|
||||
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")
|
||||
feature = "heating" if capability.endswith("heating") else "ventilation"
|
||||
index = indexes[target]
|
||||
payload = bytearray(8)
|
||||
if state == "off":
|
||||
payload[0] = 0x20
|
||||
starts = (12, 20, 28, 36) if feature == "heating" else (8, 16, 24, 32)
|
||||
_set_bits(payload, starts[index], 2, 2)
|
||||
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 = self._integer(parameters, "level", 1, 5)
|
||||
payload[0] = 0x21
|
||||
starts = (12, 20, 28, 36) if feature == "heating" else (8, 16, 24, 32)
|
||||
_set_bits(payload, starts[index], 4, level)
|
||||
return self._classic(0x4C1, payload, feedback_ids=(0x42A,), expected=f"body_seat:{target}:{feature}:{state}")
|
||||
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
|
||||
|
||||
def _reading_light_frame(
|
||||
self, target: str, parameters: dict[str, Any]
|
||||
) -> NativeFrameSpec:
|
||||
state = self._enum(parameters, "state", {"off": 1, "on": 2})
|
||||
if target == "all":
|
||||
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"reading_all:{parameters['state']}")
|
||||
starts = {
|
||||
"front_left": 0,
|
||||
"front_right": 2,
|
||||
"middle_left": 4,
|
||||
"middle_right": 6,
|
||||
"middle_left_rotary": 8,
|
||||
"middle_right_rotary": 10,
|
||||
"rear_left": 12,
|
||||
"rear_right": 14,
|
||||
}
|
||||
try:
|
||||
start = starts[target]
|
||||
except KeyError as exc:
|
||||
raise NativeCanCompileError(f"unsupported reading-light target {target}") from exc
|
||||
feature = (
|
||||
"heating"
|
||||
if capability == "seat.set_driver_heating_native"
|
||||
else "ventilation"
|
||||
)
|
||||
payload = bytearray(8)
|
||||
_set_bits(payload, start, 2, state)
|
||||
return self._fd(0x1B2, payload, 3, 100, expected=f"reading:{target}:{parameters['state']}")
|
||||
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 _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:
|
||||
@@ -691,18 +897,44 @@ class NativeCanExecutor:
|
||||
for command in command_plan["spec"]["commands"]:
|
||||
operation = operations[command["operation_ref"]]
|
||||
try:
|
||||
if command["signals"].get("special_handler") == "window_position":
|
||||
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")
|
||||
if 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
|
||||
)
|
||||
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)
|
||||
feedback_required = bool(
|
||||
str(command["signals"].get("feedback_ids", "")).strip()
|
||||
)
|
||||
verified = (
|
||||
(not feedback_required or feedback is not None)
|
||||
and feedback_match is not False
|
||||
feedback_required = True
|
||||
verified = feedback is not None and feedback_match is True
|
||||
physical_channel = (
|
||||
feedback.channel
|
||||
if feedback is not None
|
||||
else sent_frames[-1].channel
|
||||
if sent_frames
|
||||
else None
|
||||
)
|
||||
results.append(
|
||||
{
|
||||
@@ -712,7 +944,9 @@ class NativeCanExecutor:
|
||||
"message": (
|
||||
"CAN 盒已提交车端控制帧并观察到匹配反馈。"
|
||||
if feedback_match is True
|
||||
else "CAN 盒已提交车端控制帧并观察到后续状态帧。"
|
||||
else "CAN 盒已提交车端控制帧，但反馈字段与目标值不一致。"
|
||||
if feedback_match is False
|
||||
else "CAN 盒已提交车端控制帧并观察到状态帧，但该帧没有可证明动作完成的字段。"
|
||||
if feedback is not None
|
||||
else "CAN 盒已提交车端控制帧；当前反馈窗口未观察到对应状态帧。"
|
||||
),
|
||||
@@ -724,8 +958,9 @@ class NativeCanExecutor:
|
||||
"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,
|
||||
"channel": 1,
|
||||
"channel": physical_channel,
|
||||
"simulated": False,
|
||||
},
|
||||
}
|
||||
@@ -771,6 +1006,200 @@ class NativeCanExecutor:
|
||||
time.sleep(int(repeat["period_ms"]) / 1000)
|
||||
return sent
|
||||
|
||||
def _execute_driver_seat_axis(
|
||||
self, bus: CanBus, command: dict[str, Any]
|
||||
) -> 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)
|
||||
|
||||
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:
|
||||
@@ -780,14 +1209,126 @@ class NativeCanExecutor:
|
||||
}
|
||||
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 not None and frame.arbitration_id in feedback_ids:
|
||||
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]:
|
||||
@@ -843,22 +1384,5 @@ class NativeCanExecutor:
|
||||
def _feedback_matches(
|
||||
command: dict[str, Any], frame: CanFrame | None
|
||||
) -> bool | None:
|
||||
if frame is None:
|
||||
return None
|
||||
expected = str(command["signals"].get("expected_feedback", ""))
|
||||
if not expected or frame.arbitration_id != 0x404 or len(frame.data) < 6:
|
||||
return None
|
||||
parts = expected.split(":")
|
||||
if parts[0] == "climate_power":
|
||||
actual = frame.data[1] & 0x03
|
||||
return actual == (1 if parts[1] == "on" else 2)
|
||||
if parts[0] == "climate_control":
|
||||
return ((frame.data[1] >> 2) & 0x03) == int(parts[1])
|
||||
if parts[0] == "climate_mode":
|
||||
return (frame.data[2] & 0x0F) == int(parts[1])
|
||||
if parts[0] == "climate_fan":
|
||||
return ((frame.data[2] >> 4) & 0x0F) == int(parts[1])
|
||||
if parts[0] == "climate_temperature":
|
||||
byte_index = {"driver": 4, "passenger": 5}.get(parts[1])
|
||||
return None if byte_index is None else frame.data[byte_index] == int(parts[2])
|
||||
return None
|
||||
return feedback_matches(expected, frame)
|
||||
|
||||
Reference in New Issue
Block a user