diff --git a/artifacts/iqpilot_updater_private/manifest.json b/artifacts/iqpilot_updater_private/manifest.json index 2fc01871e..44989ecc9 100644 --- a/artifacts/iqpilot_updater_private/manifest.json +++ b/artifacts/iqpilot_updater_private/manifest.json @@ -14,20 +14,25 @@ "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", "size": 0 }, + "python/iqpilot_private/updater/_hf_maps_blob.cpython-312-aarch64-linux-gnu.so": { + "mode": 493, + "sha256": "f18e3a7886a2e6835c91936cc4a11b118c02576f827683cff2a8d8788c3d1628", + "size": 67616 + }, "python/iqpilot_private/updater/_iqos_pat_blob.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "949c976482e5168aa1898087a5a0c02f523e4b8a43940eaadba56e5df81819f2", + "sha256": "9bb73ab877e3c87f415b07a780c807f6f686bf5d5c01f4f4866260d2812f0b8d", "size": 67616 }, "python/iqpilot_private/updater/_pat_blob.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "908a448f4ddad2d95ef524a771ee0038c62e72c44598cc8db2755c178b950de0", + "sha256": "8e33ab06991537400b3c50d2bfe3483627b50c999d6ad0c44d27d09eacac9235", "size": 67616 }, "python/iqpilot_private/updater/git_remote.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "c3495a4480205bbf1e21e7ea0b55a27409be1da68e974ef070946bfd726c8c90", - "size": 136576 + "sha256": "55db05006382a9212c300db2236590260b66489d904e19656f2cf03309de89bf", + "size": 136608 }, "runtime": { "entries": { @@ -38,8 +43,9 @@ } }, "signatures": { - "python/iqpilot_private/updater/_iqos_pat_blob.cpython-312-aarch64-linux-gnu.so": "2BgXuTkWa3sjvPsyTgFHVBaZYOE+6MmRE+/lUlINyD05fHG6qgf6GJPeYyy6TPNZqhBWoI5z/NmRjAoVzzsSDQ==", - "python/iqpilot_private/updater/_pat_blob.cpython-312-aarch64-linux-gnu.so": "EvASRs8Z45SknoiFmd9FpFV0boTy2L+OIJqEQMfVq88jglU/RVs5RNg/JW3SrghodMvxgTIXzBrZy2OggDPRCQ==", - "python/iqpilot_private/updater/git_remote.cpython-312-aarch64-linux-gnu.so": "wbN3CwOrLv0iX3a2nSmr4dTr3kEHrSEPmQ+S6IiccUzgVo8rBQXRe0dduyiQZzXrnEBYRAvz66WXfyWPrWs8AQ==" + "python/iqpilot_private/updater/_hf_maps_blob.cpython-312-aarch64-linux-gnu.so": "8MfmjOuuJgGPd2959u6zSnbsfCGIcY8ut4vH4JW//Vqb51508zKFSZYbmsKYFSYmUYcsNrDdKlzG7cyxwi25CQ==", + "python/iqpilot_private/updater/_iqos_pat_blob.cpython-312-aarch64-linux-gnu.so": "7ygpLdx/E5TgYoL8OINkv4mYLVkAHlAG0Wh4ZBnXCdTi+fd1bySI+7WmQczoQMlk0VjtIqyJTus+tsRkXEjQDg==", + "python/iqpilot_private/updater/_pat_blob.cpython-312-aarch64-linux-gnu.so": "79iU094xiuJ25sF9qXaLqTS/f9AidQhDoTlj2HVBIWt6biVmBY0MpgHOXK7lhCskykR7H/8j9zp2SsA2dToaDA==", + "python/iqpilot_private/updater/git_remote.cpython-312-aarch64-linux-gnu.so": "rxKmOjUbaJRdo65i8bDae/zEDUXIWvOay4k+KJ3vuH1Z+jQQvRXuWjKAMtJX7a7v/zzTcg7ijY2CBfXEFZJRCQ==" } } diff --git a/artifacts/iqpilot_updater_private/manifest.json.sig b/artifacts/iqpilot_updater_private/manifest.json.sig index 75f9dd1bf..76f1c4305 100644 --- a/artifacts/iqpilot_updater_private/manifest.json.sig +++ b/artifacts/iqpilot_updater_private/manifest.json.sig @@ -1 +1 @@ -JJCbPdLrTbJu+aVPIcsTJTbsltzP0Zl97r6EM77B7ebCsnS9yU1Tx9+NPui1OVVPiQAigyDxi/TZZyIYBWigDw== +0OdQnuSdywA/tTTIVWGwz2gpZ5H50RoDjjWrXlgYDivBPpTU83nj3jbFtFenrzVNDouRC/PbTvEFNwcnHAe8DA== diff --git a/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/_hf_maps_blob.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/_hf_maps_blob.cpython-312-aarch64-linux-gnu.so new file mode 100755 index 000000000..bd904d978 Binary files /dev/null and b/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/_hf_maps_blob.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/_iqos_pat_blob.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/_iqos_pat_blob.cpython-312-aarch64-linux-gnu.so index beca065eb..260d66a4b 100755 Binary files a/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/_iqos_pat_blob.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/_iqos_pat_blob.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/_pat_blob.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/_pat_blob.cpython-312-aarch64-linux-gnu.so index caf61ce78..ac5c3e8a2 100755 Binary files a/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/_pat_blob.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/_pat_blob.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/git_remote.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/git_remote.cpython-312-aarch64-linux-gnu.so index f1f2a6d89..7b8b609a0 100755 Binary files a/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/git_remote.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_updater_private/python/iqpilot_private/updater/git_remote.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/honda/interface.py b/artifacts/package_sources/iqdbc/iqdbc/car/honda/interface.py index 8c15c59ff..7b3003e54 100755 --- a/artifacts/package_sources/iqdbc/iqdbc/car/honda/interface.py +++ b/artifacts/package_sources/iqdbc/iqdbc/car/honda/interface.py @@ -5,7 +5,8 @@ from iqdbc.car.common.conversions import Conversions as CV from iqdbc.car.disable_ecu import disable_ecu, clear_all_dtcs, clear_ecu_dtcs from iqdbc.car.honda.hondacan import CanBus from iqdbc.car.honda.values import CarControllerParams, HondaFlags, CAR, HONDA_BOSCH, HONDA_BOSCH_CANFD, \ - HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS, HondaSafetyFlags + HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS, \ + HONDA_RADAR_SCAN_VERIFIED, HondaSafetyFlags from iqdbc.car.honda.carcontroller import CarController from iqdbc.car.honda.carstate import CarState from iqdbc.car.honda.radar_interface import RadarInterface @@ -48,7 +49,9 @@ class CarInterface(CarInterfaceBase): cfgs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput)) ret.safetyConfigs = cfgs - ret.radarUnavailable = True + # The object scan survives openpilot longitudinal: the radar disable is subnet-scoped to the + # powertrain bus, while the scan rides the camera-side ACC-CAN + ret.radarUnavailable = docs or candidate not in HONDA_RADAR_SCAN_VERIFIED # Disable the radar and let openpilot control longitudinal # WARNING: THIS DISABLES AEB! # If Bosch radarless, this blocks ACC messages from the camera diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/honda/radar_interface.py b/artifacts/package_sources/iqdbc/iqdbc/car/honda/radar_interface.py index 6eb47073a..d0ed30ece 100755 --- a/artifacts/package_sources/iqdbc/iqdbc/car/honda/radar_interface.py +++ b/artifacts/package_sources/iqdbc/iqdbc/car/honda/radar_interface.py @@ -2,6 +2,7 @@ from iqdbc.can import CANParser from iqdbc.car import Bus, structs from iqdbc.car.interfaces import RadarInterfaceBase +from iqdbc.car.honda.radar_scan import SCAN_DBC_NAME, HondaRadarScanner from iqdbc.car.honda.values import DBC @@ -18,13 +19,20 @@ class RadarInterface(RadarInterfaceBase): self.radar_fault = False self.radar_wrong_config = False self.radar_off_can = CP.radarUnavailable + self.scanner = None - # Nidec if self.radar_off_can: self.rcp = None + self.trigger_msg = 0x445 + elif DBC[CP.carFingerprint].get(Bus.radar) == SCAN_DBC_NAME: + self.scanner = HondaRadarScanner(CP) + self.rcp = self.scanner.rcp + self.pts = self.scanner.pts + self.trigger_msg = self.scanner.trigger_msg else: + # Nidec self.rcp = _create_nidec_can_parser(CP.carFingerprint) - self.trigger_msg = 0x445 + self.trigger_msg = 0x445 self.updated_messages = set() def update(self, can_strings): @@ -37,6 +45,8 @@ class RadarInterface(RadarInterfaceBase): self.updated_messages.update(vls) if self.trigger_msg not in self.updated_messages: + if self.scanner is not None and self.scanner.sweep_overdue(): + return self.scanner.quiet_bus_radardata() return None rr = self._update(self.updated_messages) @@ -44,6 +54,9 @@ class RadarInterface(RadarInterfaceBase): return rr def _update(self, updated_messages): + if self.scanner is not None: + return self.scanner.process_sweep(updated_messages) + ret = structs.RadarData() for ii in sorted(updated_messages): diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/honda/radar_scan.py b/artifacts/package_sources/iqdbc/iqdbc/car/honda/radar_scan.py new file mode 100644 index 000000000..cba826ab5 --- /dev/null +++ b/artifacts/package_sources/iqdbc/iqdbc/car/honda/radar_scan.py @@ -0,0 +1,396 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" +import math +from collections import deque +from dataclasses import dataclass, field + +from iqdbc.can import CANParser +from iqdbc.car import Bus, structs +from iqdbc.car.honda.hondacan import CanBus +from iqdbc.car.honda.values import DBC +from iqdbc.dbc.generator.honda.honda_radar_scan import QUARTET_KINDS, SCAN_SLOTS, frame_address + +SCAN_DBC_NAME = 'honda_radar_scan_generated' +SWEEP_HZ = 15 + +SLOT_ADDRS = [tuple(frame_address(slot, kind) for kind in (*QUARTET_KINDS, "MOTION")) for slot in range(SCAN_SLOTS)] +ALL_SCAN_ADDRS = [addr for addrs in SLOT_ADDRS for addr in addrs] +# slot 15's IDENT frame closes every observed sweep's quartet family; its MOTION companion follows +# but must never gate an otherwise valid sweep, so the quartet frame stays the trigger +SWEEP_TRIGGER_ADDR = SLOT_ADDRS[SCAN_SLOTS - 1][3] + +DIST_LSB_M = 0.05712 +DIST_BIAS_M = -3.0 +BEARING_LSB_RAD = 1.0 / 2048.0 +BEARING_ZERO = 1024 + +STATE_INVALID = 0xF +DIST_RAW_INVALID = 0xFFF +BEARING_RAW_INVALID = 0x7FF +AGE_RAW_INVALID = 0xFFF +HANDLE_MIN = 1 +HANDLE_MAX = 0x3F + +CLOSING_SPEED_RAW_INVALID = 0x7FE +CLOSING_SPEED_RAW_MIN = 0 +CLOSING_SPEED_RAW_MAX = 1728 +CLOSING_SPEED_RAW_ZERO = 864 +CLOSING_SPEED_LSB_MPS = 1.0 / 64.0 +# replay-derived quality gate: the native speed field degrades gradually with its sigma companion; +# above this the field is no longer authoritative and the decoder coasts instead +CLOSING_SPEED_SIGMA_TRUST_MAX = 511 + +DIST_RATIO_RAW_INVALID = 0x3FF +DIST_RATIO_LSB = 0.001 +DIST_RATIO_BIAS = 0.5 + +# replay-derived acceptance gates, not recovered firmware constants; innovation is measured from the +# previous ACCEPTED observation so a reset can never become the baseline for following sweeps +DIST_SIGMA_DEGRADED_RAW = 4 +DIST_INNOVATION_SOFT_M = 2.0 +DIST_INNOVATION_HARD_M = 5.0 +RAW_RATE_LIMIT_MPS = 50.0 + +HISTORY_LEN = 8 +QUIET_TIMEOUT_S = 0.20 + + +def decode_closing_speed(raw, sigma_raw=None): + if raw is None: + return None + raw = int(raw) + if raw == CLOSING_SPEED_RAW_INVALID or not CLOSING_SPEED_RAW_MIN <= raw <= CLOSING_SPEED_RAW_MAX: + return None + if sigma_raw is not None and int(sigma_raw) > CLOSING_SPEED_SIGMA_TRUST_MAX: + return None + return (raw - CLOSING_SPEED_RAW_ZERO) * CLOSING_SPEED_LSB_MPS + + +def decode_dist_ratio(raw): + if raw is None: + return None + raw = int(raw) + if raw == DIST_RATIO_RAW_INVALID or not 0 <= raw < DIST_RATIO_RAW_INVALID: + return None + return DIST_RATIO_BIAS + DIST_RATIO_LSB * raw + + +def ratio_implied_rate(raw, dist, dt): + ratio = decode_dist_ratio(raw) + if ratio is None or dt <= 0.0 or not math.isfinite(dist): + return None + return dist * (1.0 - ratio) / dt + + +def reading_degraded(dist_sigma_raw, presence_raw, speed_sigma_raw): + geometry_bad = dist_sigma_raw >= DIST_SIGMA_DEGRADED_RAW or presence_raw in (0, 0x7F) + motion_bad = speed_sigma_raw is not None and speed_sigma_raw > CLOSING_SPEED_SIGMA_TRUST_MAX + return geometry_bad or motion_bad + + +@dataclass +class SlotReading: + slot: int + cycle: int + age: int + handle: int + handle_ok: bool + coherent: bool + dist_raw: int = 0 + bearing_raw: int = 0 + dist_sigma_raw: int = 0 + presence_raw: int = 0 + speed_raw: int | None = None + speed_sigma_raw: int | None = None + ratio_raw: int | None = None + + +@dataclass +class ObjectLedger: + handle: int + prev_cycle: int | None = None + prev_age: int | None = None + last_seen_nanos: int | None = None + wire_slot: int | None = None + history: deque = field(default_factory=lambda: deque(maxlen=HISTORY_LEN)) + held_speed: float | None = None + held_speed_nanos: int | None = None + + def continues_incarnation(self, cycle: int, age: int) -> bool: + if self.prev_cycle is None or self.prev_age is None: + return False + cycle_delta = (cycle - self.prev_cycle) & 0xF + age_delta = (age - self.prev_age) & 0xFFF + return age_delta == 2 * cycle_delta + + def restart_incarnation(self): + self.history.clear() + self.held_speed = None + self.held_speed_nanos = None + + def held_speed_fresh(self, now: int) -> bool: + return (self.held_speed is not None and self.held_speed_nanos is not None and + (now - self.held_speed_nanos) * 1e-9 <= QUIET_TIMEOUT_S) + + +def create_scan_parser(CP) -> CANParser: + # the object scan is physically on the camera-side ACC-CAN + return CANParser(DBC[CP.carFingerprint][Bus.radar], [(addr, SWEEP_HZ) for addr in ALL_SCAN_ADDRS], CanBus(CP).camera) + + +class HondaRadarScanner: + def __init__(self, CP): + self.rcp = create_scan_parser(CP) + self.trigger_msg = SWEEP_TRIGGER_ADDR + self.pts: dict[int, structs.RadarData.RadarPoint] = {} + self._ledgers: dict[int, ObjectLedger] = {} + self._slot_handles: list[int | None] = [None] * SCAN_SLOTS + self._last_sweep_nanos = -1 + + def sweep_overdue(self) -> bool: + if self._last_sweep_nanos < 0: + return False + return (self.rcp._last_update_nanos - self._last_sweep_nanos) * 1e-9 > QUIET_TIMEOUT_S + + def quiet_bus_radardata(self) -> structs.RadarData: + # whole-bus silence: drop everything and emit an EMPTY RadarData (not None) so radard sheds any + # lead within a cycle instead of freezing a phantom + self.pts.clear() + self._ledgers.clear() + self._slot_handles = [None] * SCAN_SLOTS + self._last_sweep_nanos = -1 + ret = structs.RadarData() + if not self.rcp.can_valid: + ret.errors.canError = True + ret.errors.radarUnavailableTemporary = True + return ret + + def _drop_ledger(self, handle: int): + self._ledgers.pop(handle, None) + self.pts.pop(handle, None) + for slot, bound in enumerate(self._slot_handles): + if bound == handle: + self._slot_handles[slot] = None + + def _drop_expired_ledgers(self, now: int): + for handle, ledger in list(self._ledgers.items()): + if ledger.last_seen_nanos is not None and (now - ledger.last_seen_nanos) * 1e-9 > QUIET_TIMEOUT_S: + self._drop_ledger(handle) + + def _read_slot(self, slot: int, updated_messages) -> SlotReading | None: + pos, shape, life, ident, motion = SLOT_ADDRS[slot] + if not all(addr in updated_messages for addr in (pos, shape, life, ident)): + # a missing CAN frame is not a lifecycle event; ledgers expire on their own staleness only + return None + + v_pos, v_shape, v_life, v_ident = (self.rcp.vl[a] for a in (pos, shape, life, ident)) + cycle = int(v_pos['CYCLE']) + if not (cycle == int(v_shape['CYCLE']) == int(v_life['CYCLE']) == int(v_ident['CYCLE'])): + # the quartet doesn't share one radar cycle: not a coherent observation this window + return None + + state = int(v_pos['SCAN_STATE']) + dist_raw = int(v_pos['DIST_RAW']) + bearing_raw = int(v_pos['BEARING_RAW']) + age = int(v_life['AGE_RAW']) + handle = int(v_ident['OBJECT_HANDLE']) + reading = SlotReading( + slot=slot, + cycle=cycle, + age=age, + handle=handle, + handle_ok=HANDLE_MIN <= handle <= HANDLE_MAX, + coherent=(state != STATE_INVALID and dist_raw != DIST_RAW_INVALID and + bearing_raw != BEARING_RAW_INVALID and age != AGE_RAW_INVALID), + dist_raw=dist_raw, + bearing_raw=bearing_raw, + dist_sigma_raw=int(v_pos['DIST_SIGMA_RAW']), + presence_raw=int(v_shape['PRESENCE_RAW']), + ) + + # the MOTION companion only contributes when it rides the same cycle; its absence never + # invalidates the quartet, it only removes independent motion evidence + if motion in updated_messages: + v_motion = self.rcp.vl[motion] + if int(v_motion['CYCLE']) == cycle: + reading.speed_raw = int(v_motion['CLOSING_SPEED_RAW']) + reading.speed_sigma_raw = int(v_motion['CLOSING_SPEED_SIGMA_RAW']) + reading.ratio_raw = int(v_motion['DIST_RATIO_RAW']) + return reading + + def _elect_by_handle(self, readings: list[SlotReading]) -> dict[int, SlotReading]: + # one CAN identity can never yield two points; ties prefer the wire slot already bound to the + # ledger, then the lower slot for deterministic handling of a malformed duplicate + elected: dict[int, SlotReading] = {} + for reading in readings: + if not (reading.coherent and reading.handle_ok): + # an invalid observation ends publication for the slot's current occupant without destroying + # persistent state; the object may be multiplexed elsewhere or return before its deadline + hidden = {self._slot_handles[reading.slot]} + if reading.handle_ok: + hidden.add(reading.handle) + for handle in hidden - {None}: + self.pts.pop(handle, None) + continue + current = elected.get(reading.handle) + if current is None: + elected[reading.handle] = reading + continue + bound_slot = self._ledgers[reading.handle].wire_slot if reading.handle in self._ledgers else None + current_rank = (0 if bound_slot == current.slot else 1, current.slot) + candidate_rank = (0 if bound_slot == reading.slot else 1, reading.slot) + if candidate_rank < current_rank: + elected[reading.handle] = reading + return elected + + def _bind_slot(self, ledger: ObjectLedger, reading: SlotReading, now: int): + ledger.prev_cycle = reading.cycle + ledger.prev_age = reading.age + ledger.last_seen_nanos = now + ledger.wire_slot = reading.slot + for slot, bound in enumerate(self._slot_handles): + if slot != reading.slot and bound == ledger.handle: + self._slot_handles[slot] = None + self._slot_handles[reading.slot] = ledger.handle + + def _coast_point(self, ledger: ObjectLedger, now: int, dist: float, y_rel: float): + # coasting keeps trustworthy geometry visible with the last authoritative motion, unmeasured, + # instead of publishing a synthesized rate; without fresh held motion the point drops + if ledger.held_speed_fresh(now): + point = self.pts.get(ledger.handle) + if point is not None: + point.dRel = dist + point.yRel = y_rel + point.vRel = ledger.held_speed + point.measured = False + return + ledger.held_speed = None + ledger.held_speed_nanos = None + self.pts.pop(ledger.handle, None) + + def process_sweep(self, updated_messages) -> structs.RadarData: + ret = structs.RadarData() + if not self.rcp.can_valid: + ret.errors.canError = True + + now = self.rcp._last_update_nanos + self._last_sweep_nanos = now + self._drop_expired_ledgers(now) + + readings = [r for r in (self._read_slot(slot, updated_messages) for slot in range(SCAN_SLOTS)) if r is not None] + elected = self._elect_by_handle(readings) + elected_handles = set(elected) + + for handle, reading in sorted(elected.items(), key=lambda item: item[1].slot): + # a wire-slot replacement ends publication for the old occupant, but not its persistent state + old_handle = self._slot_handles[reading.slot] + if old_handle is not None and old_handle != handle and old_handle not in elected_handles: + self.pts.pop(old_handle, None) + + ledger = self._ledgers.get(handle) + if ledger is None: + ledger = ObjectLedger(handle=handle) + self._ledgers[handle] = ledger + + if not ledger.continues_incarnation(reading.cycle, reading.age): + # the CAN identity stays the external key, but a lifecycle discontinuity starts a new + # incarnation and must not inherit the previous object's range-rate history + ledger.restart_incarnation() + self.pts.pop(handle, None) + + dist = DIST_LSB_M * reading.dist_raw + DIST_BIAS_M + bearing = BEARING_LSB_RAD * (reading.bearing_raw - BEARING_ZERO) + # the radar consumes range as a forward-axis quantity; positive bearing is left of center, + # matching the RadarPoint.yRel sign contract + y_rel = dist * math.tan(bearing) + + now_s = now * 1e-9 + native_speed = decode_closing_speed(reading.speed_raw, reading.speed_sigma_raw) + unqualified_speed = decode_closing_speed(reading.speed_raw) + # a live native speed rejected only by its sigma companion: geometry acceptance is still decided + # on the same terms as every other sweep (high sigma correlates with bad/discontinuous range), + # and only then does the point coast instead of publishing a synthesized rate + sigma_veto = (native_speed is None and unqualified_speed is not None and + reading.speed_sigma_raw is not None and + reading.speed_sigma_raw > CLOSING_SPEED_SIGMA_TRUST_MAX) + + degraded = reading_degraded(reading.dist_sigma_raw, reading.presence_raw, reading.speed_sigma_raw) + previous = ledger.history[-1] if ledger.history else None + ratio_rate = None + dist_rejected = False + if previous is not None: + prev_time, prev_dist = previous + dt = now_s - prev_time + if dt <= 0.0: + dist_rejected = True + else: + ratio = decode_dist_ratio(reading.ratio_raw) + ratio_rate = ratio_implied_rate(reading.ratio_raw, dist, dt) + + residuals_m = [] + if native_speed is not None: + residuals_m.append(abs(dist - (prev_dist + native_speed * dt))) + if ratio is not None: + residuals_m.append(abs(prev_dist - dist * ratio)) + + if residuals_m: + innovation_m = min(residuals_m) + dist_rejected = (innovation_m > DIST_INNOVATION_HARD_M or + (degraded and innovation_m > DIST_INNOVATION_SOFT_M)) + else: + dist_rejected = abs((dist - prev_dist) / dt) > RAW_RATE_LIMIT_MPS + + if dist_rejected: + # keep the last accepted point briefly as an unmeasured coast; rejected geometry is never + # published and never becomes the baseline for a later derivative + accepted_fresh = previous is not None and now_s - previous[0] <= QUIET_TIMEOUT_S + point = self.pts.get(handle) + if accepted_fresh and point is not None: + point.measured = False + else: + self.pts.pop(handle, None) + self._bind_slot(ledger, reading, now) + continue + + if sigma_veto: + self._coast_point(ledger, now, dist, y_rel) + self._bind_slot(ledger, reading, now) + continue + + # with neither a qualified native speed nor a usable ratio, the only remaining source is the + # raw one-sweep derivative, which must never become an authoritative measurement: coast instead. + # A true birth (no previous accepted sample) cannot mature this cycle regardless, so only + # intercept once a derivative would have something to poison + if native_speed is None and (ratio_rate is None or degraded) and previous is not None: + self._coast_point(ledger, now, dist, y_rel) + self._bind_slot(ledger, reading, now) + continue + + ledger.history.append((now_s, dist)) + + speed = native_speed if native_speed is not None else ratio_rate + ledger.held_speed = speed + ledger.held_speed_nanos = now + + # a birth observation has no range rate yet: keep it as history, publish only once a second + # coherent observation of the same identity supplies a finite rate + matured = len(ledger.history) >= 2 and math.isfinite(speed) + if matured: + if handle not in self.pts: + point = structs.RadarData.RadarPoint() + point.trackId = handle + point.aRel = float('nan') + point.yvRel = float('nan') + self.pts[handle] = point + self.pts[handle].dRel = dist + self.pts[handle].yRel = y_rel + self.pts[handle].vRel = speed + self.pts[handle].measured = True + else: + self.pts.pop(handle, None) + + self._bind_slot(ledger, reading, now) + + ret.points = [self.pts[handle] for handle in sorted(self.pts)] + return ret diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/honda/tests/test_radar_scan.py b/artifacts/package_sources/iqdbc/iqdbc/car/honda/tests/test_radar_scan.py new file mode 100644 index 000000000..88ecf1d24 --- /dev/null +++ b/artifacts/package_sources/iqdbc/iqdbc/car/honda/tests/test_radar_scan.py @@ -0,0 +1,490 @@ +import math + +import pytest + +from iqdbc.can import CANParser +from iqdbc.car.honda.radar_scan import (AGE_RAW_INVALID, ALL_SCAN_ADDRS, BEARING_RAW_INVALID, BEARING_ZERO, + CLOSING_SPEED_RAW_INVALID, CLOSING_SPEED_RAW_ZERO, + CLOSING_SPEED_SIGMA_TRUST_MAX, DIST_BIAS_M, DIST_LSB_M, + DIST_RATIO_RAW_INVALID, DIST_RAW_INVALID, HondaRadarScanner, + QUIET_TIMEOUT_S, SCAN_DBC_NAME, SCAN_SLOTS, STATE_INVALID, + SWEEP_TRIGGER_ADDR, decode_closing_speed, decode_dist_ratio) +from iqdbc.dbc.generator.honda.honda_radar_scan import FRAME_SIGNALS, frame_address + +BUS = 2 +SWEEP_DT_NS = 66_000_000 + + +def set_bits(data, start_bit, size, value): + value = int(value) & ((1 << size) - 1) + pos = start_bit + for i in range(size): + bit = (value >> (size - 1 - i)) & 1 + byte_i, bit_i = pos // 8, pos % 8 + if bit: + data[byte_i] |= (1 << bit_i) + pos = pos - 1 if bit_i > 0 else pos + 15 + + +GEOMETRY = {kind: {name: (start, size) for name, start, size in sigs} for kind, sigs in FRAME_SIGNALS.items()} + + +def build_frame(slot, kind, **fields): + data = bytearray(8) + for name, value in fields.items(): + set_bits(data, *GEOMETRY[kind][name], value) + return (frame_address(slot, kind), bytes(data), BUS) + + +def quartet(slot, cycle, dist_raw=1000, bearing_raw=BEARING_ZERO, state=1, dist_sigma=0, presence=40, + age=100, handle=5): + return [ + build_frame(slot, "POS", SCAN_STATE=state, CYCLE=cycle, DIST_RAW=dist_raw, BEARING_RAW=bearing_raw, + DIST_SIGMA_RAW=dist_sigma), + build_frame(slot, "SHAPE", CYCLE=cycle, PRESENCE_RAW=presence), + build_frame(slot, "LIFE", CYCLE=cycle, AGE_RAW=age), + build_frame(slot, "IDENT", CYCLE=cycle, OBJECT_HANDLE=handle), + ] + + +def motion_frame(slot, cycle, speed_raw=CLOSING_SPEED_RAW_ZERO, sigma_raw=0, ratio_raw=500): + return build_frame(slot, "MOTION", CYCLE=cycle, CLOSING_SPEED_RAW=speed_raw, + CLOSING_SPEED_SIGMA_RAW=sigma_raw, DIST_RATIO_RAW=ratio_raw) + + +def closing_sweep(slot_msgs, cycle): + # slot 15's quartet closes every sweep so the trigger fires + msgs = list(slot_msgs) + if not any(m[0] == SWEEP_TRIGGER_ADDR for m in msgs): + msgs += quartet(15, cycle, state=STATE_INVALID, dist_raw=DIST_RAW_INVALID, + bearing_raw=BEARING_RAW_INVALID, age=AGE_RAW_INVALID, handle=0) + return msgs + + +class ScanHarness: + def __init__(self): + self.scanner = object.__new__(HondaRadarScanner) + self.scanner.rcp = CANParser(SCAN_DBC_NAME, [(a, 15) for a in ALL_SCAN_ADDRS], BUS) + self.scanner.trigger_msg = SWEEP_TRIGGER_ADDR + self.scanner.pts = {} + self.scanner._ledgers = {} + self.scanner._slot_handles = [None] * SCAN_SLOTS + self.scanner._last_sweep_nanos = -1 + self.updated = set() + self.nanos = 0 + self.cycle = 0 + + def feed(self, msgs, dt_ns=SWEEP_DT_NS): + self.nanos += dt_ns + vls = self.scanner.rcp.update([self.nanos, list(msgs)]) + self.updated.update(vls) + if self.scanner.trigger_msg not in self.updated: + if self.scanner.sweep_overdue(): + return self.scanner.quiet_bus_radardata() + return None + result = self.scanner.process_sweep(self.updated) + self.updated.clear() + return result + + def sweep(self, slot_msgs=(), cycle_step=1, dt_ns=SWEEP_DT_NS): + self.cycle = (self.cycle + cycle_step) & 0xF + return self.feed(closing_sweep(slot_msgs, self.cycle), dt_ns=dt_ns) + + def object_sweep(self, slot=0, handle=5, dist_raw=1000, with_motion=True, cycle_step=1, age_step=None, + dt_ns=SWEEP_DT_NS, **kwargs): + if age_step is None: + age_step = 2 * cycle_step + self._age = (getattr(self, "_age", 100) + age_step) & 0xFFF + cycle = (self.cycle + cycle_step) & 0xF + msgs = quartet(slot, cycle, dist_raw=dist_raw, age=self._age, handle=handle, **kwargs) + if with_motion: + msgs.append(motion_frame(slot, cycle)) + return self.sweep(msgs, cycle_step=cycle_step, dt_ns=dt_ns) + + +class TestFieldDecoding: + def test_dist_conversion(self): + assert DIST_LSB_M * 1000 + DIST_BIAS_M == pytest.approx(54.12) + + def test_closing_speed_decode_and_domain(self): + assert decode_closing_speed(CLOSING_SPEED_RAW_ZERO) == 0.0 + assert decode_closing_speed(CLOSING_SPEED_RAW_ZERO + 64) == 1.0 + assert decode_closing_speed(CLOSING_SPEED_RAW_INVALID) is None + assert decode_closing_speed(1729) is None + assert decode_closing_speed(None) is None + + def test_closing_speed_sigma_veto(self): + assert decode_closing_speed(CLOSING_SPEED_RAW_ZERO, CLOSING_SPEED_SIGMA_TRUST_MAX) == 0.0 + assert decode_closing_speed(CLOSING_SPEED_RAW_ZERO, CLOSING_SPEED_SIGMA_TRUST_MAX + 1) is None + + def test_dist_ratio_decode(self): + assert decode_dist_ratio(500) == pytest.approx(1.0) + assert decode_dist_ratio(DIST_RATIO_RAW_INVALID) is None + assert decode_dist_ratio(None) is None + + def test_bearing_sign_convention(self): + h = ScanHarness() + h.object_sweep(bearing_raw=BEARING_ZERO + 100) + result = h.object_sweep(bearing_raw=BEARING_ZERO + 100) + assert result.points[0].yRel > 0 # left of center is positive + dist = result.points[0].dRel + assert result.points[0].yRel == pytest.approx(dist * math.tan(100 / 2048)) + + def test_bearing_right_of_center_is_negative(self): + h = ScanHarness() + h.object_sweep(bearing_raw=BEARING_ZERO - 100) + result = h.object_sweep(bearing_raw=BEARING_ZERO - 100) + assert result.points[0].yRel < 0 + + def test_boresight_is_zero(self): + h = ScanHarness() + h.object_sweep(bearing_raw=BEARING_ZERO) + result = h.object_sweep(bearing_raw=BEARING_ZERO) + assert result.points[0].yRel == 0.0 + + +class TestPublicationRules: + def test_birth_is_withheld_until_second_observation(self): + h = ScanHarness() + result = h.object_sweep() + assert len(result.points) == 0 + result = h.object_sweep() + assert len(result.points) == 1 + point = result.points[0] + assert point.trackId == 5 + assert point.measured + assert math.isnan(point.aRel) and math.isnan(point.yvRel) + + def test_handle_is_wire_identity_not_synthetic(self): + h = ScanHarness() + h.object_sweep(handle=0x22) + result = h.object_sweep(handle=0x22) + assert result.points[0].trackId == 0x22 + + @pytest.mark.parametrize("field,value", [("state", STATE_INVALID), ("dist_raw", DIST_RAW_INVALID), + ("bearing_raw", BEARING_RAW_INVALID)]) + def test_sentinels_invalidate_observation(self, field, value): + h = ScanHarness() + h.object_sweep() + h.object_sweep() + kwargs = {field: value} + result = h.object_sweep(**kwargs) + assert len(result.points) == 0 + + def test_age_sentinel_invalidates_observation(self): + h = ScanHarness() + h.object_sweep() + h.object_sweep() + cycle = (h.cycle + 1) & 0xF + msgs = quartet(0, cycle, age=AGE_RAW_INVALID, handle=5) + [motion_frame(0, cycle)] + result = h.sweep(msgs) + assert len(result.points) == 0 + + @pytest.mark.parametrize("handle", [0, 0x40, 0xFF]) + def test_out_of_range_handle_invalidates(self, handle): + h = ScanHarness() + h.object_sweep() + h.object_sweep() + result = h.object_sweep(handle=handle) + assert len(result.points) == 0 + + def test_incomplete_quartet_is_not_an_observation(self): + h = ScanHarness() + h.object_sweep() + h.object_sweep() + cycle = (h.cycle + 1) & 0xF + h._age = (h._age + 2) & 0xFFF + msgs = quartet(0, cycle, age=h._age, handle=5)[:3] # drop IDENT + result = h.sweep(msgs) + # a dropped CAN frame is not a lifecycle event: the published point persists untouched + assert len(result.points) == 1 + result = h.object_sweep() + assert len(result.points) == 1 + assert result.points[0].measured + + def test_cycle_mismatch_across_quartet_is_incoherent(self): + h = ScanHarness() + h.object_sweep() + h.object_sweep() + cycle = (h.cycle + 1) & 0xF + msgs = quartet(0, cycle, age=200, handle=5) + bad_life = build_frame(0, "LIFE", CYCLE=(cycle + 1) & 0xF, AGE_RAW=200) + msgs[2] = bad_life + result = h.sweep(msgs) + # an incoherent quartet is not an observation: the published point persists untouched + assert len(result.points) == 1 + assert result.points[0].measured + + +class TestLifecycle: + def test_age_advances_two_per_cycle_keeps_identity(self): + h = ScanHarness() + h.object_sweep() + h.object_sweep() + result = h.object_sweep() + assert len(result.points) == 1 + + def test_continuity_across_skipped_cycles(self): + h = ScanHarness() + h.object_sweep() + h.object_sweep() + result = h.object_sweep(cycle_step=3, age_step=6) + assert len(result.points) == 1 + + def test_cycle_and_age_wraparound_stay_same_incarnation(self): + h = ScanHarness() + h.cycle = 14 + h._age = 4094 + h.object_sweep() # cycle 15, age 4094+2 wraps + h.object_sweep() # cycle 0 + result = h.object_sweep() + assert len(result.points) == 1 + + def test_lifecycle_break_starts_new_incarnation(self): + h = ScanHarness() + h.object_sweep() + h.object_sweep() + # same handle, age jumps arbitrarily: history must not carry over, so no publication this sweep + result = h.object_sweep(age_step=500) + assert len(result.points) == 0 + result = h.object_sweep() + assert len(result.points) == 1 + + def test_death_then_rebirth_reuses_handle_with_clean_history(self): + h = ScanHarness() + h.object_sweep() + h.object_sweep() + for _ in range(4): + h.sweep() # object absent long enough to expire its ledger + result = h.object_sweep() + assert len(result.points) == 0 + result = h.object_sweep() + assert len(result.points) == 1 + + +class TestMotionPolicy: + def test_native_speed_is_published(self): + h = ScanHarness() + speed_raw = CLOSING_SPEED_RAW_ZERO + 128 + cycle = (h.cycle + 1) & 0xF + h.sweep(quartet(0, cycle, age=100, handle=5) + [motion_frame(0, cycle, speed_raw=speed_raw)]) + cycle = (h.cycle + 1) & 0xF + result = h.sweep(quartet(0, cycle, age=102, handle=5) + [motion_frame(0, cycle, speed_raw=speed_raw)]) + assert result.points[0].vRel == pytest.approx(2.0) + assert result.points[0].measured + + def test_missing_motion_frame_never_invalidates_geometry(self): + h = ScanHarness() + h.object_sweep(with_motion=False) + result = h.object_sweep(with_motion=False) + # without any motion source and no held speed, the point is withheld rather than synthesized + assert len(result.points) == 0 + + def test_stale_motion_cycle_is_ignored(self): + h = ScanHarness() + h.object_sweep() + h.object_sweep() + cycle = (h.cycle + 1) & 0xF + h._age = (h._age + 2) & 0xFFF + msgs = quartet(0, cycle, age=h._age, handle=5) + [motion_frame(0, (cycle - 1) & 0xF)] + result = h.sweep(msgs) + # motion from another cycle contributes nothing: coasts on held speed, unmeasured + assert len(result.points) == 1 + assert not result.points[0].measured + + def test_high_sigma_speed_coasts_instead_of_synthesizing(self): + h = ScanHarness() + h.object_sweep() + h.object_sweep() + cycle = (h.cycle + 1) & 0xF + h._age = (h._age + 2) & 0xFFF + msgs = quartet(0, cycle, age=h._age, handle=5) + \ + [motion_frame(0, cycle, sigma_raw=CLOSING_SPEED_SIGMA_TRUST_MAX + 1)] + result = h.sweep(msgs) + assert len(result.points) == 1 + assert not result.points[0].measured + assert result.points[0].vRel == pytest.approx(0.0) # the held speed, not a derivative + + def test_ratio_field_supplies_speed_when_native_missing(self): + h = ScanHarness() + dist_raw = 1000 + cycle = (h.cycle + 1) & 0xF + h.sweep(quartet(0, cycle, dist_raw=dist_raw, age=100, handle=5) + + [motion_frame(0, cycle, speed_raw=CLOSING_SPEED_RAW_INVALID, ratio_raw=490)]) + cycle = (h.cycle + 1) & 0xF + result = h.sweep(quartet(0, cycle, dist_raw=dist_raw, age=102, handle=5) + + [motion_frame(0, cycle, speed_raw=CLOSING_SPEED_RAW_INVALID, ratio_raw=490)]) + assert len(result.points) == 1 + dist = DIST_LSB_M * dist_raw + DIST_BIAS_M + dt = SWEEP_DT_NS * 1e-9 + assert result.points[0].vRel == pytest.approx(dist * (1.0 - 0.99) / dt) + assert result.points[0].measured + + def test_fast_clean_range_rate_without_sources_is_withheld(self): + h = ScanHarness() + h.object_sweep(with_motion=False, dist_raw=1000) + # large clean jump with no motion evidence: raw-rate limit rejects the range outright + result = h.object_sweep(with_motion=False, dist_raw=3000) + assert len(result.points) == 0 + + +class TestRangeAcceptance: + def test_discontinuity_is_rejected_and_never_becomes_baseline(self): + h = ScanHarness() + h.object_sweep(dist_raw=1000) + h.object_sweep(dist_raw=1002) + # jump far beyond the hard innovation gate while claiming zero closing speed + result = h.object_sweep(dist_raw=3000) + assert len(result.points) == 1 + assert not result.points[0].measured + # the rejected range did not become the derivative baseline: returning to the + # consistent range publishes measured again + result = h.object_sweep(dist_raw=1004) + assert result.points[0].measured + + def test_small_innovation_accepted(self): + h = ScanHarness() + h.object_sweep(dist_raw=1000) + result = h.object_sweep(dist_raw=1005) + assert result.points[0].measured + + +class TestSlotsAndIdentity: + def test_slot_migration_preserves_identity(self): + h = ScanHarness() + h.object_sweep(slot=2) + h.object_sweep(slot=2) + result = h.object_sweep(slot=9) + assert len(result.points) == 1 + assert result.points[0].trackId == 5 + + def test_duplicate_identity_prefers_bound_slot(self): + h = ScanHarness() + h.object_sweep(slot=2, dist_raw=1000) + h.object_sweep(slot=2, dist_raw=1002) + cycle = (h.cycle + 1) & 0xF + h._age = (h._age + 2) & 0xFFF + msgs = quartet(2, cycle, dist_raw=1004, age=h._age, handle=5) + [motion_frame(2, cycle)] + \ + quartet(9, cycle, dist_raw=2000, age=h._age, handle=5) + [motion_frame(9, cycle)] + result = h.sweep(msgs) + assert len(result.points) == 1 + assert result.points[0].dRel == pytest.approx(DIST_LSB_M * 1004 + DIST_BIAS_M) + + def test_slot_replacement_hides_old_occupant(self): + h = ScanHarness() + h.object_sweep(slot=3, handle=7) + h.object_sweep(slot=3, handle=7) + # a different identity takes the slot; the old one is hidden but not destroyed + result = h.object_sweep(slot=3, handle=9, age_step=333) + assert all(p.trackId != 7 for p in result.points) + + def test_one_identity_never_two_points(self): + h = ScanHarness() + cycle = (h.cycle + 1) & 0xF + msgs = quartet(1, cycle, age=100, handle=5) + [motion_frame(1, cycle)] + \ + quartet(6, cycle, age=100, handle=5) + [motion_frame(6, cycle)] + h.sweep(msgs) + cycle = (h.cycle + 1) & 0xF + msgs = quartet(1, cycle, age=102, handle=5) + [motion_frame(1, cycle)] + \ + quartet(6, cycle, age=102, handle=5) + [motion_frame(6, cycle)] + result = h.sweep(msgs) + assert len(result.points) == 1 + + +class TestBusSilence: + def test_quiet_bus_publishes_empty_not_none(self): + h = ScanHarness() + h.object_sweep() + h.object_sweep() + result = None + for _ in range(30): + result = h.feed([], dt_ns=10_000_000) + if result is not None: + break + assert result is not None + assert result.errors.radarUnavailableTemporary + assert len(result.points) == 0 + + def test_recovery_after_silence_starts_fresh(self): + h = ScanHarness() + h.object_sweep() + h.object_sweep() + for _ in range(30): + if h.feed([], dt_ns=10_000_000) is not None: + break + result = h.object_sweep() + assert len(result.points) == 0 + result = h.object_sweep() + assert len(result.points) == 1 + + def test_no_stale_publication_before_first_sweep(self): + h = ScanHarness() + for _ in range(50): + assert h.feed([], dt_ns=10_000_000) is None + + +class TestQuietTimeoutValue: + def test_timeout_is_about_three_sweeps(self): + assert QUIET_TIMEOUT_S == pytest.approx(3 / 15, abs=0.01) + + +class TestScanInterfaceGating: + def build(self, candidate, alpha_long=False, docs=False): + from iqdbc.car import gen_empty_fingerprint + from iqdbc.car.honda.interface import CarInterface + CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], alpha_long, False, docs) + return CP + + def test_verified_platform_has_radar(self): + from iqdbc.car.honda.values import CAR + for car in (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_ACCORD, CAR.HONDA_CRV_5G): + assert not self.build(car).radarUnavailable + + def test_radar_survives_openpilot_longitudinal(self): + from iqdbc.car.honda.values import CAR + CP = self.build(CAR.HONDA_CIVIC_BOSCH, alpha_long=True) + assert CP.openpilotLongitudinalControl + assert not CP.radarUnavailable + + def test_unverified_family_platform_stays_off(self): + from iqdbc.car.honda.values import CAR + for car in (CAR.HONDA_E, CAR.HONDA_INSIGHT, CAR.HONDA_NBOX_2G, CAR.ACURA_RDX_3G, CAR.HONDA_CRV_HYBRID): + assert self.build(car).radarUnavailable + + def test_radarless_and_canfd_stay_off(self): + from iqdbc.car.honda.values import CAR + assert self.build(CAR.HONDA_CIVIC_2022).radarUnavailable + assert self.build(CAR.HONDA_CRV_6G).radarUnavailable + + def test_docs_never_claim_radar(self): + from iqdbc.car.honda.values import CAR + assert self.build(CAR.HONDA_CIVIC_BOSCH, docs=True).radarUnavailable + + def test_radar_interface_routes_scanner(self): + from iqdbc.car import gen_empty_fingerprint + from iqdbc.car.honda.interface import CarInterface + from iqdbc.car.honda.values import CAR + CP = self.build(CAR.HONDA_CIVIC_BOSCH) + CP_IQ = CarInterface.get_params_iq(CP, CAR.HONDA_CIVIC_BOSCH, gen_empty_fingerprint(), [], False, False, False) + ri = CarInterface.RadarInterface(CP, CP_IQ) + assert ri.scanner is not None + assert ri.trigger_msg == SWEEP_TRIGGER_ADDR + + def test_radar_interface_keeps_nidec_path(self): + from iqdbc.car import gen_empty_fingerprint + from iqdbc.car.honda.interface import CarInterface + from iqdbc.car.honda.values import CAR + CP = self.build(CAR.HONDA_CIVIC) + CP_IQ = CarInterface.get_params_iq(CP, CAR.HONDA_CIVIC, gen_empty_fingerprint(), [], False, False, False) + ri = CarInterface.RadarInterface(CP, CP_IQ) + assert ri.scanner is None + assert ri.trigger_msg == 0x445 + + def test_radar_interface_sleeps_when_unavailable(self): + from iqdbc.car import gen_empty_fingerprint + from iqdbc.car.honda.interface import CarInterface + from iqdbc.car.honda.values import CAR + CP = self.build(CAR.HONDA_E) + CP_IQ = CarInterface.get_params_iq(CP, CAR.HONDA_E, gen_empty_fingerprint(), [], False, False, False) + ri = CarInterface.RadarInterface(CP, CP_IQ) + assert ri.scanner is None and ri.rcp is None diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/honda/tests/test_radar_scan_dbc.py b/artifacts/package_sources/iqdbc/iqdbc/car/honda/tests/test_radar_scan_dbc.py new file mode 100644 index 000000000..9ec752e56 --- /dev/null +++ b/artifacts/package_sources/iqdbc/iqdbc/car/honda/tests/test_radar_scan_dbc.py @@ -0,0 +1,84 @@ +from iqdbc.can.dbc import DBC as DbcFile +from iqdbc.car import Bus +from iqdbc.car.honda.values import CAR, DBC, HONDA_RADAR_SCAN_CAPABLE, HONDA_RADAR_SCAN_VERIFIED +from iqdbc.dbc.generator.honda.honda_radar_scan import (FRAME_SIGNALS, QUARTET_KINDS, SCAN_SLOTS, + frame_address, motion_address, quartet_base_address) + +SCAN_DBC_NAME = 'honda_radar_scan_generated' + + +class TestScanAddressing: + def test_quartet_bases(self): + assert [quartet_base_address(s) for s in range(SCAN_SLOTS)] == \ + [0x280, 0x284, 0x288, 0x28C, 0x2D0, 0x2D4, 0x2D8, 0x2DC, 0x2E0, 0x2E4, 0x2E8, 0x2EC, 0x2F0, 0x2F4, 0x2F8, 0x2FC] + + def test_motion_addresses(self): + assert [motion_address(s) for s in range(SCAN_SLOTS)] == \ + [0x2C8, 0x2C9, 0x2CA, 0x2CB, 0x2CC, 0x2CD, 0x2CE, 0x2CF, 0x290, 0x291, 0x292, 0x293, 0x294, 0x295, 0x296, 0x297] + + def test_eighty_unique_addresses(self): + addrs = [frame_address(s, k) for s in range(SCAN_SLOTS) for k in (*QUARTET_KINDS, "MOTION")] + assert len(addrs) == 80 + assert len(set(addrs)) == 80 + + def test_quartet_kind_order(self): + for slot in range(SCAN_SLOTS): + base = quartet_base_address(slot) + assert [frame_address(slot, k) for k in QUARTET_KINDS] == [base, base + 1, base + 2, base + 3] + + +class TestScanDbcGeometry: + def setup_method(self): + self.dbc = DbcFile(SCAN_DBC_NAME) + + def geometry(self, addr): + msg = self.dbc.addr_to_msg[addr] + return {sig.name: (sig.start_bit, sig.size) for sig in msg.sigs.values()} + + def test_every_frame_present_with_size_8(self): + for slot in range(SCAN_SLOTS): + for kind in (*QUARTET_KINDS, "MOTION"): + msg = self.dbc.addr_to_msg[frame_address(slot, kind)] + assert msg.name == f"RADAR_SCAN_{slot:02d}_{kind}" + assert msg.size == 8 + + def test_bit_geometry_matches_spec(self): + expected = {kind: {name: (start, size) for name, start, size in sigs} for kind, sigs in FRAME_SIGNALS.items()} + for slot in range(SCAN_SLOTS): + for kind in (*QUARTET_KINDS, "MOTION"): + assert self.geometry(frame_address(slot, kind)) == expected[kind], (slot, kind) + + def test_pos_frame_field_widths(self): + geo = self.geometry(frame_address(0, "POS")) + assert geo["DIST_RAW"] == (23, 12) + assert geo["BEARING_RAW"] == (39, 11) + assert geo["SCAN_STATE"] == (15, 4) + assert geo["DIST_SIGMA_RAW"] == (7, 7) + + def test_ident_handle_is_byte_six(self): + geo = self.geometry(frame_address(0, "IDENT")) + assert geo["OBJECT_HANDLE"] == (55, 8) + + def test_motion_field_widths(self): + geo = self.geometry(frame_address(0, "MOTION")) + assert geo["CLOSING_SPEED_RAW"] == (7, 11) + assert geo["CLOSING_SPEED_SIGMA_RAW"] == (23, 10) + assert geo["DIST_RATIO_RAW"] == (55, 10) + + def test_cycle_positions_per_kind(self): + positions = {"POS": (27, 4), "SHAPE": (28, 4), "LIFE": (11, 4), "IDENT": (12, 4), "MOTION": (12, 4)} + for kind, expected in positions.items(): + assert self.geometry(frame_address(3, kind))["CYCLE"] == expected + + +class TestScanPlatformWiring: + def test_scan_dbc_on_exactly_the_capable_family(self): + for car in CAR: + has_scan_dbc = DBC[car].get(Bus.radar) == SCAN_DBC_NAME + assert has_scan_dbc == (car in HONDA_RADAR_SCAN_CAPABLE), car + + def test_verified_platforms_are_capable(self): + assert HONDA_RADAR_SCAN_VERIFIED <= HONDA_RADAR_SCAN_CAPABLE + + def test_verified_set(self): + assert HONDA_RADAR_SCAN_VERIFIED == {CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G} diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/honda/values.py b/artifacts/package_sources/iqdbc/iqdbc/car/honda/values.py index de27848f8..5b29df2dd 100644 --- a/artifacts/package_sources/iqdbc/iqdbc/car/honda/values.py +++ b/artifacts/package_sources/iqdbc/iqdbc/car/honda/values.py @@ -162,7 +162,7 @@ class CAR(Platforms): HondaCarDocs("Honda N-Box 2018", "All", min_steer_speed=5.), ], CarSpecs(mass=890., wheelbase=2.520, steerRatio=18.64), - {Bus.pt: 'acura_rdx_2020_can_generated'}, + {Bus.pt: 'acura_rdx_2020_can_generated', Bus.radar: 'honda_radar_scan_generated'}, ) HONDA_ACCORD = HondaBoschPlatformConfig( [ @@ -172,7 +172,7 @@ class CAR(Platforms): ], # steerRatio: 11.82 is spec end-to-end CarSpecs(mass=3279 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=16.33, centerToFrontRatio=0.39, tireStiffnessFactor=0.8467), - {Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated'}, + {Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated', Bus.radar: 'honda_radar_scan_generated'}, flags=HondaFlags.ALLOW_MANUAL_TRANS, ) HONDA_ACCORD_11G = HondaBoschCANFDPlatformConfig( @@ -190,13 +190,13 @@ class CAR(Platforms): HondaCarDocs("Honda Civic Hatchback 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS), ], CarSpecs(mass=1326, wheelbase=2.7, steerRatio=15.38, centerToFrontRatio=0.4), # steerRatio: 10.93 is end-to-end spec - {Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated'}, + {Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated', Bus.radar: 'honda_radar_scan_generated'}, flags=HondaFlags.ALLOW_MANUAL_TRANS, ) HONDA_CIVIC_BOSCH_DIESEL = HondaBoschPlatformConfig( [], # don't show in docs HONDA_CIVIC_BOSCH.specs, - {Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated'}, + {Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated', Bus.radar: 'honda_radar_scan_generated'}, ) HONDA_CIVIC_2022 = HondaBoschPlatformConfig( [ @@ -215,7 +215,8 @@ class CAR(Platforms): [HondaCarDocs("Honda CR-V 2017-22", min_steer_speed=15. * CV.MPH_TO_MS)], # steerRatio: 12.3 is spec end-to-end CarSpecs(mass=3410 * CV.LB_TO_KG, wheelbase=2.66, steerRatio=16.0, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), - {Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated', Bus.body: 'honda_crv_ex_2017_body_generated'}, + {Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated', Bus.body: 'honda_crv_ex_2017_body_generated', + Bus.radar: 'honda_radar_scan_generated'}, flags=HondaFlags.BOSCH_ALT_BRAKE, ) HONDA_CRV_6G = HondaBoschCANFDPlatformConfig( @@ -229,7 +230,7 @@ class CAR(Platforms): [HondaCarDocs("Honda CR-V Hybrid 2017-22", min_steer_speed=12. * CV.MPH_TO_MS)], # mass: mean of 4 models in kg, steerRatio: 12.3 is spec end-to-end CarSpecs(mass=1667, wheelbase=2.66, steerRatio=16, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), - {Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated'}, + {Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated', Bus.radar: 'honda_radar_scan_generated'}, ) HONDA_HRV_3G = HondaBoschPlatformConfig( [HondaCarDocs("Honda HR-V 2023-25", "All")], @@ -246,18 +247,18 @@ class CAR(Platforms): ACURA_RDX_3G = HondaBoschPlatformConfig( [HondaCarDocs("Acura RDX 2019-21", "All", min_steer_speed=3. * CV.MPH_TO_MS)], CarSpecs(mass=4068 * CV.LB_TO_KG, wheelbase=2.75, steerRatio=11.95, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), # as spec - {Bus.pt: 'acura_rdx_2020_can_generated'}, + {Bus.pt: 'acura_rdx_2020_can_generated', Bus.radar: 'honda_radar_scan_generated'}, flags=HondaFlags.BOSCH_ALT_BRAKE, ) HONDA_INSIGHT = HondaBoschPlatformConfig( [HondaCarDocs("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS)], CarSpecs(mass=2987 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.0, centerToFrontRatio=0.39, tireStiffnessFactor=0.82), # as spec - {Bus.pt: 'honda_insight_ex_2019_can_generated'}, + {Bus.pt: 'honda_insight_ex_2019_can_generated', Bus.radar: 'honda_radar_scan_generated'}, ) HONDA_E = HondaBoschPlatformConfig( [HondaCarDocs("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS)], CarSpecs(mass=3338.8 * CV.LB_TO_KG, wheelbase=2.5, centerToFrontRatio=0.5, steerRatio=16.71, tireStiffnessFactor=0.82), - {Bus.pt: 'acura_rdx_2020_can_generated'}, + {Bus.pt: 'acura_rdx_2020_can_generated', Bus.radar: 'honda_radar_scan_generated'}, ) HONDA_PILOT_4G = HondaBoschCANFDPlatformConfig( [HondaCarDocs("Honda Pilot 2023-25", "All")], @@ -387,6 +388,11 @@ HONDA_BOSCH_CANFD = CAR.with_flags(HondaFlags.BOSCH_CANFD) HONDA_BOSCH_ALT_RADAR = CAR.with_flags(HondaFlags.BOSCH_ALT_RADAR) HONDA_BOSCH_TJA_CONTROL = CAR.with_flags(HondaFlags.BOSCH_TJA_CONTROL) +# Bosch harness family whose radar broadcasts the decodable 16-slot object scan +HONDA_RADAR_SCAN_CAPABLE = HONDA_BOSCH - HONDA_BOSCH_RADARLESS - HONDA_BOSCH_CANFD - HONDA_BOSCH_ALT_RADAR +# scan decode stays off per platform until a real route capture has been validated +HONDA_RADAR_SCAN_VERIFIED = frozenset({CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G}) + DBC = CAR.create_dbc_map() diff --git a/artifacts/package_sources/iqdbc/iqdbc/dbc/generator/honda/honda_radar_scan.dbc b/artifacts/package_sources/iqdbc/iqdbc/dbc/generator/honda/honda_radar_scan.dbc new file mode 100644 index 000000000..e49279cc3 --- /dev/null +++ b/artifacts/package_sources/iqdbc/iqdbc/dbc/generator/honda/honda_radar_scan.dbc @@ -0,0 +1,436 @@ +VERSION "honda_radar_scan -- Honda Bosch radar object scan: 16 object slots, each a POS/SHAPE/LIFE/IDENT frame quartet plus a synchronized MOTION companion. Raw counts only; physical conversion lives in iqdbc/car/honda/radar_scan.py" + +NS_ : + NS_DESC_ + CM_ + BA_DEF_ + BA_ + VAL_ + CAT_DEF_ + CAT_ + FILTER + BA_DEF_DEF_ + EV_DATA_ + ENVVAR_DATA_ + SGTYPE_ + SGTYPE_VAL_ + BA_DEF_SGTYPE_ + BA_SGTYPE_ + SIG_TYPE_REF_ + VAL_TABLE_ + SIG_GROUP_ + SIG_VALTYPE_ + SIGTYPE_VALTYPE_ + BO_TX_BU_ + BA_DEF_REL_ + BA_REL_ + BA_DEF_DEF_REL_ + BU_SG_REL_ + BU_EV_REL_ + BU_BO_REL_ + SG_MUL_VAL_ + +BS_: + +BU_: SCANNER XXX + + +BO_ 640 RADAR_SCAN_00_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 641 RADAR_SCAN_00_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 642 RADAR_SCAN_00_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 643 RADAR_SCAN_00_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 712 RADAR_SCAN_00_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 644 RADAR_SCAN_01_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 645 RADAR_SCAN_01_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 646 RADAR_SCAN_01_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 647 RADAR_SCAN_01_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 713 RADAR_SCAN_01_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 648 RADAR_SCAN_02_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 649 RADAR_SCAN_02_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 650 RADAR_SCAN_02_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 651 RADAR_SCAN_02_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 714 RADAR_SCAN_02_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 652 RADAR_SCAN_03_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 653 RADAR_SCAN_03_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 654 RADAR_SCAN_03_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 655 RADAR_SCAN_03_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 715 RADAR_SCAN_03_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 720 RADAR_SCAN_04_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 721 RADAR_SCAN_04_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 722 RADAR_SCAN_04_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 723 RADAR_SCAN_04_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 716 RADAR_SCAN_04_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 724 RADAR_SCAN_05_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 725 RADAR_SCAN_05_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 726 RADAR_SCAN_05_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 727 RADAR_SCAN_05_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 717 RADAR_SCAN_05_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 728 RADAR_SCAN_06_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 729 RADAR_SCAN_06_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 730 RADAR_SCAN_06_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 731 RADAR_SCAN_06_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 718 RADAR_SCAN_06_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 732 RADAR_SCAN_07_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 733 RADAR_SCAN_07_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 734 RADAR_SCAN_07_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 735 RADAR_SCAN_07_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 719 RADAR_SCAN_07_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 736 RADAR_SCAN_08_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 737 RADAR_SCAN_08_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 738 RADAR_SCAN_08_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 739 RADAR_SCAN_08_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 656 RADAR_SCAN_08_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 740 RADAR_SCAN_09_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 741 RADAR_SCAN_09_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 742 RADAR_SCAN_09_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 743 RADAR_SCAN_09_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 657 RADAR_SCAN_09_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 744 RADAR_SCAN_10_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 745 RADAR_SCAN_10_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 746 RADAR_SCAN_10_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 747 RADAR_SCAN_10_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 658 RADAR_SCAN_10_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 748 RADAR_SCAN_11_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 749 RADAR_SCAN_11_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 750 RADAR_SCAN_11_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 751 RADAR_SCAN_11_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 659 RADAR_SCAN_11_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 752 RADAR_SCAN_12_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 753 RADAR_SCAN_12_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 754 RADAR_SCAN_12_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 755 RADAR_SCAN_12_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 660 RADAR_SCAN_12_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 756 RADAR_SCAN_13_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 757 RADAR_SCAN_13_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 758 RADAR_SCAN_13_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 759 RADAR_SCAN_13_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 661 RADAR_SCAN_13_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 760 RADAR_SCAN_14_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 761 RADAR_SCAN_14_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 762 RADAR_SCAN_14_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 763 RADAR_SCAN_14_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 662 RADAR_SCAN_14_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX + +BO_ 764 RADAR_SCAN_15_POS: 8 SCANNER + SG_ SCAN_STATE : 15|4@0+ (1,0) [0|15] "" XXX + SG_ CYCLE : 27|4@0+ (1,0) [0|15] "" XXX + SG_ DIST_RAW : 23|12@0+ (1,0) [0|4095] "" XXX + SG_ BEARING_RAW : 39|11@0+ (1,0) [0|2047] "" XXX + SG_ DIST_SIGMA_RAW : 7|7@0+ (1,0) [0|127] "" XXX + +BO_ 765 RADAR_SCAN_15_SHAPE: 8 SCANNER + SG_ CYCLE : 28|4@0+ (1,0) [0|15] "" XXX + SG_ PRESENCE_RAW : 46|7@0+ (1,0) [0|127] "" XXX + +BO_ 766 RADAR_SCAN_15_LIFE: 8 SCANNER + SG_ CYCLE : 11|4@0+ (1,0) [0|15] "" XXX + SG_ AGE_RAW : 7|12@0+ (1,0) [0|4095] "" XXX + +BO_ 767 RADAR_SCAN_15_IDENT: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ OBJECT_HANDLE : 55|8@0+ (1,0) [0|255] "" XXX + +BO_ 663 RADAR_SCAN_15_MOTION: 8 SCANNER + SG_ CYCLE : 12|4@0+ (1,0) [0|15] "" XXX + SG_ CLOSING_SPEED_RAW : 7|11@0+ (1,0) [0|2047] "" XXX + SG_ CLOSING_SPEED_SIGMA_RAW : 23|10@0+ (1,0) [0|1023] "" XXX + SG_ DIST_RATIO_RAW : 55|10@0+ (1,0) [0|1023] "" XXX diff --git a/artifacts/package_sources/iqdbc/iqdbc/dbc/generator/honda/honda_radar_scan.py b/artifacts/package_sources/iqdbc/iqdbc/dbc/generator/honda/honda_radar_scan.py new file mode 100644 index 000000000..5a9dbc906 --- /dev/null +++ b/artifacts/package_sources/iqdbc/iqdbc/dbc/generator/honda/honda_radar_scan.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" +import os + +SCAN_SLOTS = 16 + +FRAME_SIGNALS = { + "POS": [ + ("SCAN_STATE", 15, 4), + ("CYCLE", 27, 4), + ("DIST_RAW", 23, 12), + ("BEARING_RAW", 39, 11), + ("DIST_SIGMA_RAW", 7, 7), + ], + "SHAPE": [ + ("CYCLE", 28, 4), + ("PRESENCE_RAW", 46, 7), + ], + "LIFE": [ + ("CYCLE", 11, 4), + ("AGE_RAW", 7, 12), + ], + "IDENT": [ + ("CYCLE", 12, 4), + ("OBJECT_HANDLE", 55, 8), + ], + "MOTION": [ + ("CYCLE", 12, 4), + ("CLOSING_SPEED_RAW", 7, 11), + ("CLOSING_SPEED_SIGMA_RAW", 23, 10), + ("DIST_RATIO_RAW", 55, 10), + ], +} + +QUARTET_KINDS = ("POS", "SHAPE", "LIFE", "IDENT") + + +def quartet_base_address(slot: int) -> int: + return 0x280 + 4 * slot if slot < 4 else 0x2D0 + 4 * (slot - 4) + + +def motion_address(slot: int) -> int: + return 0x2C8 + slot if slot < 8 else 0x290 + (slot - 8) + + +def frame_address(slot: int, kind: str) -> int: + if kind == "MOTION": + return motion_address(slot) + return quartet_base_address(slot) + QUARTET_KINDS.index(kind) + + +HEADER = """VERSION "honda_radar_scan -- Honda Bosch radar object scan: 16 object slots, each a POS/SHAPE/LIFE/IDENT \ +frame quartet plus a synchronized MOTION companion. Raw counts only; physical conversion lives in \ +iqdbc/car/honda/radar_scan.py" + +NS_ : +\tNS_DESC_ +\tCM_ +\tBA_DEF_ +\tBA_ +\tVAL_ +\tCAT_DEF_ +\tCAT_ +\tFILTER +\tBA_DEF_DEF_ +\tEV_DATA_ +\tENVVAR_DATA_ +\tSGTYPE_ +\tSGTYPE_VAL_ +\tBA_DEF_SGTYPE_ +\tBA_SGTYPE_ +\tSIG_TYPE_REF_ +\tVAL_TABLE_ +\tSIG_GROUP_ +\tSIG_VALTYPE_ +\tSIGTYPE_VALTYPE_ +\tBO_TX_BU_ +\tBA_DEF_REL_ +\tBA_REL_ +\tBA_DEF_DEF_REL_ +\tBU_SG_REL_ +\tBU_EV_REL_ +\tBU_BO_REL_ +\tSG_MUL_VAL_ + +BS_: + +BU_: SCANNER XXX + +""" + + +def render() -> str: + parts = [HEADER] + for slot in range(SCAN_SLOTS): + for kind in (*QUARTET_KINDS, "MOTION"): + addr = frame_address(slot, kind) + parts.append(f"\nBO_ {addr} RADAR_SCAN_{slot:02d}_{kind}: 8 SCANNER\n") + for name, start_bit, size in FRAME_SIGNALS[kind]: + parts.append(f" SG_ {name} : {start_bit}|{size}@0+ (1,0) [0|{(1 << size) - 1}] \"\" XXX\n") + return "".join(parts) + + +if __name__ == "__main__": + out_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), "honda_radar_scan.dbc") + with open(out_path, "w", encoding="utf-8") as f: + f.write(render()) diff --git a/iqpilot/common/params_keys.h b/iqpilot/common/params_keys.h index ffe56c284..1ce292e0a 100644 --- a/iqpilot/common/params_keys.h +++ b/iqpilot/common/params_keys.h @@ -175,6 +175,7 @@ inline static std::unordered_map keys = { {"IQCarParamsPersistent", {PERSISTENT, BYTES}}, {"IQCarParamsPersistentV2", {PERSISTENT, BYTES}}, {"IQLongLearnedFactors", {PERSISTENT, JSON}}, + {"IQHondaRadarScan", {PERSISTENT, BOOL, "1"}}, {"CarPlatformBundle", {PERSISTENT, JSON}}, {"Konn3ktVwOdometers", {PERSISTENT, JSON}}, {"Konn3ktVehicleOdometers", {PERSISTENT, JSON}}, diff --git a/iqpilot/iq_maps/tile_bundle_downloader.py b/iqpilot/iq_maps/tile_bundle_downloader.py index b9c021335..b78a44b1a 100644 --- a/iqpilot/iq_maps/tile_bundle_downloader.py +++ b/iqpilot/iq_maps/tile_bundle_downloader.py @@ -24,6 +24,10 @@ except Exception: # ProprietaryModuleMissing or import errors in stripped build # Tile bundles live as LFS objects in the PRIVATE repo IQ.Lvbs/iqmaps (R2 is gone). # Anonymous access 404s by design; devices authenticate with the embedded read-only PAT # carried by the closed-source updater bundle (same fetch account as the OS images). +# Hugging Face is primary: it is CDN-served, so device downloads no longer come off the +# gitea box's home uplink. The gitea copies stay as failover -- if HF ever suspends the +# repo the fleet silently falls back instead of losing maps entirely. +HF_TILE_BUNDLE_BASE_URL = "https://huggingface.co/datasets/T3vl/iqmaps/resolve/main" DEFAULT_TILE_BUNDLE_BASE_URL = "https://git.konn3kt.com/IQ.Lvbs/iqmaps/raw/branch/master" FALLBACK_TILE_BUNDLE_BASE_URL = "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqmaps/raw/branch/master" @@ -47,7 +51,9 @@ def candidate_base_urls(params: Params) -> list[str]: override = (override or "").strip() if override: return [override.rstrip("/")] - urls: list[str] = [] + # HF first (CDN, and it keeps device traffic off the gitea box's uplink); the bundle's + # own endpoints and the self-hosted defaults follow as failover. + urls: list[str] = [HF_TILE_BUNDLE_BASE_URL] if _private_base_urls is not None: try: urls.extend(url.rstrip("/") for url in _private_base_urls()) @@ -55,7 +61,12 @@ def candidate_base_urls(params: Params) -> list[str]: pass urls.append(DEFAULT_TILE_BUNDLE_BASE_URL) urls.append(FALLBACK_TILE_BUNDLE_BASE_URL) - return urls + seen: set[str] = set() + return [u for u in urls if not (u in seen or seen.add(u))] + + +def _is_hf(url: str) -> bool: + return "huggingface.co" in url.lower() def _maps_auth_module(): @@ -83,6 +94,16 @@ def _maps_auth_module(): def request_headers(url: str) -> dict: mod = _maps_auth_module() if mod is not None: + if _is_hf(url): + # HF wants a bearer token, not basic auth; a build whose bundle predates HF + # hosting simply gets nothing here and falls through to the gitea mirrors. + try: + token = mod.map_tiles_hf_token() + if token: + return {"Authorization": f"Bearer {token}"} + except Exception: + pass + return {} try: headers = mod.map_tiles_headers(url) if headers: @@ -302,10 +323,12 @@ class TileBundleDownloader: # They stream back-to-back into ONE .part file: concatenating afterwards would need # double the free space, which devices do not have. base = f"{base_url}/{remote_path.lstrip('/')}" - if objects: - urls = [None] * len(objects) # resolved per-attempt from the oid + count = len(objects) if objects else parts + if objects and not _is_hf(base_url): + urls = [None] * count # gitea: resolved per-attempt from the oid else: - urls = [base] if parts <= 1 else [f"{base}.p{i:02d}" for i in range(parts)] + # HF (and plain mirrors) serve the same chunks as ordinary .pNN files + urls = [base] if count <= 1 else [f"{base}.p{i:02d}" for i in range(count)] part_path = final_path.with_name(final_path.name + ".part") part_path.parent.mkdir(parents=True, exist_ok=True) @@ -335,7 +358,7 @@ class TileBundleDownloader: mode = "ab" if resume_from else "wb" with open(part_path, mode) as f: for index in range(first_part, len(urls)): - if objects: + if objects and not _is_hf(base_url): url_headers = request_headers(base_url) auth = None if url_headers else request_auth() object_url, object_headers = _resolve_oid_url( diff --git a/iqpilot/selfdrive/car/interfaces.py b/iqpilot/selfdrive/car/interfaces.py index b61ee6820..d4afb38cb 100644 --- a/iqpilot/selfdrive/car/interfaces.py +++ b/iqpilot/selfdrive/car/interfaces.py @@ -2,7 +2,7 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos """ from iqdbc.car import structs as _dbc -from iqpilot.common.params import Params as _Store +from iqpilot.common.params import Params as _Store, UnknownKeyName as _UnknownKey from iqpilot.common.swaglog import cloudlog as _log from iqpilot.selfdrive.controls.lib.latcontrol_torque import get_nn_model_path as _resolve_nn @@ -70,8 +70,23 @@ def _cleanup_unsupported_params(cp, cp_iq, store=None) -> None: set_speed_limit_controller_availability(cp, cp_iq, store) +def _apply_radar_scan_switch(cp, store) -> None: + if cp.brand != "honda" or cp.radarUnavailable: + return + try: + switch = store.get("IQHondaRadarScan") + except _UnknownKey: + # params store predates the key (mid-update): the switch defaults on + return + # opt-out kill switch: only an explicit "0" disables, so a never-materialized param stays on + if switch in (b"0", "0"): + _log.warning("IQHondaRadarScan disabled, running without the radar object scan") + cp.radarUnavailable = True + + def apply_iq_car_config(ci, store=None) -> None: store = store or _Store() if _stamp_lateral_model(ci.CP, ci.CP_IQ, store): ci.configure_torque_tune(ci.CP.carFingerprint, ci.CP.lateralTuning) _cleanup_unsupported_params(ci.CP, ci.CP_IQ, store) + _apply_radar_scan_switch(ci.CP, store) diff --git a/iqpilot/selfdrive/controls/radard.py b/iqpilot/selfdrive/controls/radard.py index e81680b4d..f698ffcb4 100755 --- a/iqpilot/selfdrive/controls/radard.py +++ b/iqpilot/selfdrive/controls/radard.py @@ -13,6 +13,7 @@ from iqpilot.common.swaglog import cloudlog from iqpilot.common.simple_kalman import KF1D from iqdbc.car import structs +from iqdbc.car.honda.values import HONDA_RADAR_SCAN_CAPABLE from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiFlagsIQ from iqpilot.selfdrive.controls.lib.custom_stop_distance import CustomStopDistance @@ -29,6 +30,19 @@ V_EGO_STATIONARY = 4. # no stationary object flag below this speed RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame +# Honda radar object scan: 15Hz sweeps consumed at the 20Hz model rate, so measurement absorption is +# gated on fresh sweep data and lead selection carries continuity/staleness evidence +SCAN_SWEEP_DT = 1.0 / 15 +SCAN_LEAD_PROB = 0.35 +SCAN_LEAD_MIN_CYCLES = 3 +SCAN_CHALLENGER_STALE_CYCLES = 2 +SCAN_DISTANCE_STALE_CYCLES = 3 +SCAN_DISTANCE_STALE_M = 25.0 + + +def uses_scan_radar(CP) -> bool: + return CP.brand == "honda" and CP.carFingerprint in HONDA_RADAR_SCAN_CAPABLE and not CP.radarUnavailable + class KalmanParams: def __init__(self, dt: float): @@ -62,7 +76,8 @@ class Track: self.K_K = kalman_params.K self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K) - def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float): + def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float, + absorb_measurement: bool = True): # relative values, copy self.dRel = d_rel # LONG_DIST self.yRel = y_rel # -LAT_DIST @@ -70,18 +85,19 @@ class Track: self.vLead = v_lead self.measured = measured # measured or estimate - # computed velocity and accelerations - if self.cnt > 0: + # a repeated scan payload between 15Hz sweeps must not be absorbed as a second measurement + if absorb_measurement and self.cnt > 0: self.kf.update(self.vLead) self.vLeadK = float(self.kf.x[SPEED][0]) self.aLeadK = float(self.kf.x[ACCEL][0]) - # Learn if constant acceleration - if abs(self.aLeadK) < 0.5: - self.aLeadTau.x = _LEAD_ACCEL_TAU - else: - self.aLeadTau.update(0.0) + if absorb_measurement: + # Learn if constant acceleration + if abs(self.aLeadK) < 0.5: + self.aLeadTau.x = _LEAD_ACCEL_TAU + else: + self.aLeadTau.update(0.0) self.cnt += 1 @@ -119,18 +135,35 @@ def laplacian_pdf(x: float, mu: float, b: float): return math.exp(-abs(x-mu)/b) +def model_association_score(track: Track, lead: capnp._DynamicStructReader, v_ego: float) -> float: + offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA + prob_d = laplacian_pdf(track.dRel, offset_vision_dist, lead.xStd[0]) + prob_y = laplacian_pdf(track.yRel, -lead.y[0], lead.yStd[0]) + prob_v = laplacian_pdf(track.vRel + v_ego, lead.v[0], lead.vStd[0]) + + # This isn't exactly right, but it's a good heuristic + return prob_d * prob_y * prob_v + + +def track_agrees_with_model(track: Track, lead: capnp._DynamicStructReader, v_ego: float, strict: bool) -> bool: + dist_scale, dist_floor, vel_limit, y_std_scale, y_floor = \ + (0.25, 5.0, 10.0, 1.0, 1.0) if strict else (0.40, 8.0, 13.0, 2.0, 1.5) + vision_dist = lead.x[0] - RADAR_TO_CAMERA + dist_ok = abs(track.dRel - vision_dist) < max(abs(vision_dist) * dist_scale, dist_floor) + vel_ok = (abs(track.vRel + v_ego - lead.v[0]) < vel_limit) or (v_ego + track.vRel > 3) + lat_ok = abs(track.yRel + lead.y[0]) < max(y_floor, y_std_scale * max(float(lead.yStd[0]), 0.2)) + return dist_ok and vel_ok and lat_ok + + +def scan_low_speed_candidate(track: Track, v_ego: float) -> bool: + # require a few real cycles before a radar-only low-speed takeover + return track.cnt >= SCAN_LEAD_MIN_CYCLES and track.potential_low_speed_lead(v_ego) + + def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]): offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA - def prob(c): - prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0]) - prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0]) - prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) - - # This isn't exactly right, but it's a good heuristic - return prob_d * prob_y * prob_v - - track = max(tracks.values(), key=prob) + track = max(tracks.values(), key=lambda c: model_association_score(c, lead, v_ego)) # if no 'sane' match is found return -1 # stationary radar points can be false positives @@ -161,22 +194,60 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, - model_v_ego: float, CP: structs.CarParams, CP_IQ: structs.IQCarParams, low_speed_override: bool = True) -> dict[str, Any]: + model_v_ego: float, CP: structs.CarParams, CP_IQ: structs.IQCarParams, low_speed_override: bool = True, + scan_radar: bool = False, filtered_prob: float | None = None, held_track_id: int = -1) -> dict[str, Any]: + lead_prob = float(lead_msg.prob if filtered_prob is None else filtered_prob) + prob_threshold = SCAN_LEAD_PROB if scan_radar else .5 + # Determine leads, this is where the essential logic happens - if len(tracks) > 0 and ready and lead_msg.prob > .5: + if len(tracks) > 0 and ready and lead_prob > prob_threshold: track = match_vision_to_track(v_ego, lead_msg, tracks) else: track = None lead_dict = {'status': False} if track is not None: - lead_dict = track.get_RadarState(lead_msg.prob) + lead_dict = track.get_RadarState(lead_prob) lead_dict = get_custom_yrel(CP, CP_IQ, lead_dict, lead_msg) - elif (track is None) and ready and (lead_msg.prob > .5): + elif (track is None) and ready and (lead_prob > prob_threshold): lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) if low_speed_override: - low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)] + if scan_radar: + low_speed_tracks = [c for c in tracks.values() if scan_low_speed_candidate(c, v_ego)] + else: + low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)] + + model_lead_available = ready and lead_prob > prob_threshold + + if scan_radar: + # Keep the held radar lead through ordinary model-probability fluctuations while it stays + # coherent. With a valid model lead it must still agree with it; without one, a mature radar + # track remains eligible for continuity + held = tracks.get(held_track_id) + if held is not None and scan_low_speed_candidate(held, v_ego): + held_matches_model = (not model_lead_available or + track_agrees_with_model(held, lead_msg, v_ego, strict=True)) + held_is_current = (not lead_dict.get('status', False) or + lead_dict.get('radarTrackId', -1) == held_track_id or + (lead_dict.get('status', False) and not lead_dict.get('radar', False))) + if held_is_current and held_matches_model: + lead_dict = held.get_RadarState(lead_prob) + + def candidate_established(candidate: Track) -> bool: + if candidate.cnt < SCAN_LEAD_MIN_CYCLES: + return False + if not lead_dict.get('status', False): + # a mature centered scan point may provide the radar-only low-speed lead + return True + if lead_dict.get('radarTrackId', -1) == candidate.identifier: + return True + # never replace an established lead with an unrelated closer point without model evidence + # to arbitrate them + return model_lead_available and track_agrees_with_model(candidate, lead_msg, v_ego, strict=True) + + low_speed_tracks = [c for c in low_speed_tracks if candidate_established(c)] + if len(low_speed_tracks) > 0: closest_track = min(low_speed_tracks, key=lambda c: c.dRel) @@ -204,7 +275,16 @@ class RadarD: self.current_time = 0.0 self.tracks: dict[int, Track] = {} - self.kalman_params = KalmanParams(DT_MDL) + self.scan_radar = uses_scan_radar(CP) + # the lead KF absorbs scan measurements at the physical 15Hz sweep cadence; lead probability + # filtering stays on model-loop timing + self.kalman_params = KalmanParams(SCAN_SWEEP_DT if self.scan_radar else DT_MDL) + self.lead_prob_filters = [FirstOrderFilter(0.0, 0.2, DT_MDL) for _ in range(2)] + self.held_lead_ids = [-1, -1] + self._held_evidence_ids = [-1, -1] + self._challenger_stale_counts = [0, 0] + self._distance_stale_counts = [0, 0] + self._last_tracks_frame = -1 self.v_ego = 0.0 self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL))+1) @@ -217,6 +297,53 @@ class RadarD: self.custom_stop_distance = CustomStopDistance() + def _refresh_held_lead_evidence(self, lead_index: int, lead: capnp._DynamicStructReader, + lead_prob: float) -> None: + held_id = self.held_lead_ids[lead_index] + if self._held_evidence_ids[lead_index] != held_id: + self._reset_held_evidence(lead_index, held_id) + + held = self.tracks.get(held_id) + if held_id < 0 or held is None or not self.ready or lead_prob <= SCAN_LEAD_PROB: + self._reset_held_evidence(lead_index, held_id) + return + + strict_match = track_agrees_with_model(held, lead, self.v_ego, strict=True) + relaxed_match = track_agrees_with_model(held, lead, self.v_ego, strict=False) + + # evidence arm 1: another live track scores better against the model while the held one no + # longer passes even relaxed continuity. Releasing the hold never selects that challenger; the + # strict-match path in get_lead stays the only way it becomes the radar lead + if relaxed_match: + self._challenger_stale_counts[lead_index] = 0 + else: + best = max(self.tracks.values(), key=lambda c: model_association_score(c, lead, self.v_ego)) + if best.identifier != held_id and \ + model_association_score(best, lead, self.v_ego) > model_association_score(held, lead, self.v_ego): + self._challenger_stale_counts[lead_index] += 1 + else: + self._challenger_stale_counts[lead_index] = 0 + + # evidence arm 2: gross absolute range disagreement, with a strict match staying authoritative + # even when model uncertainty would permit the error + distance_mismatch = abs(held.dRel - (lead.x[0] - RADAR_TO_CAMERA)) + if strict_match: + self._distance_stale_counts[lead_index] = 0 + elif distance_mismatch > SCAN_DISTANCE_STALE_M: + self._distance_stale_counts[lead_index] += 1 + else: + self._distance_stale_counts[lead_index] = 0 + + if (self._challenger_stale_counts[lead_index] >= SCAN_CHALLENGER_STALE_CYCLES or + self._distance_stale_counts[lead_index] >= SCAN_DISTANCE_STALE_CYCLES): + self.held_lead_ids[lead_index] = -1 + self._reset_held_evidence(lead_index) + + def _reset_held_evidence(self, lead_index: int, held_id: int = -1) -> None: + self._held_evidence_ids[lead_index] = held_id + self._challenger_stale_counts[lead_index] = 0 + self._distance_stale_counts[lead_index] = 0 + def update(self, sm: messaging.SubMaster, rr: car.RadarData): self.ready = sm.seen['modelV2'] self.current_time = 1e-9*max(sm.logMonoTime.values()) @@ -227,6 +354,11 @@ class RadarD: self.v_ego_hist.append(self.v_ego) self.last_v_ego_frame = sm.recv_frame['carState'] + sweep_fresh = True + if self.scan_radar: + sweep_fresh = sm.recv_frame['radarTracks'] != self._last_tracks_frame + self._last_tracks_frame = sm.recv_frame['radarTracks'] + ar_pts = {pt.trackId: [pt.dRel, pt.yRel, pt.vRel, pt.measured] for pt in rr.points} # *** remove missing points from meta data *** @@ -244,7 +376,11 @@ class RadarD: # create the track if it doesn't exist or it's a new track if ids not in self.tracks: self.tracks[ids] = Track(ids, v_lead, self.kalman_params) - self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) + if self.scan_radar: + measured = bool(rpt[3] and sweep_fresh) + self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, measured, absorb_measurement=measured) + else: + self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) # *** publish radarState *** self.radar_state_valid = sm.all_checks() @@ -259,8 +395,34 @@ class RadarD: model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - lead_one = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.CP, self.CP_IQ, low_speed_override=True) - lead_two = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.CP, self.CP_IQ, low_speed_override=False) + if self.scan_radar: + for i in range(2): + lead_prob = float(leads_v3[i].prob) + # probability rises instantly, decays filtered: a one-cycle model dip must not drop the lead + if lead_prob > self.lead_prob_filters[i].x: + self.lead_prob_filters[i].x = lead_prob + else: + self.lead_prob_filters[i].update(lead_prob) + self._refresh_held_lead_evidence(i, leads_v3[i], self.lead_prob_filters[i].x) + + lead_one = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.CP, self.CP_IQ, + low_speed_override=True, scan_radar=True, filtered_prob=self.lead_prob_filters[0].x, + held_track_id=self.held_lead_ids[0]) + lead_two = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.CP, self.CP_IQ, + low_speed_override=False, scan_radar=True, filtered_prob=self.lead_prob_filters[1].x, + held_track_id=self.held_lead_ids[1]) + + for i, lead in enumerate((lead_one, lead_two)): + if lead.get('status', False) and lead.get('radar', False): + track_id = int(lead.get('radarTrackId', -1)) + if track_id != self.held_lead_ids[i]: + self._reset_held_evidence(i, track_id) + self.held_lead_ids[i] = track_id + elif (not lead.get('status', False)) or (self.held_lead_ids[i] not in self.tracks): + self.held_lead_ids[i] = -1 + else: + lead_one = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.CP, self.CP_IQ, low_speed_override=True) + lead_two = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.CP, self.CP_IQ, low_speed_override=False) self.radar_state.leadOne = self.custom_stop_distance.apply_lead(lead_one) self.radar_state.leadTwo = self.custom_stop_distance.apply_lead(lead_two) diff --git a/iqpilot/selfdrive/controls/tests/test_scan_radar_leads.py b/iqpilot/selfdrive/controls/tests/test_scan_radar_leads.py new file mode 100644 index 000000000..dac15b219 --- /dev/null +++ b/iqpilot/selfdrive/controls/tests/test_scan_radar_leads.py @@ -0,0 +1,191 @@ +from types import SimpleNamespace + +import pytest + +from iqdbc.car.honda.values import CAR +from iqpilot.selfdrive.controls.radard import (SCAN_CHALLENGER_STALE_CYCLES, SCAN_DISTANCE_STALE_CYCLES, + SCAN_LEAD_MIN_CYCLES, SCAN_LEAD_PROB, SCAN_SWEEP_DT, + KalmanParams, RadarD, Track, get_lead, + scan_low_speed_candidate, track_agrees_with_model, + uses_scan_radar) + +DT_MDL = 0.05 + + +def honda_cp(fingerprint=CAR.HONDA_CIVIC_BOSCH, radar_unavailable=False, brand="honda"): + return SimpleNamespace(brand=brand, carFingerprint=fingerprint, radarUnavailable=radar_unavailable, flags=0) + + +def model_lead(x=30.0, y=0.0, v=10.0, prob=0.9, x_std=2.0, y_std=0.5, v_std=1.0): + return SimpleNamespace(x=[x], y=[y], v=[v], a=[0.0], prob=prob, xStd=[x_std], yStd=[y_std], vStd=[v_std]) + + +def make_track(identifier, d_rel, y_rel=0.0, v_rel=0.0, v_ego=10.0, cycles=SCAN_LEAD_MIN_CYCLES, measured=True): + track = Track(identifier, v_rel + v_ego, KalmanParams(SCAN_SWEEP_DT)) + for _ in range(cycles): + track.update(d_rel, y_rel, v_rel, v_rel + v_ego, measured) + return track + + +class TestScanRadarGating: + def test_capable_verified_car_uses_scan(self): + assert uses_scan_radar(honda_cp()) + + def test_radar_unavailable_disables(self): + assert not uses_scan_radar(honda_cp(radar_unavailable=True)) + + def test_non_family_car_never_scans(self): + assert not uses_scan_radar(honda_cp(fingerprint=CAR.HONDA_CIVIC)) + assert not uses_scan_radar(honda_cp(fingerprint=CAR.HONDA_CIVIC_2022)) + + def test_other_brand_never_scans(self): + assert not uses_scan_radar(honda_cp(brand="toyota")) + + +class TestTrackMeasurementGating: + def test_repeated_payload_not_absorbed_twice(self): + kp = KalmanParams(SCAN_SWEEP_DT) + absorbed = Track(1, 10.0, kp) + starved = Track(1, 10.0, kp) + absorbed.update(30.0, 0.0, 5.0, 15.0, True) + starved.update(30.0, 0.0, 5.0, 15.0, True) + v_after_first = starved.vLeadK + + for _ in range(5): + absorbed.update(30.0, 0.0, 5.0, 15.0, True, absorb_measurement=True) + starved.update(30.0, 0.0, 5.0, 15.0, False, absorb_measurement=False) + + assert starved.vLeadK == v_after_first + assert absorbed.vLeadK != v_after_first + assert starved.cnt == absorbed.cnt + + +class TestLowSpeedCandidate: + def test_needs_minimum_cycles(self): + young = make_track(1, 10.0, v_ego=2.0, cycles=SCAN_LEAD_MIN_CYCLES - 1) + mature = make_track(2, 10.0, v_ego=2.0) + assert not scan_low_speed_candidate(young, 2.0) + assert scan_low_speed_candidate(mature, 2.0) + + def test_geometry_still_applies(self): + offset = make_track(3, 10.0, y_rel=2.0, v_ego=2.0) + assert not scan_low_speed_candidate(offset, 2.0) + + +class TestModelAgreement: + def test_strict_tighter_than_relaxed(self): + track = make_track(1, 40.0, v_ego=10.0) + # ~15m disagreement: beyond the strict 25% envelope, inside the relaxed 40% one + lead = model_lead(x=56.5) + assert not track_agrees_with_model(track, lead, 10.0, strict=True) + assert track_agrees_with_model(track, lead, 10.0, strict=False) + + +class TestScanLeadSelection: + def make_cp(self): + return honda_cp(), SimpleNamespace() + + def test_held_lead_survives_probability_dip(self): + CP, CP_IQ = self.make_cp() + track = make_track(7, 10.0, v_ego=2.0) + lead = model_lead(x=11.5, v=2.0, prob=0.05) + # raw prob is below threshold, the filtered prob is passed in above it + result = get_lead(2.0, True, {7: track}, lead, 2.0, CP, CP_IQ, low_speed_override=True, + scan_radar=True, filtered_prob=0.6, held_track_id=7) + assert result['status'] and result['radarTrackId'] == 7 + + def test_unrelated_closer_point_cannot_usurp_without_model_evidence(self): + CP, CP_IQ = self.make_cp() + held = make_track(7, 10.0, v_ego=2.0) + interloper = make_track(9, 4.0, y_rel=0.5, v_ego=2.0) + lead = model_lead(x=11.5, v=2.0, prob=0.9) + result = get_lead(2.0, True, {7: held, 9: interloper}, lead, 2.0, CP, CP_IQ, low_speed_override=True, + scan_radar=True, filtered_prob=0.9, held_track_id=7) + assert result['radarTrackId'] == 7 + + def test_matching_closer_point_takes_over(self): + CP, CP_IQ = self.make_cp() + held = make_track(7, 10.0, v_ego=2.0) + closer = make_track(9, 4.0, v_ego=2.0) + lead = model_lead(x=5.5, v=2.0, prob=0.9) # model agrees with the closer car + result = get_lead(2.0, True, {7: held, 9: closer}, lead, 2.0, CP, CP_IQ, low_speed_override=True, + scan_radar=True, filtered_prob=0.9, held_track_id=7) + assert result['radarTrackId'] == 9 + + def test_radar_only_takeover_when_no_model_lead(self): + CP, CP_IQ = self.make_cp() + track = make_track(4, 8.0, v_ego=2.0) + lead = model_lead(prob=0.0) + result = get_lead(2.0, True, {4: track}, lead, 2.0, CP, CP_IQ, low_speed_override=True, + scan_radar=True, filtered_prob=0.0, held_track_id=-1) + assert result['status'] and result['radarTrackId'] == 4 + + def test_young_track_cannot_lead_alone(self): + CP, CP_IQ = self.make_cp() + track = make_track(4, 8.0, v_ego=2.0, cycles=1) + lead = model_lead(prob=0.0) + result = get_lead(2.0, True, {4: track}, lead, 2.0, CP, CP_IQ, low_speed_override=True, + scan_radar=True, filtered_prob=0.0, held_track_id=-1) + assert not result['status'] + + def test_non_scan_behavior_unchanged(self): + CP, CP_IQ = self.make_cp() + track = make_track(4, 8.0, v_ego=2.0, cycles=1) + lead = model_lead(prob=0.0) + result = get_lead(2.0, True, {4: track}, lead, 2.0, CP, CP_IQ, low_speed_override=True) + assert result['status'] # legacy path has no maturity gate + + +class TestHeldLeadStaleness: + def make_radard(self, tracks): + rd = object.__new__(RadarD) + rd.tracks = tracks + rd.ready = True + rd.v_ego = 10.0 + rd.held_lead_ids = [7, -1] + rd._held_evidence_ids = [7, -1] + rd._challenger_stale_counts = [0, 0] + rd._distance_stale_counts = [0, 0] + return rd + + def test_relaxed_match_keeps_hold(self): + held = make_track(7, 30.0, v_ego=10.0) + rd = self.make_radard({7: held}) + for _ in range(5): + rd._refresh_held_lead_evidence(0, model_lead(x=32.0), 0.9) + assert rd.held_lead_ids[0] == 7 + + def test_better_challenger_releases_hold(self): + held = make_track(7, 80.0, v_ego=10.0) + challenger = make_track(9, 30.0, v_ego=10.0) + rd = self.make_radard({7: held, 9: challenger}) + lead = model_lead(x=31.5) + for _ in range(SCAN_CHALLENGER_STALE_CYCLES): + rd._refresh_held_lead_evidence(0, lead, 0.9) + assert rd.held_lead_ids[0] == -1 + + def test_gross_distance_disagreement_releases_hold(self): + held = make_track(7, 80.0, v_ego=10.0) + rd = self.make_radard({7: held}) + lead = model_lead(x=31.5, x_std=60.0, y_std=30.0, v_std=30.0) # huge model uncertainty + for _ in range(SCAN_DISTANCE_STALE_CYCLES): + rd._refresh_held_lead_evidence(0, lead, 0.9) + assert rd.held_lead_ids[0] == -1 + + def test_low_probability_resets_evidence_without_release(self): + held = make_track(7, 80.0, v_ego=10.0) + rd = self.make_radard({7: held}) + for _ in range(10): + rd._refresh_held_lead_evidence(0, model_lead(x=31.5), SCAN_LEAD_PROB) + assert rd.held_lead_ids[0] == 7 + assert rd._distance_stale_counts[0] == 0 + + +class TestProbFilterTiming: + def test_rises_instantly_decays_slowly(self): + from iqpilot.common.filter_simple import FirstOrderFilter + f = FirstOrderFilter(0.0, 0.2, DT_MDL) + f.x = max(f.x, 0.9) + assert f.x == pytest.approx(0.9) + f.update(0.0) + assert f.x > 0.6 diff --git a/iqpilot/selfdrive/iqmodeld/egpu_prefetch.py b/iqpilot/selfdrive/iqmodeld/egpu_prefetch.py index e10432371..00084a774 100644 --- a/iqpilot/selfdrive/iqmodeld/egpu_prefetch.py +++ b/iqpilot/selfdrive/iqmodeld/egpu_prefetch.py @@ -10,7 +10,8 @@ import time from iqpilot.common.params import Params from iqpilot.common.swaglog import cloudlog -from iqpilot.selfdrive.iqmodeld.egpu_helpers import download_precompiled, egpu_policy_pkl_path, egpu_selected, patch_tinygrad_fetch_fw, usbgpu_present +from iqpilot.selfdrive.iqmodeld.egpu_helpers import (download_precompiled, egpu_oob_pkl_path, egpu_policy_pkl_path, patch_tinygrad_fetch_fw, + usbgpu_present) from iqpilot.selfdrive.iqmodeld.egpu_model import resolve_egpu_model POLL_S = 30.0 @@ -28,7 +29,8 @@ def _selected_meta(params: Params) -> dict | None: def _drop_stale_partials(keep: str) -> None: root = os.path.dirname(keep) - for path in glob.glob(os.path.join(root, "egpu_*_amd_policy.pkl.part")) + glob.glob(os.path.join(root, "big_driving_supercombo_*.onnx.part")): + partial_globs = ("egpu_*_amd_policy.pkl.part", "egpu_*_amd_policy_oob.pkl.part", "big_driving_supercombo_*.onnx.part") + for path in [p for g in partial_globs for p in glob.glob(os.path.join(root, g))]: if not path.startswith(keep): try: os.remove(path) @@ -37,15 +39,16 @@ def _drop_stale_partials(keep: str) -> None: def prefetch_once(params: Params) -> bool: - if not usbgpu_present() or not egpu_selected(params): + if params.get_bool("IQEgpuDisabled"): return False meta = _selected_meta(params) if meta is None: return False - dst = egpu_policy_pkl_path(meta) + oob = bool(meta.get("egpu_oob_artifact")) + dst = egpu_oob_pkl_path(meta) if oob else egpu_policy_pkl_path(meta) if os.path.isfile(dst): return True - if not meta.get("egpu_policy_artifact"): + if not oob and not meta.get("egpu_policy_artifact"): return False _drop_stale_partials(dst) params.put("UsbGpuSetupProgress", "0.0") @@ -56,8 +59,8 @@ def prefetch_once(params: Params) -> bool: last[0] = p params.put("UsbGpuSetupProgress", f"{p:.3f}") - cloudlog.warning(f"egpu_prefetch downloading {meta['key']} policy artifact offroad") - out = download_precompiled(meta, progress_cb=_prog, policy=True) + cloudlog.warning(f"egpu_prefetch downloading {meta['key']} {'streamable' if oob else 'policy'} artifact offroad") + out = download_precompiled(meta, progress_cb=_prog, policy=not oob, oob=oob) cloudlog.warning(f"egpu_prefetch ready -> {out}") return out is not None diff --git a/iqpilot/selfdrive/ui/mici/layouts/settings/models.py b/iqpilot/selfdrive/ui/mici/layouts/settings/models.py index 9586367df..d0ce457c0 100644 --- a/iqpilot/selfdrive/ui/mici/layouts/settings/models.py +++ b/iqpilot/selfdrive/ui/mici/layouts/settings/models.py @@ -137,7 +137,7 @@ class ModelsLayoutMici(NavScroller): self._redownload.set_enabled(self._can_redownload) self._refresh = BigButton(tr("reload model list")) - self._refresh.set_click_callback(lambda: ui_state.params.put("ModelManager_LastSyncTime", 0)) + self._refresh.set_click_callback(self._reload_model_lists) self._supercombo = GreyBigButton(tr("combined model")) self._supercombo.set_visible(False) @@ -355,10 +355,13 @@ class ModelsLayoutMici(NavScroller): btns = [_ModelButton(b, self._select_model, self._toggle_favorite, b.ref in favorites) for b in bundles] gui_app.push_widget(_ModelSelectPanel(btns)) + def _reload_model_lists(self): + ui_state.params.put("ModelManager_LastSyncTime", 0) + _refresh_big_catalog() + def _show_big_models(self): + _refresh_big_catalog() options = _big_options() - if len(options) <= 1: - _refresh_big_catalog() off = BigButton(tr("Off")) off.set_click_callback(lambda: self._select_big(None)) btns = [off] diff --git a/iqpilot/system/manager/process_config.py b/iqpilot/system/manager/process_config.py index 77b650722..2b1de9aee 100644 --- a/iqpilot/system/manager/process_config.py +++ b/iqpilot/system/manager/process_config.py @@ -138,6 +138,11 @@ def egpu_enabled(started, params, CP: car.CarParams) -> bool: return (resolve_backend(params.get_bool("IQEmacEnabled"), egpu_selected(params), _egpu_present(params)) == "egpu" and _egpu_present(params)) +def egpu_prefetch_enabled(started, params, CP: car.CarParams) -> bool: + if params.get_bool("IQEgpuDisabled"): + return False + return resolve_backend(params.get_bool("IQEmacEnabled"), True, _egpu_present(params)) == "egpu" + def big_model_enabled(started, params, CP: car.CarParams) -> bool: return params.get_bool("IQEmacEnabled") or egpu_selected(params) @@ -223,7 +228,7 @@ procs += [ PythonProcess("iqegpumodeld", "iqpilot.selfdrive.iqmodeld.iqegpumodeld", and_(only_onroad, and_(is_tinygrad_model, egpu_enabled)), restart_if_crash=True), PythonProcess("egpu_prefetch", "iqpilot.selfdrive.iqmodeld.egpu_prefetch", - and_(only_offroad, and_(is_tinygrad_model, egpu_enabled)), restart_if_crash=True), + and_(only_offroad, and_(is_tinygrad_model, egpu_prefetch_enabled)), restart_if_crash=True), BundleProcess("backup_manager_k3", "iqpilot_hephaestusd_private", "iqpilot_private.konn3kt.backups.backup_orchestrator", and_(only_offroad, hephaestus_ready_shim, not_low_power)),