IQ.Pilot Release Commit @ 9ba2ea7
This commit is contained in:
@@ -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=="
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1 +1 @@
|
||||
JJCbPdLrTbJu+aVPIcsTJTbsltzP0Zl97r6EM77B7ebCsnS9yU1Tx9+NPui1OVVPiQAigyDxi/TZZyIYBWigDw==
|
||||
0OdQnuSdywA/tTTIVWGwz2gpZ5H50RoDjjWrXlgYDivBPpTU83nj3jbFtFenrzVNDouRC/PbTvEFNwcnHAe8DA==
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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
|
||||
|
||||
@@ -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):
|
||||
|
||||
396
artifacts/package_sources/iqdbc/iqdbc/car/honda/radar_scan.py
Normal file
396
artifacts/package_sources/iqdbc/iqdbc/car/honda/radar_scan.py
Normal file
@@ -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
|
||||
@@ -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
|
||||
@@ -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}
|
||||
@@ -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()
|
||||
|
||||
|
||||
@@ -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
|
||||
@@ -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())
|
||||
@@ -175,6 +175,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"IQCarParamsPersistent", {PERSISTENT, BYTES}},
|
||||
{"IQCarParamsPersistentV2", {PERSISTENT, BYTES}},
|
||||
{"IQLongLearnedFactors", {PERSISTENT, JSON}},
|
||||
{"IQHondaRadarScan", {PERSISTENT, BOOL, "1"}},
|
||||
{"CarPlatformBundle", {PERSISTENT, JSON}},
|
||||
{"Konn3ktVwOdometers", {PERSISTENT, JSON}},
|
||||
{"Konn3ktVehicleOdometers", {PERSISTENT, JSON}},
|
||||
|
||||
@@ -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(
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
191
iqpilot/selfdrive/controls/tests/test_scan_radar_leads.py
Normal file
191
iqpilot/selfdrive/controls/tests/test_scan_radar_leads.py
Normal file
@@ -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
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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]
|
||||
|
||||
@@ -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)),
|
||||
|
||||
Reference in New Issue
Block a user