feat: synchronize CH1 vehicle control and feedback

2026-08-11 10:42:13 +08:00
parent 3701192a11
commit 32379a1573
50 changed files with 13636 additions and 736 deletions
@@ -0,0 +1,154 @@
+from __future__ import annotations
+
+from copy import deepcopy
+import unittest
+
+from cognitive_agent.application.native_can_feedback import (
+    authoritative_physical_ch1_feedback_state,
+    decode_feedback,
+    feedback_matches,
+    merge_state,
+)
+from cognitive_agent.infrastructure.can.gateway import CanFrame
+
+
+class NativeCanFeedbackTests(unittest.TestCase):
+    def frame(self, frame_id: int, data_hex: str) -> CanFrame:
+        return CanFrame(frame_id, bytes.fromhex(data_hex), channel=1, is_fd=True)
+
+    def test_passive_ch1_samples_decode_stationary_vehicle(self) -> None:
+        gear = decode_feedback(self.frame(0x342, "00001fb1c1fff778"))
+        speed = decode_feedback(self.frame(0x4EB, "0700f00001516766"))
+
+        self.assertEqual(gear["vehicle_safety"]["gear"], "P")
+        self.assertEqual(speed["vehicle_safety"]["speed_kmh"], 0)
+        self.assertEqual(gear["source"], "physical_can_ch1")
+        self.assertEqual(gear["channel"], 1)
+
+    def test_climate_feedback_is_matched_by_signal_value(self) -> None:
+        feedback = self.frame(0x404, "4369116c1818a2dd")
+        decoded = decode_feedback(feedback)["domain_state"]["climate"]
+
+        self.assertEqual(decoded["power"], "on")
+        self.assertEqual(decoded["zones"]["driver"]["temperature_c"], 24)
+        self.assertTrue(feedback_matches("climate_power:on", feedback))
+        self.assertTrue(feedback_matches("climate_temperature:driver:24", feedback))
+        self.assertFalse(feedback_matches("climate_temperature:driver:25", feedback))
+
+    def test_window_position_match_uses_authoritative_0x388(self) -> None:
+        feedback = self.frame(0x388, "0000320000005500")
+        self.assertTrue(feedback_matches("window_position:front_left:50", feedback))
+        self.assertFalse(feedback_matches("window_position:front_left:80", feedback))
+
+    def test_light_feedback_requires_target_value_not_only_frame_id(self) -> None:
+        passive_feedback = self.frame(0x42E, "11140a03880200c6")
+        self.assertEqual(
+            decode_feedback(passive_feedback)["domain_state"]["interior_light"]["power"],
+            "unknown",
+        )
+        feedback = self.frame(0x42E, "11140a03200200c6")
+        decoded = decode_feedback(feedback)["domain_state"]
+        self.assertEqual(decoded["interior_light"]["power"], "off")
+        self.assertEqual(decoded["ambient_light"]["power"], "off")
+        self.assertTrue(feedback_matches("indoor_light:off", feedback))
+        self.assertFalse(feedback_matches("indoor_light:on", feedback))
+        self.assertTrue(feedback_matches("ambient_power:off", feedback))
+
+    def test_driver_seat_feedback_uses_0x4fa_not_0x42a(self) -> None:
+        passive_feedback = self.frame(0x4FA, "0980aa0000810000")
+        passive_driver = decode_feedback(passive_feedback)["actuator_state"]["driver"]
+        self.assertEqual(passive_driver["heating"], {"state": "off", "level": 0})
+        feedback = self.frame(0x4FA, "1180aa0000810000")
+        driver = decode_feedback(feedback)["actuator_state"]["driver"]
+        self.assertEqual(driver["ventilation"], {"state": "off", "level": 0})
+        self.assertEqual(driver["heating"], {"state": "on", "level": 1})
+        self.assertTrue(feedback_matches("driver_seat_thermal:heating:low", feedback))
+        self.assertFalse(feedback_matches("driver_seat_thermal:ventilation:high", feedback))
+
+    def test_driver_position_feedback_never_claims_absolute_position(self) -> None:
+        decoded = decode_feedback(
+            self.frame(0x494, "151515058c28203c")
+        )["actuator_state"]["driver"]["position_feedback"]
+        self.assertFalse(decoded["absolute_position_available"])
+        self.assertEqual(
+            set(decoded["initialized"]),
+            {"horizontal", "backrest", "height", "basin"},
+        )
+
+    def test_recursive_state_merge_preserves_other_devices(self) -> None:
+        target = {"driver": {"heating": {"state": "off"}}, "passenger": {"safe": True}}
+        update = {"driver": {"heating": {"state": "on", "level": 1}}}
+        merge_state(target, update)
+        self.assertEqual(target["driver"]["heating"]["state"], "on")
+        self.assertEqual(target["passenger"], {"safe": True})
+
+    def test_authoritative_state_requires_one_complete_consistent_ch1_frame(
+        self,
+    ) -> None:
+        operation = {
+            "status": "succeeded",
+            "telemetry": {
+                "channel": 1,
+                "simulated": False,
+                "feedback_required": True,
+                "feedback_observed": True,
+                "feedback_match": True,
+                "feedback_frame": {
+                    "frame_id": "0x404",
+                    "payload_size": 8,
+                    "data_hex": "5B 65 35 6C 18 18 A2 5D",
+                    "timestamp": 12.5,
+                    "channel": 1,
+                },
+                "feedback_state": {
+                    "source": "physical_can_ch1",
+                    "channel": 1,
+                    "frame_id": "0x404",
+                    "feedback_frame_id": "0x404",
+                    "valid": True,
+                    "domain_state": {"climate": {"power": "on"}},
+                },
+            },
+        }
+
+        state = authoritative_physical_ch1_feedback_state(operation)
+        self.assertIsNotNone(state)
+        self.assertTrue(state["verification"]["authoritative"])
+        self.assertEqual(state["verification"]["frame_id"], "0x404")
+
+        for label, mutate in (
+            (
+                "wrong frame channel",
+                lambda item: item["telemetry"]["feedback_frame"].update(
+                    {"channel": 0}
+                ),
+            ),
+            (
+                "short payload",
+                lambda item: item["telemetry"]["feedback_frame"].update(
+                    {"payload_size": 7}
+                ),
+            ),
+            (
+                "mismatched decoded id",
+                lambda item: item["telemetry"]["feedback_state"].update(
+                    {"feedback_frame_id": "0x4FA"}
+                ),
+            ),
+            (
+                "empty decoded patch",
+                lambda item: item["telemetry"]["feedback_state"].pop(
+                    "domain_state"
+                ),
+            ),
+        ):
+            invalid = deepcopy(operation)
+            mutate(invalid)
+            with self.subTest(label=label):
+                self.assertIsNone(
+                    authoritative_physical_ch1_feedback_state(invalid)
+                )
+
+
+if __name__ == "__main__":
+    unittest.main()