vee/Cognitive_Agent
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: synchronize CH1 vehicle control and feedback

Browse Source
This commit is contained in:
vee
2026-08-11 10:42:13 +08:00
parent 3701192a11
commit 32379a1573
50 changed files with 13636 additions and 736 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/cognitive_agent/application/native_can_feedback.py
+614
View File
@@ -0,0 +1,614 @@
"""Decode authoritative CH1 vehicle feedback and match requested targets.
The ZLG SDK accepting a transmit request is not evidence that an ECU acted.
This module contains the small, byte-exact subset of CAN_DEV feedback that the
web console is allowed to present as physical state.
"""
from __future__ import annotations
from copy import deepcopy
import math
import re
import time
from datetime import UTC, datetime
from typing import Any
from cognitive_agent.infrastructure.can.gateway import CanBus, CanFrame
PHYSICAL_SOURCE = "physical_can_ch1"
FRAME_ID_PATTERN = re.compile(r"^0x[0-9A-F]{3}$", re.IGNORECASE)
HEX_BYTE_PATTERN = re.compile(r"^[0-9A-F]{2}$")
class VehicleSafetyPreflightError(RuntimeError):
"""Raised when fresh CH1 evidence cannot prove a stationary vehicle."""
def authoritative_physical_ch1_feedback_state(
operation: dict[str, Any],
) -> dict[str, Any] | None:
"""Return a verified decoded state patch for one real CH1 operation.
A transmit acknowledgement or a matching flag alone is insufficient. The
raw feedback frame, its channel, and both decoded frame identifiers must
describe the same complete frame before its state may reach the UI store.
"""
telemetry = operation.get("telemetry")
if not isinstance(telemetry, dict):
return None
feedback_frame = telemetry.get("feedback_frame")
feedback_state = telemetry.get("feedback_state")
if not isinstance(feedback_frame, dict) or not isinstance(
feedback_state, dict
):
return None
raw_frame_id = str(feedback_frame.get("frame_id", ""))
frame_id = (
f"0x{raw_frame_id[2:].upper()}"
if FRAME_ID_PATTERN.fullmatch(raw_frame_id)
else raw_frame_id
)
payload_size = feedback_frame.get("payload_size")
data_hex = feedback_frame.get("data_hex")
if (
operation.get("status") != "succeeded"
or telemetry.get("channel") != 1
or telemetry.get("simulated") is not False
or telemetry.get("feedback_required") is not True
or telemetry.get("feedback_observed") is not True
or telemetry.get("feedback_match") is not True
or feedback_frame.get("channel") != 1
or not FRAME_ID_PATTERN.fullmatch(frame_id)
or not isinstance(payload_size, int)
or isinstance(payload_size, bool)
or not 1 <= payload_size <= 64
or not isinstance(data_hex, str)
):
return None
payload = data_hex.upper().split()
if len(payload) != payload_size or not all(
HEX_BYTE_PATTERN.fullmatch(byte) for byte in payload
):
return None
timestamp = feedback_frame.get("timestamp")
if timestamp is not None and (
not isinstance(timestamp, (int, float))
or isinstance(timestamp, bool)
or not math.isfinite(float(timestamp))
or float(timestamp) < 0
):
return None
state_payload_present = any(
isinstance(feedback_state.get(key), dict) and bool(feedback_state.get(key))
for key in ("domain_state", "actuator_state")
)
if (
feedback_state.get("source") != PHYSICAL_SOURCE
or feedback_state.get("channel") != 1
or feedback_state.get("valid") is not True
or str(feedback_state.get("frame_id", "")).lower() != frame_id.lower()
or str(feedback_state.get("feedback_frame_id", "")).lower()
!= frame_id.lower()
or not state_payload_present
):
return None
verified = deepcopy(feedback_state)
verified["frame_id"] = frame_id
verified["feedback_frame_id"] = frame_id
verified["verification"] = {
"authoritative": True,
"channel": 1,
"simulated": False,
"feedback_required": True,
"feedback_observed": True,
"feedback_match": True,
"frame_id": frame_id,
"payload_size": payload_size,
}
return verified
def _now() -> str:
return datetime.now(UTC).isoformat().replace("+00:00", "Z")
def _bits(data: bytes, start: int, length: int) -> int:
value = 0
for offset in range(length):
bit = start + offset
if bit // 8 >= len(data):
raise ValueError("feedback frame is shorter than the requested signal")
if data[bit // 8] & (1 << (bit % 8)):
value |= 1 << offset
return value
def _state(value: int, *, on: int = 1, off: int = 2) -> str:
if value == on:
return "on"
if value == off:
return "off"
return "unknown"
def decode_feedback(frame: CanFrame) -> dict[str, Any]:
"""Return only state fields proven by one recognized feedback frame."""
data = frame.data
decoded: dict[str, Any] = {
"source": PHYSICAL_SOURCE,
"channel": frame.channel,
"frame_id": f"0x{frame.arbitration_id:03X}",
}
if frame.arbitration_id == 0x342 and len(data) >= 8:
gear_raw = _bits(data, 24, 4)
decoded["vehicle_safety"] = {
"gear": {1: "P", 2: "R", 3: "N", 4: "D"}.get(gear_raw, "unknown")
}
return decoded
if frame.arbitration_id == 0x4EB and len(data) >= 8 and data[0] == 0x07:
speed_raw = _bits(data, 8, 12)
decoded["vehicle_safety"] = {
"speed_kmh": None if speed_raw == 0xFFF else round(speed_raw * 0.06875, 3)
}
return decoded
if frame.arbitration_id == 0x407 and len(data) >= 8:
names = ("front_left", "front_right", "rear_left", "rear_right")
decoded["domain_state"] = {
"door": {
"locks": {
name: {"state": {1: "unlock", 2: "lock"}.get(_bits(data, index * 2, 2), "unknown")}
for index, name in enumerate(names)
},
"vehicle_lock": {
"state": {1: "unlock", 2: "lock"}.get(_bits(data, 10, 2), "unknown")
},
}
}
return decoded
if frame.arbitration_id == 0x388 and len(data) >= 8:
names = ("front_left", "rear_left", "front_right", "rear_right")
positions = {
name: {
"position_pct": int(data[2 + index]) if data[2 + index] <= 100 else None,
"closed": {1: True, 2: False}.get(_bits(data, 48 + index * 2, 2)),
}
for index, name in enumerate(names)
}
decoded["domain_state"] = {"window": {"positions": positions}}
return decoded
if frame.arbitration_id == 0x404 and len(data) >= 8:
power = _state(_bits(data, 8, 2))
control = {1: "manual", 2: "auto"}.get(_bits(data, 10, 2), "unknown")
cycle = {1: "external", 2: "internal", 3: "auto"}.get(
_bits(data, 12, 2), "unknown"
)
mode_raw = _bits(data, 16, 4)
mode = {
1: "face",
2: "face_feet",
3: "feet",
4: "feet_defrost",
5: "defrost",
6: "face_feet_defrost",
7: "face_defrost",
}.get(mode_raw, "unknown")
decoded["domain_state"] = {
"climate": {
"power": power,
"ac": power,
"control_mode": control,
"cycle_mode": cycle,
"fan_level": _bits(data, 20, 4),
"zones": {
"driver": {"temperature_c": int(data[4]), "outlet_mode": mode},
"passenger": {"temperature_c": int(data[5]), "outlet_mode": mode},
},
}
}
return decoded
if frame.arbitration_id == 0x4FA and len(data) >= 8:
thermal = {1: ("off", 0), 2: ("on", 1), 3: ("on", 3)}
vent_state, vent_level = thermal.get(_bits(data, 0, 3), ("unknown", 0))
heat_state, heat_level = thermal.get(_bits(data, 3, 3), ("unknown", 0))
massage_mode_raw = _bits(data, 40, 4)
massage_strength_raw = _bits(data, 44, 4)
decoded["actuator_state"] = {
"driver": {
"ventilation": {"state": vent_state, "level": vent_level},
"heating": {"state": heat_state, "level": heat_level},
"massage": {
"mode": {
1: "off",
2: "wave",
3: "pulse",
4: "stretch",
5: "slack",
6: "relax",
}.get(massage_mode_raw, "unknown"),
"strength": {8: "off", 9: "weak", 10: "medium", 11: "strong"}.get(
massage_strength_raw, "unknown"
),
},
}
}
return decoded
if frame.arbitration_id == 0x494 and len(data) >= 8:
decoded["actuator_state"] = {
"driver": {
"position_feedback": {
"absolute_position_available": False,
"initialized": {
"horizontal": bool(_bits(data, 58, 1)),
"backrest": bool(_bits(data, 59, 1)),
"height": bool(_bits(data, 60, 1)),
"basin": bool(_bits(data, 61, 1)),
},
}
}
}
return decoded
if frame.arbitration_id == 0x42E and len(data) >= 8:
front_brightness_raw = _bits(data, 0, 4)
rear_brightness_raw = _bits(data, 4, 4)
front_color = _bits(data, 8, 7)
rear_color = _bits(data, 15, 7)
ambient_raw = _bits(data, 32, 3)
indoor_raw = _bits(data, 37, 2)
if ambient_raw == 1 or (
ambient_raw == 0
and 2 <= front_brightness_raw <= 6
and 2 <= rear_brightness_raw <= 6
):
ambient_power = "on"
elif ambient_raw == 2 or (
ambient_raw == 0
and front_brightness_raw == 1
and rear_brightness_raw == 1
):
ambient_power = "off"
else:
ambient_power = "unknown"
decoded["domain_state"] = {
"interior_light": {"power": _state(indoor_raw, on=2, off=1)},
"ambient_light": {
"power": ambient_power,
"front_brightness_level": (
front_brightness_raw - 1 if 1 <= front_brightness_raw <= 6 else None
),
"rear_brightness_level": (
rear_brightness_raw - 1 if 1 <= rear_brightness_raw <= 6 else None
),
"front_color_index": front_color,
"rear_color_index": rear_color,
},
}
return decoded
if frame.arbitration_id == 0x41A and len(data) >= 8:
position = int(data[1]) if data[1] <= 100 else None
decoded["domain_state"] = {
"roof": {
"shade_position_pct": position,
"initialized": _bits(data, 18, 2) == 1,
"closed": {1: True, 2: False}.get(_bits(data, 22, 2)),
"motion_state": {
1: "stopped",
2: "opening",
3: "closing",
4: "anti_pinch",
}.get(_bits(data, 27, 3), "unknown"),
"thermal_protection": bool(_bits(data, 49, 1)),
}
}
return decoded
return decoded
def feedback_matches(expected: str, frame: CanFrame | None) -> bool | None:
"""Compare one authoritative feedback frame with a compiled expectation."""
if frame is None or not expected:
return None
decoded = decode_feedback(frame)
parts = expected.split(":")
family = parts[0]
if family.startswith("climate_"):
climate = decoded.get("domain_state", {}).get("climate")
if not climate:
return None
if family == "climate_power":
return climate["power"] == parts[1]
if family == "climate_control":
return climate["control_mode"] == {"1": "manual", "2": "auto"}.get(parts[1])
if family == "climate_mode":
expected_mode = {
"1": "face",
"2": "face_feet",
"3": "feet",
"4": "feet_defrost",
"5": "defrost",
"6": "face_feet_defrost",
"7": "face_defrost",
}.get(parts[1])
return climate["zones"]["driver"]["outlet_mode"] == expected_mode
if family == "climate_fan":
return climate["fan_level"] == int(parts[1])
if family == "climate_temperature":
return climate["zones"].get(parts[1], {}).get("temperature_c") == int(parts[2])
if family == "rear_lock":
locks = decoded.get("domain_state", {}).get("door", {}).get("locks", {})
return bool(locks) and all(
locks.get(target, {}).get("state") == parts[1]
for target in ("rear_left", "rear_right")
)
if family == "vehicle_lock":
door = decoded.get("domain_state", {}).get("door", {})
locks = door.get("locks", {})
requested = parts[1]
return bool(locks) and door.get("vehicle_lock", {}).get("state") == requested and all(
locks.get(target, {}).get("state") == requested
for target in ("front_left", "front_right", "rear_left", "rear_right")
)
if family == "window_motion":
positions = decoded.get("domain_state", {}).get("window", {}).get("positions", {})
target, motion = parts[1], parts[2]
targets = {
"all": ("front_left", "rear_left", "front_right", "rear_right"),
"front": ("front_left", "front_right"),
"rear": ("rear_left", "rear_right"),
}.get(target, (target,))
if not positions or motion == "stop":
return None
desired_closed = motion == "close"
return all(positions.get(item, {}).get("closed") is desired_closed for item in targets)
if family == "window_position":
position = (
decoded.get("domain_state", {})
.get("window", {})
.get("positions", {})
.get(parts[1], {})
.get("position_pct")
)
return position is not None and abs(position - int(parts[2])) <= 3
if family == "indoor_light":
power = decoded.get("domain_state", {}).get("interior_light", {}).get("power")
return power == parts[1]
if family == "ambient_power":
power = decoded.get("domain_state", {}).get("ambient_light", {}).get("power")
return power == parts[1]
if family == "ambient_color":
ambient = decoded.get("domain_state", {}).get("ambient_light", {})
return ambient.get("front_color_index") == int(parts[1]) and ambient.get(
"rear_color_index"
) == int(parts[1])
if family == "ambient_brightness":
ambient = decoded.get("domain_state", {}).get("ambient_light", {})
return ambient.get("front_brightness_level") == int(parts[1]) and ambient.get(
"rear_brightness_level"
) == int(parts[1])
if family == "shade_stopped":
roof = decoded.get("domain_state", {}).get("roof")
return None if not roof else roof.get("motion_state") == "stopped"
if family in {"shade_motion", "shade_position"}:
position = decoded.get("domain_state", {}).get("roof", {}).get("shade_position_pct")
if position is None:
return False
if family == "shade_position":
target = int(parts[1])
else:
target = {"1": 100, "2": 0, "3": 50}.get(parts[1])
if target is None:
return None
tolerance = 2 if target in {0, 100} else 3
return abs(position - target) <= tolerance
if family == "driver_seat_thermal":
feature, target_state = parts[1], parts[2]
driver = decoded.get("actuator_state", {}).get("driver", {})
actual = driver.get(feature, {})
if target_state == "off":
return actual.get("state") == "off"
target_level = 1 if target_state == "low" else 3
return actual.get("state") == "on" and actual.get("level") == target_level
# The compiler keeps the raw 0x4FA enum in these expectations so its
# byte-exact contract remains directly comparable with CAN_DEV.
if family in {"driver_seat_ventilation", "driver_seat_heating"}:
if frame.arbitration_id != 0x4FA or len(frame.data) < 8:
return None
actual_raw = (
frame.data[0] & 0x07
if family == "driver_seat_ventilation"
else (frame.data[0] >> 3) & 0x07
)
return actual_raw == int(parts[1])
if family == "driver_seat_massage_mode":
actual = (
decoded.get("actuator_state", {})
.get("driver", {})
.get("massage", {})
.get("mode")
)
expected_mode = {
"1": "off",
"2": "wave",
"3": "pulse",
"4": "stretch",
"5": "slack",
"6": "relax",
}.get(parts[1], parts[1])
return actual == expected_mode
if family == "driver_seat_massage_strength":
actual = (
decoded.get("actuator_state", {})
.get("driver", {})
.get("massage", {})
.get("strength")
)
expected_strength = {
"8": "off",
"9": "weak",
"10": "medium",
"11": "strong",
}.get(parts[1], parts[1])
return actual == expected_strength
if family == "driver_seat_axis":
# 0x494 exposes initialization only. It has no motor position or
# target-reached signal, so it cannot prove a physical axis movement.
return None
if family == "driver_seat_axis_initialized":
initialized = (
decoded.get("actuator_state", {})
.get("driver", {})
.get("position_feedback", {})
.get("initialized", {})
)
return initialized.get(parts[1]) is True
return None
def observe_vehicle_safety(
bus: CanBus,
*,
timeout_ms: int = 2_500,
required_channel: int = 1,
) -> dict[str, Any]:
"""Passively collect fresh gear and speed frames without transmitting.
A newly opened channel is observed until both independent safety frames are
received. Frames from any channel other than the physical CH1 body bus are
ignored, so a caller cannot accidentally satisfy the gate with CH0 traffic.
"""
if required_channel != 1:
raise VehicleSafetyPreflightError("vehicle safety policy permits only CH1")
if not 100 <= int(timeout_ms) <= 10_000:
raise VehicleSafetyPreflightError(
"vehicle safety observation timeout must be between 100 and 10000 ms"
)
deadline = time.monotonic() + int(timeout_ms) / 1_000
gear: str | None = None
speed_kmh: float | None = None
frame_count = 0
ignored_wrong_channel = 0
evidence: dict[str, dict[str, Any]] = {}
while time.monotonic() < deadline and (gear is None or speed_kmh is None):
remaining_ms = max(1, int((deadline - time.monotonic()) * 1_000))
frame = bus.receive(min(remaining_ms, 100), include_fd=True)
if frame is None:
continue
frame_count += 1
if frame.channel != required_channel:
ignored_wrong_channel += 1
continue
decoded = decode_feedback(frame)
safety = decoded.get("vehicle_safety")
if not isinstance(safety, dict):
continue
if "gear" in safety:
gear = str(safety["gear"])
evidence["gear"] = {
"frame_id": decoded["frame_id"],
"channel": frame.channel,
"data_hex": frame.data.hex(" ").upper(),
}
if "speed_kmh" in safety:
raw_speed = safety["speed_kmh"]
speed_kmh = (
float(raw_speed) if isinstance(raw_speed, (int, float)) else None
)
evidence["speed"] = {
"frame_id": decoded["frame_id"],
"channel": frame.channel,
"data_hex": frame.data.hex(" ").upper(),
}
return {
"source": PHYSICAL_SOURCE,
"channel": required_channel,
"read_only": True,
"captured_at": _now(),
"gear": gear,
"speed_kmh": speed_kmh,
"stationary": gear == "P" and speed_kmh is not None and abs(speed_kmh) <= 0.05,
"complete": gear is not None and speed_kmh is not None,
"frame_count": frame_count,
"ignored_wrong_channel": ignored_wrong_channel,
"evidence": evidence,
}
def require_stationary_vehicle(snapshot: dict[str, Any]) -> None:
"""Fail closed unless independent fresh CH1 frames prove P and 0 km/h."""
if snapshot.get("source") != PHYSICAL_SOURCE:
raise VehicleSafetyPreflightError(
"vehicle safety evidence is not authoritative physical CH1 feedback"
)
if snapshot.get("channel") != 1 or snapshot.get("read_only") is not True:
raise VehicleSafetyPreflightError(
"vehicle safety evidence was not captured read-only on CH1"
)
evidence = snapshot.get("evidence")
if not isinstance(evidence, dict):
raise VehicleSafetyPreflightError("vehicle safety evidence is missing")
if evidence.get("gear", {}).get("frame_id") != "0x342":
raise VehicleSafetyPreflightError("fresh CH1 0x342 gear evidence is missing")
if evidence.get("speed", {}).get("frame_id") != "0x4EB":
raise VehicleSafetyPreflightError("fresh CH1 0x4EB speed evidence is missing")
if str(snapshot.get("gear", "")).upper() != "P":
raise VehicleSafetyPreflightError("vehicle is not confirmed in P")
speed = snapshot.get("speed_kmh")
if (
isinstance(speed, bool)
or not isinstance(speed, (int, float))
or abs(float(speed)) > 0.05
):
raise VehicleSafetyPreflightError(
"vehicle speed is not confirmed at 0 km/h"
)
def merge_state(target: dict[str, Any], update: dict[str, Any]) -> dict[str, Any]:
"""Recursively merge decoded feedback without discarding untouched devices."""
for key, value in update.items():
if isinstance(value, dict) and isinstance(target.get(key), dict):
merge_state(target[key], value)
else:
target[key] = value
return target