diff --git a/opendbc/car/honda/bosch_a_radar.py b/opendbc/car/honda/bosch_a_radar.py new file mode 100644 index 00000000000..be02f7d018b --- /dev/null +++ b/opendbc/car/honda/bosch_a_radar.py @@ -0,0 +1,1458 @@ +"""Honda Bosch-A object bank decoder (Civic Bosch, CR-V 5G), ported from Trung's (trung791997) ns-bosch-radar-testing +@ 216050fd98 (2026-10-17) onto 7.0's BoschARadar. + +What changed from 7.0's decoder: firmware range scale 1/16 m with the per-car ROM offset (was 0.05712 and -3.0), U11 +closing speed at 1/72 m/s per count (was 1/64), the saturation rails treated as bounds by the gates (D-063), the +D-054/D-055/D-057/D-059/D-062 range gate, re-anchor and join logic, the one-sided coast bounds, and a saturated +lifecycle counter that no longer deletes a long-tracked lead. + +Left out on purpose, because they depend on Trung's radard, which 7.0 does not have: the newborn range publish (needs +radard's NEWBORN_* exemptions; BOSCH_A_NEWBORN_RANGE_PUBLISH is False), the D-089 far-range recovery (only safe behind +radard's RECOVERED_CAM_GATE; BOSCH_A_REANCHOR_RECOVER_SIGMA_FRAC is 0), and the NORMALIZED_CLOSING outputs (no RadarPoint +fields for them in 7.0, and the NC-at-rail vRel was rejected, D-069). Point flags use 7.0's RadarPoint.deprecated group. +Trung's D-numbers and STATUS items refer to his branch's DECISIONS.md / STATUS.md. +""" +import math +from collections import deque +from dataclasses import dataclass, field + +from opendbc.can import CANParser +from opendbc.car import Bus, structs +from opendbc.car.honda.hondacan import CanBus +from opendbc.car.honda.values import CAR, DBC, HondaFlags +from opendbc.car.interfaces import RadarInterfaceBase + +# ============================================================================ +# Honda Bosch-A 16-slot object bank +# ============================================================================ +# 16 CAN-visible object slots. Each slot has 4 main frames (f0..f3, one CAN ID each -- NOT sub-frames +# muxed onto a shared ID) plus one synchronized auxiliary 5th frame. This supersedes any prior model of +# 0x280/0x284/0x288/0x28C as pieces of one object, or of 0x2C8/0x2C9 as a separate "coarse" list: it is +# ONE 16-slot bank with one auxiliary frame per slot. See honda_bosch_a_radar.dbc for the bit geometry. +BOSCH_A_DBC_NAME = 'honda_bosch_a_radar' +BOSCH_A_NUM_SLOTS = 16 + + +def _bosch_a_main_base(slot: int) -> int: + return 0x280 + 4 * slot if slot < 4 else 0x2D0 + 4 * (slot - 4) + + +def _bosch_a_aux_id(slot: int) -> int: + return 0x2C8 + slot if slot < 8 else 0x290 + (slot - 8) + + +BOSCH_A_MAIN_IDS = [[_bosch_a_main_base(s) + i for i in range(4)] for s in range(BOSCH_A_NUM_SLOTS)] +BOSCH_A_AUX_IDS = [_bosch_a_aux_id(s) for s in range(BOSCH_A_NUM_SLOTS)] +BOSCH_A_ALL_IDS = [addr for ids in BOSCH_A_MAIN_IDS for addr in ids] + BOSCH_A_AUX_IDS + +# Publish RadarPoints when the last MAIN object frame arrives. Passive captures prove that slot 15's +# companion frame, 0x297, follows 0x2FF and is the final observed object-family frame in a full sweep: +# +# ... 0x2FC, 0x2FD, 0x2FE, 0x2FF, 0x297 +# +# Keep 0x2FF as the RadarPoint trigger while the companion data remains optional/debug-only. Making +# 0x297 the sole trigger would allow one dropped auxiliary frame to suppress an otherwise-valid point +# update, contrary to the parser's "aux never gates validity" contract. +BOSCH_A_TRIGGER_MSG = BOSCH_A_MAIN_IDS[BOSCH_A_NUM_SLOTS - 1][3] +BOSCH_A_SWEEP_END_MSG = BOSCH_A_AUX_IDS[BOSCH_A_NUM_SLOTS - 1] # 0x297 + +# Coherent Bosch-A sweep cadence, measured from 0x280 inter-arrival across Peter's routes: +# median 14.35 Hz (p5 12.5, p95 16.9). This also sets the lead-Kalman dt in radard, so the +# previous round 15 ran the filter ~4.5% fast. +BOSCH_A_FREQ_HZ = 14.35 + +# Range: f0 raw_range (12-bit, B2:B3 high nibble) -> meters. Firmware q16 = 8*raw_range. +# +# The scale is firmware-exact, not fitted. AC004 converts the internal value with +# (q16 - n) / 128, and q16 = sat16(round(8 * raw_range)), so +# +# range_m = (8 * raw_range - n) / 128 = raw_range / 16 - n / 128 +# +# Corroboration that 1/16 is the designed mapping rather than a coincidence: 8 * 4095 = 32760 +# fits int16 with 7 counts to spare, so the *8 exists to make the 12-bit field fill the internal +# word; full scale is 4095/16 = 255.9 m; and /128 (Q7) is this firmware's unit for physical +# quantities throughout. The previous 0.05712 was 16 * 0.00357, and that 0.00357 was solved from +# a single tape point with the offset assumed, so it read progressively short with distance +# (~9% low, -9 m at 60 m against vision). +BOSCH_A_RANGE_SCALE_M = 1.0 / 16.0 + +# Offset: the radar firmware's ROM default n0, as -n0/128 m, per car (D-076, 2026-10-07, Peter; +# Civic corrected 2026-10-16, Jason's static trace). +# +# Civic (36802TBA A160): cfg 5530 -> n0 = trunc(5530/16 + 0.5) = 346 -> -2.703125 m +# CR-V (36802TLA A070): cfg 5448 -> n0 = trunc(5448/16 + 0.5) = 341 -> -2.6640625 m +# +# Evidence and its limits (static only, never road-measured): +# - D-076 first took Civic cfg = 0, which selects the 335 literal fallback (-2.6171875 m). The ROM refutes that: +# at startup the radar copies a ROM record into RAM config S = 0x40039A64 (memcpy(S+3, rec+3, 241) at 0xB732E, +# fallback copy 0xB7386) and reads cfg from S+0xD6 (0xDB998, 0xDC03C). The TBA A160 table at ROM 0x15BC94 +# (stride 244) holds cfg 0x159A = 5530 at +0xD6 in all 26 records. The same method reproduces CR-V 5448 +# (positive control). 335 -> 346 moves every Civic dRel 0.0859 m shorter; vRel and range differences unchanged. +# - n0 is a ROM default. NvM can overwrite cfg at runtime (TBA writer 0xB60A8, store to S+0xD6 at 0xB61F2), so the +# runtime value is unproven; Peter's radar is A150, whose image we do not have (346 is the A160 value); whether +# the -0xC0 addend applies is open. +# - The firmware adds a runtime addend from camera CAN 0x669 X (Peter's car: n = 220). That path, and the +# BoschARangeOffsetFallback toggle that applied it, are REMOVED: 0x669 was a required parser message +# (canError risk if absent), and the firmware never applies n/128 to a transmitted range. Range is copied +# unmodified (0xdb7ee -> rec+0x10); (range - n)/128 is only a lever arm for lateral (0xdc004) and the +# width gate (0xaa5a4). +# - So this number is a firmware-sourced choice, not a proven range origin. It replaces -3.0 (James's fw +# math + tape check), moving dRel +0.30 m (Civic, 346). The real origin of the camera's range vs openpilot's +# bumper-frame dRel needs a laser/tape check to a parked car (5/10/20/40/60 m). +BOSCH_A_RANGE_OFFSET_CIVIC_M = -346.0 / 128.0 # -2.703125 m +BOSCH_A_RANGE_OFFSET_CRV_M = -341.0 / 128.0 # -2.6640625 m +BOSCH_A_RANGE_OFFSET_M = BOSCH_A_RANGE_OFFSET_CIVIC_M + + +def bosch_a_range_offset_m(car_fingerprint: str = "") -> float: + """Firmware ROM-default range offset for the car: CR-V -341/128, everything else (Civic) -346/128.""" + from opendbc.car.honda.values import CAR + if car_fingerprint in (CAR.HONDA_CRV_5G, CAR.HONDA_CRV_HYBRID): + return BOSCH_A_RANGE_OFFSET_CRV_M + return BOSCH_A_RANGE_OFFSET_CIVIC_M + + +# Azimuth: f0 raw_angle (11-bit, B4:B5 high 3 bits), offset-binary about 1024. +# +# The f3 angular-edge pair independently closes the exact center-angle scale: +# azimuth_rad = (raw_angle - 1024) / 2048 +# +# This supersedes the older empirical 0.032 deg/count fit. +BOSCH_A_AZIMUTH_SCALE_RAD = 1.0 / 2048.0 +BOSCH_A_AZIMUTH_CENTER = 1024 + +# Invalid sentinels (section 3/4/5/6 of the spec). +BOSCH_A_STATUS_INVALID = 0xF +BOSCH_A_RANGE_RAW_INVALID = 0xFFF +BOSCH_A_ANGLE_RAW_INVALID = 0x7FF +BOSCH_A_LIFE_INVALID = 0xFFF + +# LIFECYCLE_RAW is a 12-bit counter that advances by 2 per frame index, so it reaches its highest +# even value, 0xFFE, after 2047 frames -- about 137 s of continuous tracking at the ~14.9 Hz sweep +# rate -- and then stops advancing. It is NOT the invalid sentinel (0xFFF): STATUS, range, azimuth +# and track id all stay valid, and the object is still really there. +# +# Measured on 00000232--fc8dad0d18 (device commit 0083ffa, replayed through this parser): +# track 37 was born at route t=442.7 s, saturated at t=580.2 s at an age of 137.4 s, and was then +# observed on every sweep for 121.8 s (1,815 sweeps) while `life_delta == 2 * frame_delta` failed +# every time -- so the range history was cleared every sweep, no point ever matured, and the object +# was published exactly 0 times. Its last published geometry was dRel 38.9 m, yRel -0.1 m: the lead +# we were following. The device's own recording agrees -- liveTracks carries id 37 in 894/894 frames +# of segment 8 and 569/893 of segment 9, then 0 in segments 10-12, and the radar lead goes from +# 1200/1200 frames to 0/1200 across that boundary. Track 34 did the same at an age of 137.5 s. +# +# A saturated counter cannot testify either way about identity, and per D-041/D-042 the safe +# direction is to keep publishing geometry rather than delete a real object: the range-innovation +# gate below still decides whether each sweep is trustworthy, and staleness still retires a genuine +# disappearance. The cost, recorded against D-049: an ID reuse that happens WHILE the counter is +# saturated cannot be detected here at all. +BOSCH_A_LIFE_SATURATED = 0xFFE +BOSCH_A_TRACK_ID_MIN = 1 +BOSCH_A_TRACK_ID_MAX = 0x3F +# AUX logical 0x00CA has an explicit 0x3FF invalid sentinel. Firmware proves the normalization below; +# captures strongly support interpreting the active value as previous/current range ratio. +BOSCH_A_RANGE_RATIO_INVALID = 0x3FF +BOSCH_A_LOGICAL_00CA_INVALID = BOSCH_A_RANGE_RATIO_INVALID # compatibility/debug alias +BOSCH_A_RANGE_RATIO_SCALE = 0.001 +BOSCH_A_RANGE_RATIO_OFFSET = 0.5 + +# The synchronized AUX frame contains an 11-bit offset-binary velocity-like field and a 10-bit +# companion quality/uncertainty field. The byte locations are now decoded in the DBC. The exact +# Bosch descriptor name is still being verified, so keep the conversion constants isolated here. +# Capture validation shows active values in [0, 1728], with 0x7FE used as the inactive sentinel. +BOSCH_A_DIRECT_VREL_INVALID = 0x7FE +BOSCH_A_DIRECT_VREL_MIN_RAW = 0 +BOSCH_A_DIRECT_VREL_MAX_RAW = 1728 +BOSCH_A_DIRECT_VREL_CENTER_RAW = 864 +# D-074 (ACCEPTED by the owner 2026-10-02; static and replay evidence only, no road A/B): U11 is 1/72 m/s per +# count, built in; the old 1/64 decode is removed. Routes logged before 2026-10-01 published 1/64, so every m/s figure +# in this file and in radard measured on them (13.5 rails, -16.54, -19.4 ...) is in 1/64 units: multiply by 64/72 +# for 1/72. +# * static: Bosch-radar-partner camera firmware (36161-TLA-A070; same-family TGG-A080, TGH-A040, +# TFJ/TGG/TGL-G070) formats vRel as round((v + 12) / 0x3c638e45), and 0x3c638e45 is 1/72 as an f32. +# (v + 12) * 72 spans raw 0..1728, the observed rails, centre 864: the field is +-12.0 m/s by design. +# The owner's camera, 36161-TBA-A130, is not imaged. (firmware-analysis-kit: camera-re/bosch_a_inventory, +# radar-re/u11_encode) +# * replay: stationary objects vs GPS 70.75 counts per m/s; lead-stop 71.55 [71.26, 72.40]; road-speed +# approaches 70.90 [70.29, 71.40]. All three exclude 64. (radar-re/u11_gps, u11_leadstop, u11_dynamics) +# * OPEN: moving leads against the range rate (radar-re/u11_moving; 1/72 retrace 2026-10-02, R3/R4; Job's pooled fit, +# STATUS 7 src 2 U10<64, 55.7-70.0) bracket about 55-70 counts per m/s, which excludes 72; Job's per-dRel-band +# range check (D-074 second addendum) depends on the band, column A running 66.8-77.0, so the result is band- +# dependent rather than a clean exclusion. The encoder is firmware-proven 1/72 and the range scale is firmware- +# proven raw/16 (R18); the range offset (BOSCH_A_RANGE_OFFSET_M, -346/128 on Civic) is a constant and does not change a range +# slope. So this reads as a range-vs-U11 discrepancy (the range slope runs faster than U11), not a decode +# error. Unresolved; see STATUS. No closing-speed figure here is road-validated. +# Centre 864, rails raw 0/1728, sentinel 0x7FE, u10, range and azimuth are unchanged. 1/72 publishes 64/72 of the +# old closing speed (-11.1 %) and puts the rail at 12.0 instead of 13.5 m/s; no gate threshold was re-tuned. +# The decode divides by the count rate, v = (raw - 864) / 72 exactly as the firmware formatter's inverse; multiplying by +# a rounded 1/72 differs from it by one double ulp on 598 of the 1729 raws (none after the float32 publish). +BOSCH_A_DIRECT_VREL_COUNTS_PER_MPS = 72 +BOSCH_A_DIRECT_VREL_SCALE_MPS = 1.0 / BOSCH_A_DIRECT_VREL_COUNTS_PER_MPS + + +# The domain endpoints are SATURATION RAILS: at raw 0 or 1728 the true |vRel| is >= 12.0 m/s (13.5 in the 1/64 +# units the evidence below was logged in) and the exact value is not recoverable from this field. A rail is therefore a BOUND, not an unknown, +# and it must still be published. +# +# This was briefly treated as "no measurement" and routed to the coast path. That was a safety +# regression, because a stationary car approached above 12.0 m/s (27 mph; 13.5 / 30 mph at 1/64) rails on +# EVERY sweep -- so the coast never ended, it outlived BOSCH_A_STALE_S, and the radar point was +# deleted. Measured on route 000001f9 at 29:52: two stopped cars, 88 of 88 active frames on the low +# rail with healthy u10 (78-94), range closing smoothly at -19.4 m/s. The radar lead was dropped, +# radard fell back to the vision lead which reported only -11.4 m/s, and the planner commanded 0.00 +# while closing on stopped traffic at 76 m with a 6.6 s TTC. The driver had to intervene. +# +# Publishing the rail understates the closing rate (a stopped car reads as vLead = vEgo - 12.0), and +# that understatement is why it looked worth "fixing". But understating closing still brakes; +# deleting the object does not. Recovering the true value past the rail needs the range channel and +# is deliberately left for a separate, validated change. +BOSCH_A_DIRECT_VREL_RAILS_RAW = (BOSCH_A_DIRECT_VREL_MIN_RAW, BOSCH_A_DIRECT_VREL_MAX_RAW) +# D-063 (replay and static only; road evidence pending): the GATES must read a rail as the bound it +# is, too. The publish path above already does; the range-innovation gate (D-054) and the re-anchor +# test (D-057) did not -- they extrapolated the range at exactly -13.5 m/s (1/64). Route 0000025e at 12:03, +# stock ACC: track 59, the lead in lane from 102.8 m, U11 on the low rail on 51% of sweeps while +# the range closed at -15.4 to -17 m/s. The 2-3.5 m/s shortfall accumulated against the anchor, the +# range was rejected, the re-anchor refused because the fitted slope sat more than 3 m/s past the +# railed median, and the lead stayed dark until 41 m. A railed U11 now predicts an INTERVAL of range +# -- from the rail out to this physical cap -- and a range inside it is not an innovation. The cap is +# not a tuning value: it is a closing speed no same-direction or stationary target reaches from a +# car under ~145 km/h, and it keeps a rail from vouching for an unbounded range jump. +# This only lets the gates accept more; nothing is withheld that was published before, and the +# published vRel is still the rail itself. +BOSCH_A_DIRECT_VREL_RAIL_BOUND_MPS = 20.0 +# The rail interval only vouches for a track in our own lane. Replayed over 7 routes (STATUS 82, 89): +# with the interval on every track, 21 points were lost, all on tracks 24-32 m off-axis, and the +# over-closers were railed U11s on adjacent-lane tracks (25e track 6, 25f track 33, 262 track 9) +# whose range closed at 7-10 m/s. A 4 m gate kept the lead gains and lost 0 points; 2 m also cleared +# the adjacent-lane over-closers. Off-path tracks keep the exact (pre-D-063) gate. +BOSCH_A_RAIL_INTERVAL_MAX_Y_M = 2.0 +# On a DEGRADED sweep (range sigma, existence or u10 flagged) the interval applies only when the RANGE corroborates +# the rail: a least-squares fit over the TAIL of the rejected run plus this sweep (the shortest tail meeting the D-043 +# minimum window, 4 samples over 0.25 s) must lie within 3 m/s (the D-043 tolerance) of the rail interval. Otherwise the gate reads the rail +# exactly, as with the toggle off. A degraded sweep is gated at the tight 2.0 m limit because the sweep is +# suspect; the interval must not widen that on the rail's word alone. Replay evidence (STATUS 129): +# - Route 0000025e 6:43 (403 s), track 48: a range walk 47.9 -> 35.3 m admitted through the interval against a +# 0.53 s old baseline, all sweeps degraded, while the rejected run's last five ranges sat at 34.9-36.1 m (fit +# about -2 m/s, 11 m/s off the rail) and U11 left the rail on the next sweep. A fit over the last 8 sweeps still +# averaged the finished walk in (about -20 m/s) and admitted it; hence the shortest tail. The admitted rail was coasted for 0.25 s at 35 m +# and the planner went to -3.23 (vision and toggle-off lead at 42 m, -0.03). With the fit test it is not admitted. +# (Item 92's 402.912 s single-sweep admission has no run to fit and is read exactly too.) +# - Route 0000025e 12:03, track 59, the case D-063 exists for: degraded on EVERY sweep from 87 m in, admitted at +# 724.233 s against a baseline 2.16 s old. Its rejected run closes at about 16-18 m/s, inside the interval, so it +# is still admitted. Item 92's first suggestion (exact on every degraded sweep) kept it dark until 32 m, as with +# the toggle off; a baseline-age floor (0.31 s) kept 12:03 but still admitted the 6:43 walk. +# The D-057 re-anchor already refuses any window holding a degraded sweep, so this only changes the D-054 gate. +# No new number: the window and tolerance are D-043's. A bool so the replay can A/B it. +BOSCH_A_RAIL_INTERVAL_EXACT_WHEN_DEGRADED = True +# The item 92 coast bound is two-sided with the rail interval on. Its LESS-closing side exists to undo a +# rail-interval admission (a rail coasted on a range the interval let in), so it now acts only inside a hold such +# an admission started (`rail_hold`); every other coast keeps the one-sided STATUS 111 bound. Applied to every +# coast it softened three protected brakes in the 26-route replay, each a stale over-closing coast on a track the +# interval never touched: 00000232 1147.2 (-2.25 -> -1.88), 00000266 560.0 (-1.79 -> -1.38) and 00000266 795.4 +# (-1.5 crossing 3.75 s later). Replay evidence only (STATUS 129); a bool so the replay can A/B it. +BOSCH_A_RAIL_INTERVAL_DOWN_SIDE_ONLY_ON_RAIL_HOLD = True +# D-063 is built in (was the BoschARailInterval toggle; on in the owner's drives 00000287-0000028b). Replay, static and +# limited road evidence. Set False (replays, tests) and both gate calls below run `exact`, the pre-D-063 gate. +BOSCH_A_RAIL_INTERVAL = True +# The D-063 addendum's coast bound (_bosch_a_coast_vrel) also runs, without the rail interval, when the +# D-053 range-derived vRel switch is on. The coast is exactly where D-053 cannot act: radard disarms its +# assist on every unmeasured sample. Route 00000268 9:52.3-9:54.7 (STATUS 110): a 5 m range step as track +# 25 moved in made two opening U11 samples (+3.3, +4.1) the trusted vRel; the true closing U11 then failed +# the one-sided check against the stale fit and +4.06 was coasted for 2.35 s while the live, gated range +# closed 52.2 -> 38.3 m (~-6 m/s) and vision agreed. The bound pulls such a coast to within 3 m/s of the +# coast's own fresh range fit. Item 91's two wrong-direction brakes came from the rail-interval ADMISSION, +# not from this bound (item 92), so the bound is split out here, one-sided (more closing only; see +# _bosch_a_coast_vrel). Replay evidence only (STATUS 111). Built in (was the RangeDerivedVrel toggle). +BOSCH_A_COAST_RANGE_BOUND = True +# A coast that says the car ahead is reversing (vEgo + coasted vRel < -BOSCH_A_COAST_REVERSING_MARGIN_MPS) gets +# the two-sided bound even outside a rail hold: pulled to within BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS of +# the coast's own fresh range fit. With no fit yet it is floored at vLead = -margin instead. Everywhere else +# the STATUS 129 rule stands (less-closing side only inside a rail hold). Route 00000287 2:39 (STATUS 177): track +# 49 coasted its birth vRel -11.4 for 2.9 s while its range fell at about -4 m/s and ego slowed to 9.6 m/s, so +# the planner saw a lead reversing at -1.8 m/s and asked for -2.87; vision had the car at vRel -6 and the +# radar re-measured -6.8 on the next sweep. Replay: the coast publishes -7.2 instead of -11.4. A plain floor at +# vLead >= -margin only reached -10.8, and widening the down side to 5 m/s on every coast -9.2. STATUS 129's +# three protected brakes are stale over-closing coasts on real slower cars, not reversing ones, so they keep +# the one-sided bound. The point is always kept (D-041/D-042); only an implied reversing speed is bounded. A +# vEgo of None (not yet known) turns this off. Replay evidence only (STATUS 179); rides the same switches as the +# coast bound (BOSCH_A_RAIL_INTERVAL or BOSCH_A_COAST_RANGE_BOUND). +BOSCH_A_COAST_REVERSING_MARGIN_MPS = 1.0 +# Newborn range publish (REPLAY ONLY, no DECISIONS entry yet). A new identity whose U11 arrives with u10 above +# BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW has no trusted vRel to coast, so the coast branch withheld it and kept +# none of its ranges until u10 dropped. Route 000002ae seg 17 (Peter's bookmark, 1/72 on): track 39, a stopped +# car in lane, sat in that state for its first 17 sweeps from 116 m and reached liveTracks 1.28 s later at 89.7 m; +# its withheld ranges closed at -19.8 m/s, the rate the radar itself reported once u10 dropped. With this on, the +# withheld ranges are kept in `newborn_run` and, once their trailing D-043 window (4 sweeps over 0.25 s) fits a +# line within BOSCH_A_REANCHOR_MAX_RMS_M, the geometry is published unmeasured with vRel = that fit's rate, bounded +# at -vEgo (the most closing a stopped object can produce): the stationary bound. The live U11 is not used. Why: +# radard initialises its Track KF from the first published vRel and never updates it while measured=False, so the +# first newborn vRel freezes vLeadK for the whole run; and the live U11 at high u10 is junk on exactly these +# identities -- track 39 read -3.8 at u10 623, +2.4 at u10 1015, then sat on the rail -- so the first layer-1 form +# (live U11 clipped to the fit +-3, floored at the low rail) froze a lead far too fast. The fit agrees with the +# radar's own later low-u10 reading (-19.8). Radard's vision match and FAR_RAIL_VISION_BOUND also had to accept a +# range-verified closing young track (radard NEWBORN_RANGE_CLOSING_EXEMPT), since the camera had this stopped car at +# 15-18 m/s. Nothing is withheld that was published before (D-041/D-042); the first low-u10 sweep roots the range +# gate on the run's last range, so it is gated like any other sweep. Replay evidence only. +# Built in on (owner, 2026-10-03; it shipped as the BoschANewbornLeads toggle, which is removed), together with +# radard's three NEWBORN_* switches. RadarInterface copies it into self.newborn_range_publish; replays and tests +# set either one False for the code before the newborn publish. +# Night Star port: OFF. It is safe only with radard's NEWBORN_* exemptions, which 7.0's radard does not have. +BOSCH_A_NEWBORN_RANGE_PUBLISH = False + +# BOSCH_A_U11_FLOOR_UNMEASURED (2026-10-22; static + replay evidence only, no road A/B): a qualified sweep whose raw +# U11 is 0 (the -12.0 m/s floor) is NOT a velocity measurement. Route 00000008--94686ab4ae (the "rough" drive): +# 489 published points sat at raw 0, 377 of them measured=True, so radard's KF was updated with -12.0 on each. +# Their own range fits mostly read -17..-37 m/s (true saturation, the rail understates closing), but whole tracks +# read 0.00 / -0.13 / -0.54 / -0.61 / -4.76 m/s (raw 0 used where the field has nothing to say), and the leadTwo +# track 57 at 75.6-81.1 s fit -11.6. Either way the -12.0 is not the object's speed. Such a sweep now: +# * is range-gated exactly as before (D-063 rail interval; `direct_vrel` keeps the rail for the gate), +# * keeps its point and its accepted range history (`samples`, `range_anchor`): D-041/D-042, the point is never +# deleted for being on the floor, +# * publishes measured=False with vRel = the D-043 range fit over the accepted ranges (`_bosch_a_newborn_vrel`: +# trailing 4-sample / 0.25 s window, rms <= BOSCH_A_REANCHOR_MAX_RMS_M, bounded at -vEgo). With no fit yet an +# existing point keeps coasting (`_bosch_a_coast_vrel` if a trusted vRel exists, else its last published vRel), +# and a track born on the floor is not published until the fit exists, because radard freezes the first vRel +# of an unmeasured point (see BOSCH_A_NEWBORN_RANGE_PUBLISH), +# * never seeds or updates `last_trusted_vrel`. +# A high-u10 raw 0 already coasts through `high_u10_live_vrel` and is unchanged. +# UNRESOLVED: the top rail. Same route: 66 published sweeps at raw 1728 (+12.0), 1 at 1726. Tracks 53/6/19 fit +# +9.3/+15.3/+12.6 m/s (consistent with saturation, not with a sentinel), track 62 is 22 sweeps spread over 2 min +# (no usable fit). Too few to decide, so raw 1728 is still decoded as a measured +12.0 rail. +# False restores the decoder before this change (replays and tests). +BOSCH_A_U11_FLOOR_UNMEASURED = True + +# Seeding the first trusted sweep's history from the newborn run (samples, range_anchor, last_trusted_vrel) is off. +# Replay, 00000284 22:36.7 track 17: the newborn run was a 0.6 s range burst 88.7 -> 79.9 m (published fit -4.8 -> +# -17.6 m/s); seeded into the history, the coast after four measured sweeps held last_trusted_vrel -10.47 for 1.3 s +# while the range went 79.1 -> 83.2 -> 80.0 m, and the planner reached -1.0 m/s^2 (base: none below -0.88), where +# the unseeded track's coast followed the range (-0.5 .. -5.0). Replay evidence only. +BOSCH_A_NEWBORN_SEED_HISTORY = False +# u10 is a genuine uncertainty on U11, but it is CONFOUNDED WITH DYNAMICS. Measured against an +# event-local reference (quadratic fit to a centred window, derivative at the centre) over 16,834 +# frames: median |err| rises 0.26 -> 0.88 -> 1.44 -> 1.95 m/s across u10 bins 0-64 / 64-128 / +# 128-192 / 192-256, but median |a_rel| rises 0.73 -> 2.13 -> 3.51 -> 4.59 m/s^2 alongside it. +# corr(u10,|err|) = +0.40 and corr(u10,|a_rel|) = +0.43. Holding dynamics out, the quality signal is +# real (calm-frame corr +0.52) -- but u10 climbs during genuine hard braking just as reliably. +# +# 511 is the value validated in d5000fe344 by replaying 20 bookmarks through the real +# RadarInterface and radard's Kalman filter, where it collapsed recorded spikes up to -26.5 m/s^2 +# aLeadK. It was briefly lowered to 128 on error statistics alone; that undid the validated result +# and caused a measured regression -- on route 000001f3 at 19:27 u10 sat above 128 for 0.81 s during +# a real ~8 m/s^2 lead decel, the coast outlived BOSCH_A_STALE_S, the lead was deleted, and the +# vision fallback injected a 5 m / 6 m/s step that drove a -3.51 m/s^2 brake. Restored here. +# +# Do not re-tune this from offline error statistics. u10's confounding with dynamics means a lower +# threshold preferentially rejects real manoeuvring; the coast paths below are what must stay safe, +# not this number. +BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW = 511 + +# Multi-sweep velocity/range consistency. The existing per-sweep innovation gate cannot see a +# velocity error at all: over one ~70 ms sweep even a 3 m/s error moves the range by 0.2 m, far +# under its 2-5 m thresholds. Comparing U11 against a range rate fitted over several sweeps does +# have that power. +# +# Scope, measured on Peter's nine flagged events: this catches GROSS disagreement -- the 000001eb +# 6:59 event reported vRel -10.58 m/s while the range was actually opening at +0.46 m/s, an 11 m/s +# contradiction, across a track-identity change. It deliberately does NOT try to catch the milder +# 0.6-2.5 m/s overshoots seen at deceleration onset: a trailing window legitimately lags +# instantaneous velocity during real braking, so a threshold tight enough to catch those would also +# reject genuine hard decels. Those are the u10 gate's job. +# +# The test is ONE-SIDED, and that matters. U11 lags the true closure at a deceleration onset -- at +# 000001f3 19:27 the fitted range rate was -6.7 m/s while U11 still read -2.08 -- so a symmetric +# |U11 - rate| test fires during genuine hard braking and coasts exactly when the velocity is most +# needed. Lag can only make U11 UNDER-report closing while the lead is braking, so only the other +# direction is evidence of a fault: U11 claiming more closing than the geometry can support. +# Checked both ways: 000001eb 6:59 (U11 -10.58 while the range OPENED at +0.46) is rejected; +# the 000001f3 onset is not. In the mirror case, a lead accelerating away, the one-sided test +# coasts a more-conservative velocity, so it fails safe. +BOSCH_A_VREL_RATE_CHECK_MIN_SAMPLES = 4 +BOSCH_A_VREL_RATE_CHECK_MIN_SPAN_S = 0.25 +BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS = 3.0 + +# Measurement-authority policy. These are replay-derived safety/tuning gates, not recovered Bosch +# constants. Range innovation is measured from the previous accepted observation so a reset cannot +# become the baseline for following sweeps. +BOSCH_A_RANGE_SIGMA_DEGRADED_RAW = 4 +BOSCH_A_RANGE_INNOVATION_MAX_M = 2.0 +BOSCH_A_RANGE_INNOVATION_HARD_MAX_M = 5.0 +BOSCH_A_FALLBACK_RANGE_RATE_MAX_MPS = 50.0 +BOSCH_A_USE_TAN_LATERAL_PROJECTION = True + +# Accepted-range history is retained for continuity and the adjacent two-point fallback. Rejected +# range resets never enter it, and it is not used for a multi-sample OLS velocity fit. +BOSCH_A_VREL_MAX_SAMPLES = 8 + +# D-057 (replay and static only; road evidence pending): re-anchor a range-rejected identity on a LASTING, +# clean step. Replay of every range-rejection run on 00000232 / 236 / 237 / 239 / 23a (D-054 + D-055): the 13 runs of +# >= 3 sweeps that RETURNED to the baseline all lasted <= 1.2 s and every one was degraded (existence 0 +# on 62-83% of sweeps, U11 railed at -13.5 (1/64)). The lead lockouts were not: 236 track 38 (27.4 s) and 237 +# track 31 (20.4 s) had non-degraded tails whose range moved at the U11 rate. A run re-anchors only when +# it has outlasted every returning excursion, its last WINDOW sweeps are all non-degraded with U11 +# present, and their range fits a line at the U11 rate (RMS <= MAX_RMS, slope within the D-043 rate +# disagreement). Until then the identity stays coasted, never deleted (D-041). Re-based onto D-055 and +# replayed on 232/236/237/239/23a/23b/23e: 0 point-sweeps lost on every route; restores 236 track 38 +# (17.1 s) and 237 track 31 (16.2 s) as the lead and 237 track 63 (51.4 s, non-lead); re-admitted +# measured vRel over-closes the next-1 s range slope by >3 m/s on 2.2 / 2.2 / 0 % of sweeps (236 / +# 237 / 23a) against 5.4 / 5.9 / 2.6 % for the already-published reference. +BOSCH_A_REANCHOR_MIN_SPAN_S = 1.5 +BOSCH_A_REANCHOR_WINDOW = 8 +BOSCH_A_REANCHOR_MAX_RMS_M = 1.0 +# D-089 (STATUS 225; replay only, not driven): far-range recovery. RANGE_SIGMA_RAW grows with range (~0.07-0.09 x dRel), so it +# is >= BOSCH_A_RANGE_SIGMA_DEGRADED_RAW on every sweep beyond ~60 m and a far lead locked out by one D-054 range step could +# never re-anchor until it came close: 103 s, 22 % of camera-only lead time on 7 routes. For the D-057 window ONLY, range +# sigma is judged against max(BOSCH_A_RANGE_SIGMA_DEGRADED_RAW, FRAC x dRel); existence and u10 still count, and every other +# use of the degraded flag is unchanged. Alone this was rejected: the recovered U11 was wrong by 7-8 m/s on 2f2/2a4/2a6, both +# ways. So a point re-anchored only because of this rule is published with RadarPoint.recovered, and radard uses it only +# while the camera and its own range slope agree (RECOVERED_CAM_GATE). Without this rule the point would not exist at all, +# so an unconfirmed recovered point is the old picture. 0.15 is the one value replayed closed-loop (6 routes: 2f5 +34 s +# radar lead, no extra brake dip; one genuine slowdown 2f5 16:44.5 crossed -1.0 0.6 s later). +# Night Star port: 0 (off). The relaxed window is safe only behind radard's RECOVERED_CAM_GATE, which 7.0 lacks, and +# 7.0's RadarPoint has no `recovered` flag to carry; 0 keeps the shipped strict D-057 rule. +BOSCH_A_REANCHOR_RECOVER_SIGMA_FRAC = 0.0 + +# Staleness gate -- TUNING constant, reused plumbing pattern (not a firmware fact). At the observed +# ~15 Hz cadence, 0.20 s is approximately three missed sweeps. +BOSCH_A_STALE_S = 0.20 + + +@dataclass +class _BoschASlotState: + last_seen_nanos: int | None = None + + # Firmware logical-ID companion data -- telemetry/debug only for now, never a RadarPoint validity + # gate. 0x00C9 has a firmware transform but no proven physical meaning; 0x00CA has an explicit + # invalid sentinel and its physical meaning remains unresolved. + logical_00c9_raw: float = float('nan') + logical_00ca_raw: float = float('nan') + direct_vrel_raw: int | None = None + direct_vrel_uncertainty_raw: int | None = None + + def reset(self): + self.last_seen_nanos = None + self.logical_00c9_raw = float('nan') + self.logical_00ca_raw = float('nan') + self.direct_vrel_raw = None + self.direct_vrel_uncertainty_raw = None + + +@dataclass +class _BoschATrackState: + """Persistent state for one Bosch CAN object identity, independent of wire slot.""" + track_id: int + prev_frame_idx: int | None = None + prev_life: int | None = None + last_seen_nanos: int | None = None + wire_slot: int | None = None + samples: deque = field(default_factory=lambda: deque(maxlen=BOSCH_A_VREL_MAX_SAMPLES)) + # (time, range) of the last observation that PASSED the range-innovation gate, accepted or coasted + # (D-054). The gate measures the next sweep against it. It is never a velocity-derivative baseline: + # `samples` above stays accepted-only, and a range-rejected sweep never moves either of them. + range_anchor: tuple[float, float] | None = None + # D-057: the current run of range-REJECTED sweeps, (time, range, U11 or None, degraded). + # Cleared by any accepted sweep and by a lifecycle discontinuity. + rejected_run: list = field(default_factory=list) + # D-062: the current run of range-PASSED sweeps whose vRel failed the rate check, same tuple shape. + # Cleared by any rate-consistent sweep, any range rejection and a lifecycle discontinuity. + inconsistent_run: list = field(default_factory=list) + rejoin_samples: list | None = None # D-059: gated ranges since a join, until a fresh rate fit agrees with vRel + # D-063: the current D-059 hold was started (or joined) by a rail-interval admission. Only such a hold's coast + # may be pulled LESS closing toward its fresh fit (_bosch_a_coast_vrel); cleared whenever the hold ends. + rail_hold: bool = False + last_trusted_vrel: float | None = None + last_trusted_vrel_nanos: int | None = None + # BOSCH_A_NEWBORN_RANGE_PUBLISH: (time, range) of high-u10 sweeps before the first accepted sample. + newborn_run: list = field(default_factory=list) + newborn_vrel: float | None = None # the vRel the newborn point was last published with + floor_vrel: float | None = None # BOSCH_A_U11_FLOOR_UNMEASURED: the unmeasured vRel a raw-0 sweep last published + floor_run: list[tuple[float, float]] = field(default_factory=list) # raw-0 sweeps' (t, dRel), for the fit only + floor_seen: bool = False # a raw-0 sweep was seen in this lifecycle (BOSCH_A_U11_FLOOR_UNMEASURED) + # D-089: the anchor came from a re-anchor only the range-scaled sigma window allowed (RadarPoint.recovered). Cleared by + # BOSCH_A_REANCHOR_WINDOW consecutive accepted sweeps that are non-degraded by the shipped rule, i.e. the evidence a + # shipped D-057 re-anchor would have needed, and by a lifecycle discontinuity. + recovered: bool = False + strict_clean_count: int = 0 + # The F0 STATUS nibble of the latest valid observation (0 before the first one); see BoschARadar.point_statuses. + status: int = 0 + rejected_strict_dirty_t: float = -1e9 # last rejected sweep that only the relaxed window called clean + + +def _bosch_a_newborn_vrel(run: list, v_ego: float | None) -> float | None: + """BOSCH_A_NEWBORN_RANGE_PUBLISH: the vRel to publish for a newborn high-u10 run, or None while the run is too short + or its ranges do not fit a line within BOSCH_A_REANCHOR_MAX_RMS_M. The fit's own rate, bounded at -vEgo (a stopped + object is the most closing physically possible). The high-u10 live U11 is not used: radard freezes the first + published vRel of an unmeasured point, and that U11 is junk here (see the constant's comment block).""" + window = _bosch_a_trailing_fit_window(run) + rate = _bosch_a_fresh_range_rate(window) + if rate is None: + return None + ts = [w[0] for w in window] + ds = [w[1] for w in window] + n = len(window) + t_mean = sum(ts) / n + d_mean = sum(ds) / n + rms = (sum((d - (d_mean + rate * (t - t_mean))) ** 2 for t, d in zip(ts, ds, strict=True)) / n) ** 0.5 + if rms > BOSCH_A_REANCHOR_MAX_RMS_M: + return None + return rate if v_ego is None else max(rate, -v_ego) + + +def _bosch_a_direct_vrel(raw_value: int | float | None, + uncertainty_raw: int | float | None = None, + counts_per_mps: int = BOSCH_A_DIRECT_VREL_COUNTS_PER_MPS) -> float | None: + """Decode the capture-validated AUX relative-velocity candidate. + + None means that AUX was absent/invalid and the caller should use the existing fallback policy. The [0, 1728] + active domain is deliberately enforced here because raw values above the observed 1728 rail (+12.0 m/s) have + not appeared on active objects; 0x7FE is the observed inactive sentinel. `counts_per_mps` is 72 (D-074). + """ + if raw_value is None: + return None + raw = int(raw_value) + if raw == BOSCH_A_DIRECT_VREL_INVALID: + return None + if not BOSCH_A_DIRECT_VREL_MIN_RAW <= raw <= BOSCH_A_DIRECT_VREL_MAX_RAW: + return None + # u10 is retained as a raw quality indicator because its physical units remain unresolved. The + # conservative threshold is evidence-backed tuning, not a recovered firmware validity rule. + if uncertainty_raw is not None and int(uncertainty_raw) > BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW: + return None + return (raw - BOSCH_A_DIRECT_VREL_CENTER_RAW) / counts_per_mps + + +def _bosch_a_range_ratio(raw_value: int | float | None) -> float | None: + if raw_value is None: + return None + raw = int(raw_value) + if raw == BOSCH_A_RANGE_RATIO_INVALID or not 0 <= raw < BOSCH_A_RANGE_RATIO_INVALID: + return None + return BOSCH_A_RANGE_RATIO_OFFSET + BOSCH_A_RANGE_RATIO_SCALE * raw + + +def _bosch_a_range_ratio_vrel(raw_value: int | float | None, d_rel: float, dt: float) -> float | None: + """Return the range rate implied by the empirical previous/current range ratio.""" + ratio = _bosch_a_range_ratio(raw_value) + if ratio is None or dt <= 0.0 or not math.isfinite(d_rel): + return None + return d_rel * (1.0 - ratio) / dt + + +def _bosch_a_direct_vrel_interval(direct_vrel: float, exact: bool = False, + counts_per_mps: int = BOSCH_A_DIRECT_VREL_COUNTS_PER_MPS) -> tuple[float, float]: + """D-063: the vRel interval a decoded U11 vouches for. A rail is a one-sided bound, anything else exact. + `counts_per_mps` must be the one the U11 was decoded with (D-074), so the rail test matches the decode.""" + low_rail = (BOSCH_A_DIRECT_VREL_MIN_RAW - BOSCH_A_DIRECT_VREL_CENTER_RAW) / counts_per_mps + high_rail = (BOSCH_A_DIRECT_VREL_MAX_RAW - BOSCH_A_DIRECT_VREL_CENTER_RAW) / counts_per_mps + if exact: + return direct_vrel, direct_vrel + if direct_vrel <= low_rail: + return -BOSCH_A_DIRECT_VREL_RAIL_BOUND_MPS, direct_vrel + if direct_vrel >= high_rail: + return direct_vrel, BOSCH_A_DIRECT_VREL_RAIL_BOUND_MPS + return direct_vrel, direct_vrel + + +def _bosch_a_distance_to_interval(value: float, interval: tuple[float, float]) -> float: + return max(0.0, interval[0] - value, value - interval[1]) + + +def _bosch_a_range_innovation_rejected(baseline: tuple[float, float], now_s: float, dRel: float, + direct_vrel: float | None, ratio: float | None, degraded: bool, + exact: bool = False, + counts_per_mps: int = BOSCH_A_DIRECT_VREL_COUNTS_PER_MPS) -> bool: + """Does this range contradict one (time, range) baseline? See the D-054 comment at the call site.""" + previous_time, previous_range = baseline + dt = now_s - previous_time + if dt <= 0.0: + return True + residuals_m = [] + if direct_vrel is not None: + low, high = _bosch_a_direct_vrel_interval(direct_vrel, exact, counts_per_mps) + residuals_m.append(_bosch_a_distance_to_interval(dRel, (previous_range + low * dt, previous_range + high * dt))) + if ratio is not None: + residuals_m.append(abs(previous_range - dRel * ratio)) + if not residuals_m: + return abs((dRel - previous_range) / dt) > BOSCH_A_FALLBACK_RANGE_RATE_MAX_MPS + innovation_m = min(residuals_m) + return (innovation_m > BOSCH_A_RANGE_INNOVATION_HARD_MAX_M or + (degraded and innovation_m > BOSCH_A_RANGE_INNOVATION_MAX_M)) + + +def _bosch_a_measurement_degraded(range_sigma_raw: int, existence_raw: int, + direct_vrel_uncertainty_raw: int | None, + range_sigma_limit_raw: float = BOSCH_A_RANGE_SIGMA_DEGRADED_RAW) -> bool: + range_quality_bad = range_sigma_raw >= range_sigma_limit_raw or existence_raw in (0, 0x7F) + velocity_quality_bad = (direct_vrel_uncertainty_raw is not None and + direct_vrel_uncertainty_raw > BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW) + return range_quality_bad or velocity_quality_bad + + +def _bosch_a_lasting_clean_step(run: list, exact: bool = False, + counts_per_mps: int = BOSCH_A_DIRECT_VREL_COUNTS_PER_MPS) -> bool: + """D-057: True when the trailing window of a range-rejected run is long, clean and moving at the U11 rate.""" + if len(run) < BOSCH_A_REANCHOR_WINDOW or run[-1][0] - run[0][0] < BOSCH_A_REANCHOR_MIN_SPAN_S: + return False + window = run[-BOSCH_A_REANCHOR_WINDOW:] + if any(degraded or u11 is None for _, _, u11, degraded in window): + return False + ts = [w[0] for w in window] + ds = [w[1] for w in window] + n = len(window) + t_mean = sum(ts) / n + d_mean = sum(ds) / n + denom = sum((t - t_mean) ** 2 for t in ts) + if denom <= 1e-9: + return False + rate = sum((t - t_mean) * (d - d_mean) for t, d in zip(ts, ds, strict=True)) / denom + rms = (sum((d - (d_mean + rate * (t - t_mean))) ** 2 for t, d in zip(ts, ds, strict=True)) / n) ** 0.5 + u11 = sorted(w[2] for w in window)[n // 2] + return (rms <= BOSCH_A_REANCHOR_MAX_RMS_M and + _bosch_a_distance_to_interval(rate, _bosch_a_direct_vrel_interval(u11, exact, counts_per_mps)) <= + BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS) + + +def _bosch_a_fresh_range_rate(run: list) -> float | None: + """Least-squares range rate over a run of (t_s, dRel, ...) tuples that grows on EVERY sweep of a coast + (`rejoin_samples`, `inconsistent_run`), i.e. one that is not frozen the way `samples` is during a coast + (D-062). None until the D-043 minimum window (4 samples over 0.25 s) is met.""" + if len(run) < BOSCH_A_VREL_RATE_CHECK_MIN_SAMPLES or run[-1][0] - run[0][0] < BOSCH_A_VREL_RATE_CHECK_MIN_SPAN_S: + return None + ts = [w[0] for w in run] + ds = [w[1] for w in run] + n = len(run) + t_mean = sum(ts) / n + d_mean = sum(ds) / n + denom = sum((t - t_mean) ** 2 for t in ts) + if denom <= 1e-9: + return None + return sum((t - t_mean) * (d - d_mean) for t, d in zip(ts, ds, strict=True)) / denom + + +def _bosch_a_trailing_fit_window(run: list) -> list: + """The shortest tail of `run` that meets the D-043 minimum window (BOSCH_A_VREL_RATE_CHECK_MIN_SAMPLES samples + over BOSCH_A_VREL_RATE_CHECK_MIN_SPAN_S), or the whole run when it is shorter. The rate the range has NOW, not + the average over a step that is already over (STATUS 129, 0000025e 6:43).""" + for start in range(len(run) - BOSCH_A_VREL_RATE_CHECK_MIN_SAMPLES, -1, -1): + if run[-1][0] - run[start][0] >= BOSCH_A_VREL_RATE_CHECK_MIN_SPAN_S: + return run[start:] + return run + + +# D-104: the coast range bound fits only the last BOSCH_A_COAST_FIT_WINDOW_S of `inconsistent_run`, the range rate the +# lead has now. Route 00000311 84:21 (track 33, ~100 m): a +2.6 m range jump made D-062 coast the -1.5 vRel, and every +# later sweep was degraded (far range sigma), so D-062 never re-rooted and the coast ran 14 s (211 sweeps). Fitting the +# whole run averaged the old near-steady ranges in and lagged (-0.9..-4.8) while U11 read -4..-7.7 and the last 1 s of +# range -5..-11. The car closed late and braked -1.60 (replay -1.78). With 1.0 s: replay -1.43, braking starts ~3 s +# earlier, min time to contact 4.9 -> 7.0 s. The bound stays one-sided outside a rail hold (more closing only), and +# 1.0 s is 4x the D-043 minimum span, so a single bad sweep cannot set it. Replay (testing agent, 23 windows incl. the +# protected real brakes): 21 identical, 25b/25f within 0.01; the only other change is 311 86:35, sim -4.27 -> -4.38 +# with identical radar input (sim state carried over from 84:21). 0 restores the whole-run fit. +BOSCH_A_COAST_FIT_WINDOW_S = 1.0 + + +def _bosch_a_coast_vrel(track, rail_interval: bool, range_bound: bool = False, v_ego: float | None = None) -> float: + """The vRel a coast publishes. Off (pre-D-063): the last trusted vRel, verbatim. With the rail interval on + (D-063 addendum, STATUS 92): the same value bounded to within BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS of + a FRESH fit over the ranges of the coast itself, when one exists. Replayed on 0000025e 6:43 (track 48): the + rail interval admitted a slot-merge range walk, the D-059 hold then coasted the -13.5 (1/64) rail for 1.5 s while + its own post-join fit read +0.8 m/s and said so (fresh_ok false on every sweep), and the planner asked for + -3.45 m/s^2 against a lead the log and vision put at -1..-3 m/s. With the bound, replay of that window + bottomed at -2.39 and never crossed -3.0. The hold's one-sided law (a vRel that over-closes the fresh + range fit by > 3 m/s is not trusted) is applied to the value it coasts, both ways, so a stale opening + vRel is pulled toward a closing fit once one exists. Never a one-sweep derivative: the fit is the D-043 + multi-sample fit, and with fewer than 4 fresh samples (0.25 s) the coast is unchanged. That is why it + cannot act on 0000025e 12:05 (+11.1 coasted for 0.2 s at track birth, no fresh samples yet) nor on the + first 5 sweeps after the 6:43 re-root, which still coast the rail. The point is always kept (D-041/D-042). + Replay evidence only (STATUS 92). + + With only the range bound on (`range_bound`, STATUS 111) the bound is ONE-SIDED, D-053's rule: a coast may + be made more closing, never less. The two-sided clamp without the rail interval softened 6 protected + brakes on the 20-route replay (237 9:58.5 -3.10 -> -2.38, 266 8:04 -1.98 -> -1.48, 266 9:20.9 -1.66 -> + -1.11, ...): the down side exists to undo a rail-interval admission, which this path never makes. + + With the rail interval on, the down side is likewise limited to a coast inside a hold that a rail-interval + admission started (`track.rail_hold`, STATUS 129); every other coast gets the one-sided bound. Applied to every + coast, the down side softened three protected brakes in the 26-route replay (00000232 1147.2 -2.25 -> -1.88, + 00000266 560.0 -1.79 -> -1.38, 00000266 795.4 -1.5 crossing 3.75 s later), none of them near a rail admission. + + Either switch on, a coast that implies the car ahead is reversing (v_ego + vRel < -BOSCH_A_COAST_REVERSING_MARGIN_MPS) + also gets the down side, or with no fit a floor at vLead = -margin (route 00000287 2:39, STATUS 179).""" + vrel = track.last_trusted_vrel + if not (rail_interval or range_bound): + return vrel + rate = None + if track.rejoin_samples is not None: + rate = _bosch_a_fresh_range_rate(track.rejoin_samples) + if rate is None: + run = track.inconsistent_run + if BOSCH_A_COAST_FIT_WINDOW_S > 0 and run: + run = [w for w in run if w[0] >= run[-1][0] - BOSCH_A_COAST_FIT_WINDOW_S] + rate = _bosch_a_fresh_range_rate(run) + reversing = v_ego is not None and v_ego + vrel < -BOSCH_A_COAST_REVERSING_MARGIN_MPS + if rate is None: + if reversing: + vrel = max(vrel, -(v_ego + BOSCH_A_COAST_REVERSING_MARGIN_MPS)) + return vrel + vrel = min(vrel, rate + BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS) + if reversing or (rail_interval and (track.rail_hold or not BOSCH_A_RAIL_INTERVAL_DOWN_SIDE_ONLY_ON_RAIL_HOLD)): + vrel = max(vrel, rate - BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS) + return vrel + + +def _create_bosch_a_can_parser(CP): + messages = [(addr, BOSCH_A_FREQ_HZ) for addr in BOSCH_A_ALL_IDS] + # Bus.radar selects the Bosch-A DBC; the object/fusion feed itself is physically on the camera-side ACC-CAN. + return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, CanBus(CP).camera) + + +def bosch_a_alt_radar_car(car_fingerprint: str) -> bool: + """Bosch harness with BOSCH_ALT_RADAR (stock AOL family); same Bosch-A object bank, different ACC integration.""" + if car_fingerprint not in CAR: + return False + flags = int(CAR[car_fingerprint].config.flags) + return (bool(flags & HondaFlags.BOSCH) and bool(flags & HondaFlags.BOSCH_ALT_RADAR) and + not bool(flags & HondaFlags.BOSCH_CANFD)) + + +def bosch_a_radar_platform(car_fingerprint: str) -> bool: + """Bosch-A object-bank cars wired to honda_bosch_a_radar (classic harness and alt-radar stock).""" + if car_fingerprint not in CAR: + return False + if DBC.get(car_fingerprint, {}).get(Bus.radar) != BOSCH_A_DBC_NAME: + return False + flags = int(CAR[car_fingerprint].config.flags) + unsupported = int(HondaFlags.BOSCH_RADARLESS | HondaFlags.BOSCH_CANFD) + return bool(flags & HondaFlags.BOSCH) and not bool(flags & unsupported) + + +BOSCH_A_ALT_RADAR_CARS = frozenset(c for c in CAR if bosch_a_alt_radar_car(c) and bosch_a_radar_platform(c)) + +# Same set as bosch_a_radar_platform: honda_bosch_a_radar wired, not radarless / CANFD. +VERIFIED_BOSCH_A_CARS = frozenset(c for c in CAR if bosch_a_radar_platform(c)) + + +class BoschARadar(RadarInterfaceBase): + def __init__(self, CP, CP_SP): + super().__init__(CP, CP_SP) + if not bosch_a_radar_platform(CP.carFingerprint): + raise ValueError("Honda radar requires a classic Bosch-A platform with honda_bosch_a_radar wired") + self.rcp = _create_bosch_a_can_parser(CP) + self.trigger_msg = BOSCH_A_TRIGGER_MSG + # Ego speed, set by card before each update (the reversing-coast bound). None until then. + self.v_ego: float | None = None + # F0 STATUS per point of the last RadarData returned (7 confirmed; 9 on route 34 T479's phantom lead). card + # carries it to radard in starpilotCarState.radarPointStatus: car.capnp's RadarPoint is upstream-owned on 7.0. + self.point_statuses: dict[int, int] = {} + self._slots = [_BoschASlotState() for _ in range(BOSCH_A_NUM_SLOTS)] + self._tracks: dict[int, _BoschATrackState] = {} + self._slot_track_ids: list[int | None] = [None] * BOSCH_A_NUM_SLOTS + self._last_trigger_nanos = -1 + self.rail_interval = BOSCH_A_RAIL_INTERVAL + self.coast_range_bound = BOSCH_A_COAST_RANGE_BOUND + # D-074: U11 counts per m/s (72). An attribute so replays of 1/64-era logs can set it. + self.u11_counts_per_mps = BOSCH_A_DIRECT_VREL_COUNTS_PER_MPS + self.newborn_range_publish = BOSCH_A_NEWBORN_RANGE_PUBLISH + self.u11_floor_unmeasured = BOSCH_A_U11_FLOOR_UNMEASURED + # D-076: firmware ROM-default range offset (-2.703 m Civic, -2.664 m CR-V). Fixed per car. + self.range_offset_m = bosch_a_range_offset_m(CP.carFingerprint) + self.updated_messages = set() + + def update(self, can_strings): + self.updated_messages.update(self.rcp.update(can_strings)) + if self.trigger_msg not in self.updated_messages: + if self._last_trigger_nanos >= 0 and (self.rcp._last_update_nanos - self._last_trigger_nanos) * 1e-9 > BOSCH_A_STALE_S: + return self._bosch_a_stale_radardata() + return None + result = self._update_bosch_a(self.updated_messages) + self.updated_messages.clear() + return result + + def _bosch_a_stale_radardata(self): + # Whole-bus silence: clear every live point/history and emit an EMPTY RadarData (not None) so + # radard drops any lead within a cycle instead of freezing a phantom. The next observation starts + # a fresh incarnation for its CAN identity. + self.pts.clear() + self._tracks.clear() + self._slot_track_ids = [None] * BOSCH_A_NUM_SLOTS + for slot_state in self._slots: + slot_state.reset() + self._last_trigger_nanos = -1 + stale = structs.RadarData() + if not self.rcp.can_valid: + stale.errors.canError = True + stale.errors.radarUnavailableTemporary = True + return stale + + def _bosch_a_retire_track(self, track_id: int): + self._tracks.pop(track_id, None) + self.pts.pop(track_id, None) + for slot, slot_track_id in enumerate(self._slot_track_ids): + if slot_track_id == track_id: + self._slot_track_ids[slot] = None + + def _bosch_a_retire_stale_tracks(self, now: int): + for track_id, track in list(self._tracks.items()): + if track.last_seen_nanos is not None and (now - track.last_seen_nanos) * 1e-9 > BOSCH_A_STALE_S: + self._bosch_a_retire_track(track_id) + + def _update_bosch_a(self, updated_messages): + ret = structs.RadarData() + if not self.rcp.can_valid: + ret.errors.canError = True + + now = self.rcp._last_update_nanos + self._last_trigger_nanos = now + self._bosch_a_retire_stale_tracks(now) + + observations = [] + + for slot in range(BOSCH_A_NUM_SLOTS): + f0, f1, f2, f3 = BOSCH_A_MAIN_IDS[slot] + aux = BOSCH_A_AUX_IDS[slot] + st = self._slots[slot] + + if not (f0 in updated_messages and f1 in updated_messages and + f2 in updated_messages and f3 in updated_messages): + # Incomplete main-frame set: a missing CAN frame must not be treated as a lifecycle mismatch or + # death/replacement. Logical tracks are retired independently by their global staleness gate. + continue + + v0 = self.rcp.vl[f0] + v1 = self.rcp.vl[f1] + v2 = self.rcp.vl[f2] + v3 = self.rcp.vl[f3] + + idx0 = int(v0['FRAME_IDX']) + if not (idx0 == int(v1['FRAME_IDX']) == int(v2['FRAME_IDX']) == int(v3['FRAME_IDX'])): + # The four main frames don't share a common cycle index -- not a coherent observation this + # window. Only combine main-frame data from a coherent common frame index (section 2). + continue + + status = int(v0['STATUS']) + range_raw = int(v0['RANGE_RAW']) + angle_raw = int(v0['AZIMUTH_RAW']) + range_sigma_raw = int(v0['RANGE_SIGMA_RAW']) + existence_raw = int(v1['OBJECT_EXISTENCE_PROBABILITY_RAW']) + life = int(v2['LIFECYCLE_RAW']) + track_id = int(v3['TRACK_ID']) + track_id_valid = BOSCH_A_TRACK_ID_MIN <= track_id <= BOSCH_A_TRACK_ID_MAX + st.last_seen_nanos = now + direct_vrel_raw = None + direct_vrel_uncertainty_raw = None + range_ratio_raw = None + + # Attach synchronized companion data only when its cycle matches. Missing AUX never invalidates + # an otherwise coherent F0-F3 object; it only removes independent motion/quality evidence. + if aux in updated_messages: + av = self.rcp.vl[aux] + if int(av['FRAME_IDX']) == idx0: + direct_vrel_raw = int(av['REL_VELOCITY_RAW']) + direct_vrel_uncertainty_raw = int(av['REL_VELOCITY_UNCERTAINTY_RAW']) + range_ratio_raw = int(av['RANGE_RATIO_RAW']) + logical_00c9_raw = av['FW_LID_00C9_RAW'] + logical_00ca_raw = av['FW_LID_00CA_RAW'] + st.logical_00c9_raw = logical_00c9_raw + st.logical_00ca_raw = ( + logical_00ca_raw if logical_00ca_raw != BOSCH_A_LOGICAL_00CA_INVALID else float('nan') + ) + st.direct_vrel_raw = direct_vrel_raw + st.direct_vrel_uncertainty_raw = direct_vrel_uncertainty_raw + + object_valid = (status != BOSCH_A_STATUS_INVALID and range_raw != BOSCH_A_RANGE_RAW_INVALID and + angle_raw != BOSCH_A_ANGLE_RAW_INVALID and life != BOSCH_A_LIFE_INVALID) + + observations.append({ + 'slot': slot, + 'frame_idx': idx0, + 'life': life, + 'track_id': track_id, + 'track_id_valid': track_id_valid, + 'object_valid': object_valid, + 'status': status, + 'range_sigma_raw': range_sigma_raw, + 'existence_raw': existence_raw, + 'direct_vrel_raw': direct_vrel_raw, + 'direct_vrel_uncertainty_raw': direct_vrel_uncertainty_raw, + 'range_ratio_raw': range_ratio_raw, + }) + + # First collapse duplicate wire observations of one CAN identity. The dictionary is also the + # output uniqueness boundary: one valid Bosch identity can never create two RadarPoints. + valid_by_id = {} + # D-055 (replay and static only; road evidence pending): an identity valid in ANY slot this sweep + # is decided by its own valid observation below, never hidden by an invalid one, mirroring the + # slot-replacement guard. Only an accept re-creates a hidden point; a coast updates an existing + # one. So when the radar migrated an object to another slot and the old slot went invalid in the + # same sweep, the hide below deleted a point the coast would have kept. Measured on 00000232 / + # 236 / 237 / 239 / 23a: 2,252 withheld point-sweeps, 504 of them the lead (00000237 track 9: + # 21.2 s dark). On the fresh routes 0000023b / 23e (driven on D-054, 0756f810): 0 point-sweeps + # lost, 9 + 331 restored (33 lead), all coasts — no new measured vRel enters control. + valid_this_sweep = {o['track_id'] for o in observations if o['object_valid'] and o['track_id_valid']} + for observation in observations: + if not (observation['object_valid'] and observation['track_id_valid']): + # An invalid observation ends publication for the object currently occupying this wire slot, + # but it does not immediately destroy the persistent state. The object may be multiplexed to + # another slot or may return before the per-identity stale deadline; a later lifecycle break + # or staleness expiry will clear its derivative history. + ids_to_hide = {self._slot_track_ids[observation['slot']]} + if observation['track_id_valid']: + ids_to_hide.add(observation['track_id']) + for invalid_id in ids_to_hide - {None} - valid_this_sweep: + self.pts.pop(invalid_id, None) + continue + track_id = observation['track_id'] + current = valid_by_id.get(track_id) + if current is None: + valid_by_id[track_id] = observation + continue + + # Prefer the wire slot currently associated with the persistent state. If neither candidate is + # preferred, retain the lower slot for deterministic handling of a malformed duplicate frame. + track = self._tracks.get(track_id) + current_score = (0 if track is not None and track.wire_slot == current['slot'] else 1, current['slot']) + candidate_score = (0 if track is not None and track.wire_slot == observation['slot'] else 1, + observation['slot']) + if candidate_score < current_score: + valid_by_id[track_id] = observation + + valid_ids = set(valid_by_id) + for track_id, observation in sorted(valid_by_id.items(), key=lambda item: item[1]['slot']): + slot = observation['slot'] + idx0 = observation['frame_idx'] + life = observation['life'] + + # A wire-slot replacement ends publication for the old occupant, but not its persistent + # identity/history. If the old ID is still valid in another slot this sweep, keep its point; + # otherwise hide it until that ID returns or reaches its own stale deadline. + old_id = self._slot_track_ids[slot] + if old_id is not None and old_id != track_id and old_id not in valid_ids: + self.pts.pop(old_id, None) + + track = self._tracks.get(track_id) + if track is None: + track = _BoschATrackState(track_id=track_id) + self._tracks[track_id] = track + + same_incarnation = False + if track.prev_frame_idx is not None and track.prev_life is not None: + frame_delta = (idx0 - track.prev_frame_idx) & 0xF + life_delta = (life - track.prev_life) & 0xFFF + same_incarnation = life_delta == 2 * frame_delta + if not same_incarnation and life == BOSCH_A_LIFE_SATURATED and track.prev_life == BOSCH_A_LIFE_SATURATED: + # The counter is pinned at its maximum and can no longer advance (see the constant's + # evidence block). Without this, an object tracked for ~137 s is deleted for as long as it + # remains visible -- measured at 121.8 s of continuous suppression of the followed lead. + same_incarnation = True + + if not same_incarnation: + # The CAN identity remains the externally-visible key, but a lifecycle discontinuity starts a + # new incarnation and must not inherit the previous object's range-rate history. + track.samples.clear() + track.range_anchor = None + track.rejected_run.clear() + track.inconsistent_run.clear() + track.rejoin_samples = None + track.rail_hold = False + track.recovered = False + track.strict_clean_count = 0 + track.last_trusted_vrel = None + track.last_trusted_vrel_nanos = None + track.newborn_run.clear() + track.newborn_vrel = None + track.floor_vrel = None + track.floor_run.clear() + track.floor_seen = False + self.pts.pop(track_id, None) + track.status = observation['status'] + + v0 = self.rcp.vl[BOSCH_A_MAIN_IDS[slot][0]] + range_raw = int(v0['RANGE_RAW']) + angle_raw = int(v0['AZIMUTH_RAW']) + dRel = BOSCH_A_RANGE_SCALE_M * range_raw + self.range_offset_m + azimuth_rad = BOSCH_A_AZIMUTH_SCALE_RAD * (angle_raw - BOSCH_A_AZIMUTH_CENTER) + # The firmware's internal geometry path uses tan(angle) for a forward-axis distance. The + # Bosch object range is consumed as that forward-axis quantity here, so use the same projection + # rather than treating it as radial/slant range. Keep the switch explicit while the final + # output bridge remains under static review. + # + # Sign: AZIMUTH_RAW > center (positive azimuth_rad) is a LEFT-of-center detection in car frame's + # y axis (left is positive), matching the Nidec RadarPoint.yRel contract. Confirmed against real + # captures 2026-08-22: a stationary cluster of queued vehicles visible on the left in the road + # camera was rendering on the right in both the Qt and raylib radar-track overlays with the + # previously-flipped sign. + lateral_projection = math.tan if BOSCH_A_USE_TAN_LATERAL_PROJECTION else math.sin + yRel = dRel * lateral_projection(azimuth_rad) + + now_s = now * 1e-9 + direct_vrel_raw = observation['direct_vrel_raw'] + direct_vrel_uncertainty_raw = observation['direct_vrel_uncertainty_raw'] + direct_vrel = _bosch_a_direct_vrel(direct_vrel_raw, direct_vrel_uncertainty_raw, self.u11_counts_per_mps) + live_direct_vrel = _bosch_a_direct_vrel(direct_vrel_raw, counts_per_mps=self.u11_counts_per_mps) + range_ratio_raw = observation['range_ratio_raw'] + + # A live U11 rejected solely by the conservative U10 qualification threshold is neither an + # unavailable velocity nor permission to synthesize a one-sweep range derivative. It is also + # NOT permission to skip range-innovation checking below: u10 correlates with range_sigma in + # replay data, so a high-u10 sweep is exactly the condition where a bad/discontinuous range + # (slot migration, reset) is most likely, not less likely. Range acceptance is therefore + # decided on the same terms as every other sweep first; only once the range clears that gate + # does a high-u10 U11 fall back to coasting instead of publishing a synthesized derivative. + high_u10_live_vrel = (direct_vrel is None and live_direct_vrel is not None and + direct_vrel_uncertainty_raw is not None and + direct_vrel_uncertainty_raw > BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW) + + # Qualified U11 and the range-ratio field are independent corroboration paths for the range; + # high-U10 U11 is deliberately excluded from this decision. + # + # D-054: a sweep is range-rejected only if it contradicts BOTH the last ACCEPTED sample and + # `range_anchor`, the last range that passed this gate (a velocity coast also advances it). Each + # baseline on its own locked a real object out, measured through this parser on 00000232 / 236 / + # 237 / 239 / 23a: + # - Accepted sample only (the parser before D-054). A coast held it still while the range kept + # moving, and every later sweep was predicted across the growing gap with the very U11 the coast + # had just distrusted. 00000232 track 43, the followed lead pulling away 78.5 -> 84.5 m: the + # D-043 rate check coasted four sweeps, the error grew 0.86 / 1.12 / 1.53 / 1.87 / 2.11 m, the + # degraded 2.0 m limit rejected it, the point was deleted after BOSCH_A_STALE_S, and radar lost + # the lead for 15 s while it closed from 84 m to 27 m. + # - Anchor only (the first D-054 draft). 00000237 track 12, a new object at 25 m whose range walked + # out to 28.06 m while U11 said -2 m/s. The rate check rightly coasted the walk, the walk became + # the baseline, and the real ranges (25.3 m closing to 17 m) were rejected against it for 53 s + # while the accepted sample still tracked them. + # The recorded range resets this gate exists for contradict both baselines. + if track.newborn_run and (track.samples or track.range_anchor is not None): + track.newborn_run.clear() + track.newborn_vrel = None + newborn_vrel = None + if self.newborn_range_publish and track.newborn_run and not high_u10_live_vrel and track_id in self.pts: + newborn_vrel = track.newborn_vrel + if newborn_vrel is not None and not BOSCH_A_NEWBORN_SEED_HISTORY: + # The first trusted sweep starts the track exactly as it would have without newborn publishing. + track.newborn_run.clear() + track.newborn_vrel = None + elif newborn_vrel is not None: + # The published newborn run becomes the accepted history and its published vRel the velocity a coast may + # hold, so this sweep is range-gated against it and a coast here keeps the point instead of dropping it. + track.samples.extend(track.newborn_run[-BOSCH_A_VREL_MAX_SAMPLES:]) + track.range_anchor = track.newborn_run[-1] + track.last_trusted_vrel = newborn_vrel + track.last_trusted_vrel_nanos = now + track.newborn_run.clear() + track.newborn_vrel = None + previous_sample = track.samples[-1] if track.samples else None + range_anchor = track.range_anchor + ratio_vrel = None + range_rejected = False + rail_admitted = False + degraded = _bosch_a_measurement_degraded( + observation['range_sigma_raw'], observation['existence_raw'], direct_vrel_uncertainty_raw, + ) + if range_anchor is not None: + ratio = _bosch_a_range_ratio(range_ratio_raw) + # Deliberately still timed from the last ACCEPTED sample, as before D-054: changing a + # published vRel is a separate decision, recorded as open in STATUS.md. + if previous_sample is not None and now_s > previous_sample[0]: + ratio_vrel = _bosch_a_range_ratio_vrel(range_ratio_raw, dRel, now_s - previous_sample[0]) + baselines = [range_anchor] if previous_sample in (None, range_anchor) else [range_anchor, previous_sample] + # D-063: a railed U11 vouches for an interval of range, but only with the rail interval on and only + # for a track in our lane; otherwise the gate reads the rail as the exact value, as before. + exact_gate = not self.rail_interval or abs(yRel) > BOSCH_A_RAIL_INTERVAL_MAX_Y_M + if not exact_gate and degraded and BOSCH_A_RAIL_INTERVAL_EXACT_WHEN_DEGRADED: + # A DEGRADED sweep gets the interval only when the range corroborates the rail: a fit over the recent + # rejected run plus this sweep moves at a railed speed (BOSCH_A_RAIL_INTERVAL_EXACT_WHEN_DEGRADED). + run_rate = _bosch_a_fresh_range_rate(_bosch_a_trailing_fit_window(track.rejected_run + [(now_s, dRel)])) + exact_gate = (run_rate is None or direct_vrel is None or + _bosch_a_distance_to_interval(run_rate, _bosch_a_direct_vrel_interval(direct_vrel, counts_per_mps=self.u11_counts_per_mps)) > + BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS) + range_rejected = all(_bosch_a_range_innovation_rejected(baseline, now_s, dRel, direct_vrel, ratio, degraded, exact=exact_gate, + counts_per_mps=self.u11_counts_per_mps) + for baseline in baselines) + rail_admitted = not exact_gate and not range_rejected and all( + _bosch_a_range_innovation_rejected(baseline, now_s, dRel, direct_vrel, ratio, degraded, exact=True, + counts_per_mps=self.u11_counts_per_mps) + for baseline in baselines) + + if range_rejected: + track.inconsistent_run.clear() + # D-089: the window's own degraded flag, with range sigma judged against a range-scaled limit. + step_degraded = _bosch_a_measurement_degraded( + observation['range_sigma_raw'], observation['existence_raw'], direct_vrel_uncertainty_raw, + max(BOSCH_A_RANGE_SIGMA_DEGRADED_RAW, BOSCH_A_REANCHOR_RECOVER_SIGMA_FRAC * dRel)) + track.rejected_run.append((now_s, dRel, direct_vrel, step_degraded)) + track.strict_clean_count = 0 + if step_degraded != degraded: + track.rejected_strict_dirty_t = now_s + if _bosch_a_lasting_clean_step(track.rejected_run, exact=exact_gate, counts_per_mps=self.u11_counts_per_mps): + window_t0 = track.rejected_run[-BOSCH_A_REANCHOR_WINDOW][0] + track.recovered = track.rejected_strict_dirty_t >= window_t0 + rail_admitted = not exact_gate and not _bosch_a_lasting_clean_step(track.rejected_run, exact=True, + counts_per_mps=self.u11_counts_per_mps) + # D-057: the step has outlasted every returning excursion measured, cleanly and at the U11 + # rate. Re-root the accepted history and the anchor on it and gate this sweep as passed. + recent = track.rejected_run[-BOSCH_A_VREL_RATE_CHECK_MIN_SAMPLES:] + track.samples.clear() + track.samples.extend((t, d) for t, d, _, _ in recent[:-1]) + track.range_anchor = recent[-2][:2] + previous_sample = track.samples[-1] + range_anchor = track.range_anchor + range_rejected = False + track.rejected_run.clear() + else: + if track.rejected_run: + # D-059: a join. The range passed only after a rejection run, so `samples` straddle the gap. + track.rejoin_samples = [] + track.rail_hold = False + track.rejected_run.clear() + if rail_admitted: + if track.rejoin_samples is None: + track.rejoin_samples = [] + track.rail_hold = True + + if range_rejected: + # Keep the last trusted point briefly as an unmeasured coast. The rejected geometry is not + # published and never becomes the baseline for a later derivative or for this gate. + accepted_fresh = range_anchor is not None and now_s - range_anchor[0] <= BOSCH_A_STALE_S + point = self.pts.get(track_id) + if accepted_fresh and point is not None: + point.deprecated.measured = False + elif point is not None and self.u11_floor_unmeasured and track.floor_seen: + # BOSCH_A_U11_FLOOR_UNMEASURED: with the floor out of the accepted history, a floor-era identity can be left + # with no baseline that would ever accept its range again (route 00000008 track 17, 217.05-223.03 s: kept + # before only by a 3 s-old sample). The gate is unsure, so publish a bound, never delete (D-041/D-042): the + # reported geometry, unmeasured, at the rejected run's range fit, else the coast, else the floor's vRel. + run_vrel = _bosch_a_newborn_vrel([(t, d) for t, d, _, _ in track.rejected_run], self.v_ego) + if run_vrel is not None: + point.vRel = run_vrel + elif track.last_trusted_vrel is not None: + point.vRel = _bosch_a_coast_vrel(track, self.rail_interval, self.coast_range_bound, self.v_ego) + elif track.floor_vrel is not None: + point.vRel = track.floor_vrel + point.dRel = dRel + point.yRel = yRel + point.deprecated.measured = False + else: + self.pts.pop(track_id, None) + track.prev_frame_idx = idx0 + track.prev_life = life + track.last_seen_nanos = now + track.wire_slot = slot + for old_slot, old_id in enumerate(self._slot_track_ids): + if old_slot != slot and old_id == track_id: + self._slot_track_ids[old_slot] = None + self._slot_track_ids[slot] = track_id + continue + + # The range passed the gate: it is the gate's baseline from here on, whatever happens to vRel. + # Only a range that was actually GATED can advance it. With no anchor there was no gate, so a + # coast now (a high-u10 birth) would root the gate on an unchecked, unpublished range: measured + # on 00000239 that withheld 2,236 sweeps the pre-D-054 parser published, rejecting later sweeps + # 2-17 m off such a birth range. The first ACCEPTED sample below roots the anchor instead. + if range_anchor is not None: + track.range_anchor = (now_s, dRel) + + # BOSCH_A_U11_FLOOR_UNMEASURED: raw U11 0 is not a velocity. Publish the range fit unmeasured and never let + # the floor become a trusted vRel. Like every coast path, the floor sweep stays out of the accepted history + # (`samples`, `rejoin_samples`, the consistency run): it must not become a derivative baseline. Its ranges are + # kept in their own run, for the fit only. + if (self.u11_floor_unmeasured and direct_vrel is not None and direct_vrel_raw is not None and + int(direct_vrel_raw) == BOSCH_A_DIRECT_VREL_MIN_RAW): + track.floor_seen = True + track.floor_run.append((now_s, dRel)) + del track.floor_run[:-BOSCH_A_REANCHOR_WINDOW] + fit_history = sorted(list(track.samples) + track.floor_run)[-BOSCH_A_REANCHOR_WINDOW:] + fit_vrel = _bosch_a_newborn_vrel(fit_history, self.v_ego) + point = self.pts.get(track_id) + if fit_vrel is not None: + floor_vrel = fit_vrel + elif track.last_trusted_vrel is not None: + floor_vrel = _bosch_a_coast_vrel(track, self.rail_interval, self.coast_range_bound, self.v_ego) + elif point is not None: + floor_vrel = point.vRel + else: + floor_vrel = direct_vrel # no fit yet: the floor itself, as a bound (D-041), published unmeasured + # Same maturity as an accepted sweep (a second sample), so nothing published before is withheld. + if point is None and len(fit_history) >= 2: + point = self.pts[track_id] = structs.RadarData.RadarPoint() + point.trackId = track_id + point.deprecated.aRel = float('nan') + point.deprecated.yvRel = float('nan') + if point is not None: + point.dRel = dRel + point.yRel = yRel + point.vRel = floor_vrel + point.deprecated.measured = False + track.floor_vrel = floor_vrel + track.prev_frame_idx = idx0 + track.prev_life = life + track.last_seen_nanos = now + track.wire_slot = slot + for old_slot, old_id in enumerate(self._slot_track_ids): + if old_slot != slot and old_id == track_id: + self._slot_track_ids[old_slot] = None + self._slot_track_ids[slot] = track_id + continue + + # Multi-sweep consistency: does the range actually move the way this velocity claims? + # Fitted over the accepted range history, so it is immune to the single-sweep blindness above. + vrel_candidate = direct_vrel if direct_vrel is not None else ratio_vrel + vrel_inconsistent = False + if vrel_candidate is not None and len(track.samples) >= BOSCH_A_VREL_RATE_CHECK_MIN_SAMPLES - 1: + ts = [sample[0] for sample in track.samples] + [now_s] + ds = [sample[1] for sample in track.samples] + [dRel] + span = ts[-1] - ts[0] + if span >= BOSCH_A_VREL_RATE_CHECK_MIN_SPAN_S: + n = len(ts) + t_mean = sum(ts) / n + d_mean = sum(ds) / n + denom = sum((t - t_mean) ** 2 for t in ts) + if denom > 1e-9: + # strict=True: ts and ds are same-length by construction. A silent truncation here + # would bias the fitted rate and weaken the one-sided gate rather than erroring. + rate = sum((t - t_mean) * (d - d_mean) for t, d in zip(ts, ds, strict=True)) / denom + # One-sided: only U11 claiming MORE closing than the range supports is a fault. + vrel_inconsistent = vrel_candidate < rate - BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS + + # D-062: `samples` only grow on accepted sweeps, so during a coast the fit above is frozen. If the + # lead turns from opening to closing, U11 pulls away from that stale rate and the check latches: route + # 00000258 43:29-43:46, track 13 held vRel -0.45 for 12.7 s (fit frozen while it opened at +2.8 m/s) + # as its range fell 124 -> 31 m, then FCW. Appending coasted ranges to `samples` instead (F1) + # re-admits over-closing U11: replayed on 00000231, newly measured vRel over-closed the next-1 s + # range slope by >3 m/s on 17.0% of sweeps (reference 6.8%), the D-056 failure. So only a coast run + # that passes the D-057 lasting-clean-step test (>=1.5 s, non-degraded, ranges fit a line within + # 1 m and at the U11 rate) re-roots the fit. Replayed on 24 routes against the coast it replaces, + # re-rooted vRel was nearer the next-1 s range slope on 235 sweeps, the coast on 79 (lead 73/35), + # and total error halved; 00000241 and 0000024f leads went the other way, by over-closing (STATUS 70). + if vrel_inconsistent: + track.inconsistent_run.append((now_s, dRel, direct_vrel, degraded)) + if _bosch_a_lasting_clean_step(track.inconsistent_run, counts_per_mps=self.u11_counts_per_mps): + recent = track.inconsistent_run[-BOSCH_A_VREL_RATE_CHECK_MIN_SAMPLES:] + track.samples.clear() + track.samples.extend((t, d) for t, d, _, _ in recent[:-1]) + previous_sample = track.samples[-1] + track.inconsistent_run.clear() + vrel_inconsistent = False + else: + track.inconsistent_run.clear() + + # D-059: after a join, `samples` hold pre-gap ranges plus the joined one, so the fit above is set by + # the gap rather than by the object and cannot see a contradicting U11. Replayed on 00000232 / 236 / + # 237 / 239 / 23a / 23b / 23e: measured vRel in the second after a join over-closed the next-1 s + # range slope by >3 m/s on 21.0 % of sweeps (33.1 % where the rejected run's own slope contradicted + # U11) against 4.6 % for all measured points. Hold such a vRel as a coast until the same one-sided + # check also passes over the post-join ranges alone. + # - `samples` are left exactly as before: clearing them instead (fitting only the fresh ranges) + # admitted 106 new measured sweeps that over-closed on 16 % and lost 65 point-sweeps (the D-056 + # failure mode). With them untouched, this can only withdraw measured vRel, never add it. + # - The hold needs a trusted vRel to coast and re-publishes a point the rejection had dropped: a + # join must never delete an object the radar is reporting and the gate just accepted (D-041/D-042). + # Replay vs D-057: 0 lost, 0 new measured, 612 sweeps withdrawn (30 lead); the withdrawn vRel + # over-closed on 29.2 % (105 / 359 scored). + rejoin_hold = False + if track.rejoin_samples is not None: + track.rejoin_samples.append((now_s, dRel)) + del track.rejoin_samples[:-BOSCH_A_REANCHOR_WINDOW] + fresh_ok = False + if vrel_candidate is not None and len(track.rejoin_samples) >= BOSCH_A_VREL_RATE_CHECK_MIN_SAMPLES: + ts = [x[0] for x in track.rejoin_samples] + ds = [x[1] for x in track.rejoin_samples] + if ts[-1] - ts[0] >= BOSCH_A_VREL_RATE_CHECK_MIN_SPAN_S: + n = len(ts) + t_mean = sum(ts) / n + d_mean = sum(ds) / n + denom = sum((t - t_mean) ** 2 for t in ts) + if denom > 1e-9: + rate = sum((t - t_mean) * (d - d_mean) for t, d in zip(ts, ds, strict=True)) / denom + fresh_ok = vrel_candidate >= rate - BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS + if fresh_ok: + track.rejoin_samples = None + track.rail_hold = False + else: + rejoin_hold = track.last_trusted_vrel is not None and not (high_u10_live_vrel or vrel_inconsistent) + if rejoin_hold: + track.samples.append((now_s, dRel)) + if high_u10_live_vrel or vrel_inconsistent or rejoin_hold: + # The range cleared innovation checking above, so geometry here is trustworthy; only vRel is + # in question. Preserve current geometry but coast only a recent authoritative motion + # estimate without a KF update, rather than publishing a one-sweep-derivative synthesis. + # A coast means the VELOCITY is doubtful, not that the object is gone: object existence is + # decided earlier by STATUS/existence/range validity, which route elsewhere. Dropping the + # point here therefore discards a geometry the radar is still reporting and the range + # innovation check just accepted. Measured on 000001fb segments 34-35 through the real + # RadarInterface: 28 suppression gaps, 70% of them longer than BOSCH_A_STALE_S, i.e. long + # enough for radard to delete the track and fall back to vision. That is the 000001f9 + # failure mode -- there a stopped car was dropped and the planner commanded 0.00 at 76 m. + # + # Keep publishing the geometry and hold the last trusted velocity, flagged measured=False. + # An understated closing rate still brakes; a deleted object does not. Genuine disappearance + # is still handled by _bosch_a_retire_stale_tracks, which every coast path leaves armed by + # refreshing last_seen_nanos only while the radar keeps reporting this identity. + point = self.pts.get(track_id) + if point is None and rejoin_hold: + point = self.pts[track_id] = structs.RadarData.RadarPoint() + point.trackId = track_id + point.deprecated.aRel = float('nan') + point.deprecated.yvRel = float('nan') + if point is not None and track.last_trusted_vrel is not None: + point.dRel = dRel + point.yRel = yRel + point.vRel = _bosch_a_coast_vrel(track, self.rail_interval, self.coast_range_bound, self.v_ego) + point.deprecated.measured = False + elif (self.newborn_range_publish and high_u10_live_vrel and range_anchor is None and + track.last_trusted_vrel is None): + # A newborn high-u10 identity: keep its ranges outside the accepted history and publish the + # geometry once they fit a line (BOSCH_A_NEWBORN_RANGE_PUBLISH). + track.newborn_run.append((now_s, dRel)) + del track.newborn_run[:-BOSCH_A_REANCHOR_WINDOW] + newborn_vrel = track.newborn_vrel = _bosch_a_newborn_vrel(track.newborn_run, self.v_ego) + if newborn_vrel is None: + self.pts.pop(track_id, None) + else: + if point is None: + point = self.pts[track_id] = structs.RadarData.RadarPoint() + point.trackId = track_id + point.deprecated.aRel = float('nan') + point.deprecated.yvRel = float('nan') + point.dRel = dRel + point.yRel = yRel + point.vRel = newborn_vrel + point.deprecated.measured = False + elif point is not None and track.floor_vrel is not None: + # BOSCH_A_U11_FLOOR_UNMEASURED: a point published only by raw-0 sweeps has no trusted vRel, but it was + # published; keep it unmeasured at the floor's range-fit vRel rather than delete it (D-041/D-042). + point.dRel = dRel + point.yRel = yRel + point.vRel = track.floor_vrel + point.deprecated.measured = False + elif point is not None: + # No trusted velocity was ever established for this identity, so there is nothing to + # coast and no way to publish a defensible vRel. + self.pts.pop(track_id, None) + + # Do not let a coast-only range observation become a future derivative baseline. + track.prev_frame_idx = idx0 + track.prev_life = life + track.last_seen_nanos = now + track.wire_slot = slot + for old_slot, old_id in enumerate(self._slot_track_ids): + if old_slot != slot and old_id == track_id: + self._slot_track_ids[old_slot] = None + self._slot_track_ids[slot] = track_id + continue + + # Native U11 is unavailable here (sentinel/out-of-range -- the high-u10-but-live case above + # already returned before this point) and the range-ratio field is unusable or degraded too. + # The remaining option is the raw one-sweep (dRel-previous_range)/dt derivative, which is + # structurally the same hazard the high-u10 case guards against: a synthesized rate becoming an + # authoritative measurement. Coast a recent trusted velocity instead, on the same terms as above, + # rather than publish it. + # A true birth observation (no previous accepted range yet) can never mature into a published + # point this cycle regardless of vRel source -- `matured` below requires a second sample -- so + # only intercept once a fallback derivative would actually have something to poison. + u11_and_ratio_unavailable = (direct_vrel is None and (ratio_vrel is None or degraded) and + previous_sample is not None) + if u11_and_ratio_unavailable: + # A coast means the VELOCITY is doubtful, not that the object is gone: object existence is + # decided earlier by STATUS/existence/range validity, which route elsewhere. Dropping the + # point here therefore discards a geometry the radar is still reporting and the range + # innovation check just accepted. Measured on 000001fb segments 34-35 through the real + # RadarInterface: 28 suppression gaps, 70% of them longer than BOSCH_A_STALE_S, i.e. long + # enough for radard to delete the track and fall back to vision. That is the 000001f9 + # failure mode -- there a stopped car was dropped and the planner commanded 0.00 at 76 m. + # + # Keep publishing the geometry and hold the last trusted velocity, flagged measured=False. + # An understated closing rate still brakes; a deleted object does not. Genuine disappearance + # is still handled by _bosch_a_retire_stale_tracks, which every coast path leaves armed by + # refreshing last_seen_nanos only while the radar keeps reporting this identity. + point = self.pts.get(track_id) + if point is not None and track.last_trusted_vrel is not None: + point.dRel = dRel + point.yRel = yRel + point.vRel = _bosch_a_coast_vrel(track, self.rail_interval, self.coast_range_bound, self.v_ego) + point.deprecated.measured = False + elif point is not None and track.floor_vrel is not None: + # BOSCH_A_U11_FLOOR_UNMEASURED: a point published only by raw-0 sweeps has no trusted vRel, but it was + # published; keep it unmeasured at the floor's range-fit vRel rather than delete it (D-041/D-042). + point.dRel = dRel + point.yRel = yRel + point.vRel = track.floor_vrel + point.deprecated.measured = False + elif point is not None: + # No trusted velocity was ever established for this identity, so there is nothing to + # coast and no way to publish a defensible vRel. + self.pts.pop(track_id, None) + + # Do not let a coast-only range observation become a future derivative baseline. + track.prev_frame_idx = idx0 + track.prev_life = life + track.last_seen_nanos = now + track.wire_slot = slot + for old_slot, old_id in enumerate(self._slot_track_ids): + if old_slot != slot and old_id == track_id: + self._slot_track_ids[old_slot] = None + self._slot_track_ids[slot] = track_id + continue + + track.samples.append((now_s, dRel)) + track.range_anchor = (now_s, dRel) + sample_count = len(track.samples) + if track.recovered: + track.strict_clean_count = 0 if degraded else track.strict_clean_count + 1 + if track.strict_clean_count >= BOSCH_A_REANCHOR_WINDOW: + track.recovered = False + + # Prefer qualified native U11; otherwise the range-ratio field. The raw one-sweep derivative is + # never published as a measurement -- see the coast/drop branch above, which intercepts before + # this point whenever neither native U11 nor the ratio field is usable. + if direct_vrel is not None: + vRel = direct_vrel + else: + vRel = ratio_vrel + trustworthy_vrel = True + + # A birth observation has no range-rate yet. Keep it as history, but do not publish a RadarPoint + # until a second coherent observation of the same CAN identity supplies a finite derivative. + # A point the floor already published (BOSCH_A_U11_FLOOR_UNMEASURED) is not a birth: keep it on its first + # accepted sample rather than delete it (D-041/D-042). + matured = (sample_count >= 2 or (track.floor_vrel is not None and track_id in self.pts)) and vRel is not None and \ + math.isfinite(vRel) + if vRel is None and track.floor_vrel is not None and track_id in self.pts: + # A floor-published point's first accepted range with no U11 and no ratio: still no velocity measurement, + # so keep the floor bound unmeasured rather than delete the point (D-041/D-042). + trustworthy_vrel = False + point = self.pts[track_id] + point.dRel = dRel + point.yRel = yRel + point.vRel = track.floor_vrel + point.deprecated.measured = False + matured = None + if trustworthy_vrel: + track.last_trusted_vrel = vRel + track.last_trusted_vrel_nanos = now + track.floor_vrel = None + track.floor_run.clear() + if matured and track_id not in self.pts: + self.pts[track_id] = structs.RadarData.RadarPoint() + self.pts[track_id].trackId = track_id + self.pts[track_id].deprecated.aRel = float('nan') + self.pts[track_id].deprecated.yvRel = float('nan') + + if matured: + self.pts[track_id].dRel = dRel + self.pts[track_id].yRel = yRel + self.pts[track_id].vRel = vRel + self.pts[track_id].deprecated.measured = True + elif matured is not None: + self.pts.pop(track_id, None) + + track.prev_frame_idx = idx0 + track.prev_life = life + track.last_seen_nanos = now + track.wire_slot = slot + for old_slot, old_id in enumerate(self._slot_track_ids): + if old_slot != slot and old_id == track_id: + self._slot_track_ids[old_slot] = None + self._slot_track_ids[slot] = track_id + + self.point_statuses = {track_id: (self._tracks[track_id].status if track_id in self._tracks else 0) + for track_id in sorted(self.pts)} + ret.points = [self.pts[track_id] for track_id in sorted(self.pts)] + return ret diff --git a/opendbc/car/honda/radar_interface.py b/opendbc/car/honda/radar_interface.py index b5bc2cbc888..93cbd5cc7e8 100755 --- a/opendbc/car/honda/radar_interface.py +++ b/opendbc/car/honda/radar_interface.py @@ -1,11 +1,7 @@ #!/usr/bin/env python3 -import math -from collections import deque -from dataclasses import dataclass, field - from opendbc.can import CANParser from opendbc.car import Bus, structs -from opendbc.car.honda.hondacan import CanBus +from opendbc.car.honda.bosch_a_radar import BOSCH_A_DBC_NAME, BoschARadar from opendbc.car.honda.values import DBC from opendbc.car.interfaces import RadarInterfaceBase @@ -16,211 +12,6 @@ def _create_nidec_can_parser(car_fingerprint): return CANParser(DBC[car_fingerprint][Bus.radar], messages, 1) -# ============================================================================ -# Honda Bosch-A 16-slot object bank -# ============================================================================ -# 16 CAN-visible object slots. Each slot has 4 main frames (f0..f3, one CAN ID each -- NOT sub-frames -# muxed onto a shared ID) plus one synchronized auxiliary 5th frame. This supersedes any prior model of -# 0x280/0x284/0x288/0x28C as pieces of one object, or of 0x2C8/0x2C9 as a separate "coarse" list: it is -# ONE 16-slot bank with one auxiliary frame per slot. See honda_bosch_a_radar.dbc for the bit geometry. -BOSCH_A_DBC_NAME = 'honda_bosch_a_radar' -BOSCH_A_NUM_SLOTS = 16 - - -def _bosch_a_main_base(slot: int) -> int: - return 0x280 + 4 * slot if slot < 4 else 0x2D0 + 4 * (slot - 4) - - -def _bosch_a_aux_id(slot: int) -> int: - return 0x2C8 + slot if slot < 8 else 0x290 + (slot - 8) - - -BOSCH_A_MAIN_IDS = [[_bosch_a_main_base(s) + i for i in range(4)] for s in range(BOSCH_A_NUM_SLOTS)] -BOSCH_A_AUX_IDS = [_bosch_a_aux_id(s) for s in range(BOSCH_A_NUM_SLOTS)] -BOSCH_A_ALL_IDS = [addr for ids in BOSCH_A_MAIN_IDS for addr in ids] + BOSCH_A_AUX_IDS - -# Publish RadarPoints when the last MAIN object frame arrives. Passive captures prove that slot 15's -# companion frame, 0x297, follows 0x2FF and is the final observed object-family frame in a full sweep: -# -# ... 0x2FC, 0x2FD, 0x2FE, 0x2FF, 0x297 -# -# Keep 0x2FF as the RadarPoint trigger while the companion data remains optional/debug-only. Making -# 0x297 the sole trigger would allow one dropped auxiliary frame to suppress an otherwise-valid point -# update, contrary to the parser's "aux never gates validity" contract. -BOSCH_A_TRIGGER_MSG = BOSCH_A_MAIN_IDS[BOSCH_A_NUM_SLOTS - 1][3] -BOSCH_A_SWEEP_END_MSG = BOSCH_A_AUX_IDS[BOSCH_A_NUM_SLOTS - 1] # 0x297 - -# Observed coherent Bosch-A sweep cadence is ~14.5-16 Hz in the available captures. -BOSCH_A_FREQ_HZ = 15 - -# Range: f0 raw_range (12-bit, B2:B3 high nibble) -> meters. Firmware q16 = 8*raw_range. -# -# The scale is firmware-exact, not fitted. AC004 converts the internal value with -# (q16 - n) / 128, and q16 = sat16(round(8 * raw_range)), so -# -# range_m = (8 * raw_range - n) / 128 = raw_range / 16 - n / 128 -# -# Corroboration that 1/16 is the designed mapping rather than a coincidence: 8 * 4095 = 32760 -# fits int16 with 7 counts to spare, so the *8 exists to make the 12-bit field fill the internal -# word; full scale is 4095/16 = 255.9 m; and /128 (Q7) is this firmware's unit for physical -# quantities throughout. The previous 0.05712 was 16 * 0.00357, and that 0.00357 was solved from -# a single tape point with the offset assumed, so it read progressively short with distance -# (~9% low, -9 m at 60 m against vision). -BOSCH_A_RANGE_SCALE_M = 1.0 / 16.0 - -# Offset. The firmware term is -n/128, where n is assembled from a configuration word plus a -# runtime addend and is therefore a PER-UNIT CALIBRATION VALUE, not a constant; 335 (-2.617 m) is -# only the fallback the firmware uses when the config word reads zero. -3.0 is retained because it -# sits inside the plausible calibration range and the choice barely moves the residual. Do not -# re-fit this against vision: read it from the radar's own configuration instead. -BOSCH_A_RANGE_OFFSET_M = -3.0 - -# Azimuth: f0 raw_angle (11-bit, B4:B5 high 3 bits), offset-binary about 1024. -# -# The f3 angular-edge pair independently closes the exact center-angle scale: -# azimuth_rad = (raw_angle - 1024) / 2048 -# -# This supersedes the older empirical 0.032 deg/count fit. -BOSCH_A_AZIMUTH_SCALE_RAD = 1.0 / 2048.0 -BOSCH_A_AZIMUTH_CENTER = 1024 - -# Invalid sentinels (section 3/4/5/6 of the spec). -BOSCH_A_STATUS_INVALID = 0xF -BOSCH_A_RANGE_RAW_INVALID = 0xFFF -BOSCH_A_ANGLE_RAW_INVALID = 0x7FF -BOSCH_A_LIFE_INVALID = 0xFFF -BOSCH_A_TRACK_ID_MIN = 1 -BOSCH_A_TRACK_ID_MAX = 0x3F -# AUX logical 0x00CA has an explicit 0x3FF invalid sentinel. Firmware proves the normalization below; -# captures strongly support interpreting the active value as previous/current range ratio. -BOSCH_A_RANGE_RATIO_INVALID = 0x3FF -BOSCH_A_LOGICAL_00CA_INVALID = BOSCH_A_RANGE_RATIO_INVALID # compatibility/debug alias -BOSCH_A_RANGE_RATIO_SCALE = 0.001 -BOSCH_A_RANGE_RATIO_OFFSET = 0.5 - -# The synchronized AUX frame contains an 11-bit offset-binary velocity-like field and a 10-bit -# companion quality/uncertainty field. The byte locations are now decoded in the DBC. The exact -# Bosch descriptor name is still being verified, so keep the conversion constants isolated here. -# Capture validation shows active values in [0, 1728], with 0x7FE used as the inactive sentinel. -BOSCH_A_DIRECT_VREL_INVALID = 0x7FE -BOSCH_A_DIRECT_VREL_MIN_RAW = 0 -BOSCH_A_DIRECT_VREL_MAX_RAW = 1728 -BOSCH_A_DIRECT_VREL_CENTER_RAW = 864 -BOSCH_A_DIRECT_VREL_SCALE_MPS = 1.0 / 64.0 -# Empirical raw-quality policy. U11 error against an independent range-derivative reference rises -# with u10 in a graded way, not a cliff: replay evidence bins it roughly as 0-255 clean, 256-511 only -# mildly degraded, 512-767 clearly degraded, 768+ worst. 511 is set at that mild/clear boundary so -# U11 is still trusted directly through the mildly-degraded band; only u10 above this triggers the -# high-u10 coast path below. This is deliberately not presented as a Bosch physical unit or -# descriptor constant; it keeps saturated/high-uncertainty AUX values from becoming authoritative -# velocity measurements. -BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW = 511 - -# Measurement-authority policy. These are replay-derived safety/tuning gates, not recovered Bosch -# constants. Range innovation is measured from the previous accepted observation so a reset cannot -# become the baseline for following sweeps. -BOSCH_A_RANGE_SIGMA_DEGRADED_RAW = 4 -BOSCH_A_RANGE_INNOVATION_MAX_M = 2.0 -BOSCH_A_RANGE_INNOVATION_HARD_MAX_M = 5.0 -BOSCH_A_FALLBACK_RANGE_RATE_MAX_MPS = 50.0 -BOSCH_A_USE_TAN_LATERAL_PROJECTION = True - -# Accepted-range history is retained for continuity and the adjacent two-point fallback. Rejected -# range resets never enter it, and it is not used for a multi-sample OLS velocity fit. -BOSCH_A_VREL_MAX_SAMPLES = 8 - -# Staleness gate -- TUNING constant, reused plumbing pattern (not a firmware fact). At the observed -# ~15 Hz cadence, 0.20 s is approximately three missed sweeps. -BOSCH_A_STALE_S = 0.20 - - -@dataclass -class _BoschASlotState: - last_seen_nanos: int | None = None - - # Firmware logical-ID companion data -- telemetry/debug only for now, never a RadarPoint validity - # gate. 0x00C9 has a firmware transform but no proven physical meaning; 0x00CA has an explicit - # invalid sentinel and its physical meaning remains unresolved. - logical_00c9_raw: float = float('nan') - logical_00ca_raw: float = float('nan') - direct_vrel_raw: int | None = None - direct_vrel_uncertainty_raw: int | None = None - - def reset(self): - self.last_seen_nanos = None - self.logical_00c9_raw = float('nan') - self.logical_00ca_raw = float('nan') - self.direct_vrel_raw = None - self.direct_vrel_uncertainty_raw = None - - -@dataclass -class _BoschATrackState: - """Persistent state for one Bosch CAN object identity, independent of wire slot.""" - track_id: int - prev_frame_idx: int | None = None - prev_life: int | None = None - last_seen_nanos: int | None = None - wire_slot: int | None = None - samples: deque = field(default_factory=lambda: deque(maxlen=BOSCH_A_VREL_MAX_SAMPLES)) - last_trusted_vrel: float | None = None - last_trusted_vrel_nanos: int | None = None - - -def _bosch_a_direct_vrel(raw_value: int | float | None, - uncertainty_raw: int | float | None = None) -> float | None: - """Decode the capture-validated AUX relative-velocity candidate. - - None means that AUX was absent/invalid and the caller should use the existing fallback policy. The [0, 1728] - active domain is deliberately enforced here because values above the observed +13.5 m/s rail have - not appeared on active objects; 0x7FE is the observed inactive sentinel. - """ - if raw_value is None: - return None - raw = int(raw_value) - if raw == BOSCH_A_DIRECT_VREL_INVALID: - return None - if not BOSCH_A_DIRECT_VREL_MIN_RAW <= raw <= BOSCH_A_DIRECT_VREL_MAX_RAW: - return None - # u10 is retained as a raw quality indicator because its physical units remain unresolved. The - # conservative threshold is evidence-backed tuning, not a recovered firmware validity rule. - if uncertainty_raw is not None and int(uncertainty_raw) > BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW: - return None - return (raw - BOSCH_A_DIRECT_VREL_CENTER_RAW) * BOSCH_A_DIRECT_VREL_SCALE_MPS - - -def _bosch_a_range_ratio(raw_value: int | float | None) -> float | None: - if raw_value is None: - return None - raw = int(raw_value) - if raw == BOSCH_A_RANGE_RATIO_INVALID or not 0 <= raw < BOSCH_A_RANGE_RATIO_INVALID: - return None - return BOSCH_A_RANGE_RATIO_OFFSET + BOSCH_A_RANGE_RATIO_SCALE * raw - - -def _bosch_a_range_ratio_vrel(raw_value: int | float | None, d_rel: float, dt: float) -> float | None: - """Return the range rate implied by the empirical previous/current range ratio.""" - ratio = _bosch_a_range_ratio(raw_value) - if ratio is None or dt <= 0.0 or not math.isfinite(d_rel): - return None - return d_rel * (1.0 - ratio) / dt - - -def _bosch_a_measurement_degraded(range_sigma_raw: int, existence_raw: int, - direct_vrel_uncertainty_raw: int | None) -> bool: - range_quality_bad = range_sigma_raw >= BOSCH_A_RANGE_SIGMA_DEGRADED_RAW or existence_raw in (0, 0x7F) - velocity_quality_bad = (direct_vrel_uncertainty_raw is not None and - direct_vrel_uncertainty_raw > BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW) - return range_quality_bad or velocity_quality_bad - - -def _create_bosch_a_can_parser(CP): - messages = [(addr, BOSCH_A_FREQ_HZ) for addr in BOSCH_A_ALL_IDS] - # Bus.radar selects the Bosch-A DBC; the object/fusion feed itself is - # physically on the camera-side ACC-CAN. - return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, CanBus(CP).camera) - - class RadarInterface(RadarInterfaceBase): def __init__(self, CP, CP_SP): super().__init__(CP, CP_SP) @@ -228,18 +19,19 @@ def __init__(self, CP, CP_SP): self.radar_wrong_config = False self.radar_off_can = CP.radarUnavailable self.bosch_a_radar = (not self.radar_off_can and Bus.radar in DBC[CP.carFingerprint] and - DBC[CP.carFingerprint][Bus.radar] == BOSCH_A_DBC_NAME) + DBC[CP.carFingerprint][Bus.radar] == BOSCH_A_DBC_NAME) + # The Bosch-A object-bank decoder lives in bosch_a_radar.BoschARadar; this class only selects and delegates. + self.bosch_a = None + # Ego speed, set by card before each update; the Bosch-A decoder's reversing-coast bound reads it. + self.v_ego: float | None = None if self.radar_off_can: self.rcp = None self.trigger_msg = 0x445 elif self.bosch_a_radar: - self.rcp = _create_bosch_a_can_parser(CP) - self.trigger_msg = BOSCH_A_TRIGGER_MSG - self._slots = [_BoschASlotState() for _ in range(BOSCH_A_NUM_SLOTS)] - self._tracks: dict[int, _BoschATrackState] = {} - self._slot_track_ids: list[int | None] = [None] * BOSCH_A_NUM_SLOTS - self._last_trigger_nanos = -1 + self.bosch_a = BoschARadar(CP, CP_SP) + self.rcp = self.bosch_a.rcp + self.trigger_msg = self.bosch_a.trigger_msg else: # Nidec self.rcp = _create_nidec_can_parser(CP.carFingerprint) @@ -248,7 +40,16 @@ def __init__(self, CP, CP_SP): self.updated_messages = set() + @property + def point_statuses(self) -> dict[int, int] | None: + """Bosch-A F0 STATUS per point of the last RadarData returned; None for other radars.""" + return self.bosch_a.point_statuses if self.bosch_a is not None else None + def update(self, can_strings): + if self.bosch_a is not None: + self.bosch_a.v_ego = self.v_ego + return self.bosch_a.update(can_strings) + if self.radar_off_can or self.rcp is None: return super().update(None) @@ -256,390 +57,18 @@ def update(self, can_strings): self.updated_messages.update(vls) if self.trigger_msg not in self.updated_messages: - if self.bosch_a_radar and self._last_trigger_nanos >= 0: - now = self.rcp._last_update_nanos - if (now - self._last_trigger_nanos) * 1e-9 > BOSCH_A_STALE_S: - return self._bosch_a_stale_radardata() return None rr = self._update(self.updated_messages) self.updated_messages.clear() return rr - def _bosch_a_stale_radardata(self): - # Whole-bus silence: clear every live point/history and emit an EMPTY RadarData (not None) so - # radard drops any lead within a cycle instead of freezing a phantom. The next observation starts - # a fresh incarnation for its CAN identity. - self.pts.clear() - self._tracks.clear() - self._slot_track_ids = [None] * BOSCH_A_NUM_SLOTS - for slot_state in self._slots: - slot_state.reset() - self._last_trigger_nanos = -1 - stale = structs.RadarData() - if not self.rcp.can_valid: - stale.errors.canError = True - stale.errors.radarUnavailableTemporary = True - return stale - - def _bosch_a_retire_track(self, track_id: int): - self._tracks.pop(track_id, None) - self.pts.pop(track_id, None) - for slot, slot_track_id in enumerate(self._slot_track_ids): - if slot_track_id == track_id: - self._slot_track_ids[slot] = None - - def _bosch_a_retire_stale_tracks(self, now: int): - for track_id, track in list(self._tracks.items()): - if track.last_seen_nanos is not None and (now - track.last_seen_nanos) * 1e-9 > BOSCH_A_STALE_S: - self._bosch_a_retire_track(track_id) - def _update(self, updated_messages): - if self.bosch_a_radar: - return self._update_bosch_a(updated_messages) + if self.bosch_a is not None: + self.bosch_a.v_ego = self.v_ego + return self.bosch_a._update_bosch_a(updated_messages) return self._update_nidec(updated_messages) - def _update_bosch_a(self, updated_messages): - ret = structs.RadarData() - if not self.rcp.can_valid: - ret.errors.canError = True - - now = self.rcp._last_update_nanos - self._last_trigger_nanos = now - self._bosch_a_retire_stale_tracks(now) - - observations = [] - - for slot in range(BOSCH_A_NUM_SLOTS): - f0, f1, f2, f3 = BOSCH_A_MAIN_IDS[slot] - aux = BOSCH_A_AUX_IDS[slot] - st = self._slots[slot] - - if not (f0 in updated_messages and f1 in updated_messages and - f2 in updated_messages and f3 in updated_messages): - # Incomplete main-frame set: a missing CAN frame must not be treated as a lifecycle mismatch or - # death/replacement. Logical tracks are retired independently by their global staleness gate. - continue - - v0 = self.rcp.vl[f0] - v1 = self.rcp.vl[f1] - v2 = self.rcp.vl[f2] - v3 = self.rcp.vl[f3] - - idx0 = int(v0['FRAME_IDX']) - if not (idx0 == int(v1['FRAME_IDX']) == int(v2['FRAME_IDX']) == int(v3['FRAME_IDX'])): - # The four main frames don't share a common cycle index -- not a coherent observation this - # window. Only combine main-frame data from a coherent common frame index (section 2). - continue - - status = int(v0['STATUS']) - range_raw = int(v0['RANGE_RAW']) - angle_raw = int(v0['AZIMUTH_RAW']) - range_sigma_raw = int(v0['RANGE_SIGMA_RAW']) - existence_raw = int(v1['OBJECT_EXISTENCE_PROBABILITY_RAW']) - life = int(v2['LIFECYCLE_RAW']) - track_id = int(v3['TRACK_ID']) - track_id_valid = BOSCH_A_TRACK_ID_MIN <= track_id <= BOSCH_A_TRACK_ID_MAX - st.last_seen_nanos = now - direct_vrel_raw = None - direct_vrel_uncertainty_raw = None - range_ratio_raw = None - - # Attach synchronized companion data only when its cycle matches. Missing AUX never invalidates - # an otherwise coherent F0-F3 object; it only removes independent motion/quality evidence. - if aux in updated_messages: - av = self.rcp.vl[aux] - if int(av['FRAME_IDX']) == idx0: - direct_vrel_raw = int(av['REL_VELOCITY_RAW']) - direct_vrel_uncertainty_raw = int(av['REL_VELOCITY_UNCERTAINTY_RAW']) - range_ratio_raw = int(av['RANGE_RATIO_RAW']) - logical_00c9_raw = av['FW_LID_00C9_RAW'] - logical_00ca_raw = av['FW_LID_00CA_RAW'] - st.logical_00c9_raw = logical_00c9_raw - st.logical_00ca_raw = ( - logical_00ca_raw if logical_00ca_raw != BOSCH_A_LOGICAL_00CA_INVALID else float('nan') - ) - st.direct_vrel_raw = direct_vrel_raw - st.direct_vrel_uncertainty_raw = direct_vrel_uncertainty_raw - - object_valid = (status != BOSCH_A_STATUS_INVALID and range_raw != BOSCH_A_RANGE_RAW_INVALID and - angle_raw != BOSCH_A_ANGLE_RAW_INVALID and life != BOSCH_A_LIFE_INVALID) - - observations.append({ - 'slot': slot, - 'frame_idx': idx0, - 'life': life, - 'track_id': track_id, - 'track_id_valid': track_id_valid, - 'object_valid': object_valid, - 'range_sigma_raw': range_sigma_raw, - 'existence_raw': existence_raw, - 'direct_vrel_raw': direct_vrel_raw, - 'direct_vrel_uncertainty_raw': direct_vrel_uncertainty_raw, - 'range_ratio_raw': range_ratio_raw, - }) - - # First collapse duplicate wire observations of one CAN identity. The dictionary is also the - # output uniqueness boundary: one valid Bosch identity can never create two RadarPoints. - valid_by_id = {} - for observation in observations: - if not (observation['object_valid'] and observation['track_id_valid']): - # An invalid observation ends publication for the object currently occupying this wire slot, - # but it does not immediately destroy the persistent state. The object may be multiplexed to - # another slot or may return before the per-identity stale deadline; a later lifecycle break - # or staleness expiry will clear its derivative history. - ids_to_hide = {self._slot_track_ids[observation['slot']]} - if observation['track_id_valid']: - ids_to_hide.add(observation['track_id']) - for invalid_id in ids_to_hide - {None}: - self.pts.pop(invalid_id, None) - continue - track_id = observation['track_id'] - current = valid_by_id.get(track_id) - if current is None: - valid_by_id[track_id] = observation - continue - - # Prefer the wire slot currently associated with the persistent state. If neither candidate is - # preferred, retain the lower slot for deterministic handling of a malformed duplicate frame. - track = self._tracks.get(track_id) - current_score = (0 if track is not None and track.wire_slot == current['slot'] else 1, current['slot']) - candidate_score = (0 if track is not None and track.wire_slot == observation['slot'] else 1, - observation['slot']) - if candidate_score < current_score: - valid_by_id[track_id] = observation - - valid_ids = set(valid_by_id) - for track_id, observation in sorted(valid_by_id.items(), key=lambda item: item[1]['slot']): - slot = observation['slot'] - idx0 = observation['frame_idx'] - life = observation['life'] - - # A wire-slot replacement ends publication for the old occupant, but not its persistent - # identity/history. If the old ID is still valid in another slot this sweep, keep its point; - # otherwise hide it until that ID returns or reaches its own stale deadline. - old_id = self._slot_track_ids[slot] - if old_id is not None and old_id != track_id and old_id not in valid_ids: - self.pts.pop(old_id, None) - - track = self._tracks.get(track_id) - if track is None: - track = _BoschATrackState(track_id=track_id) - self._tracks[track_id] = track - - same_incarnation = False - if track.prev_frame_idx is not None and track.prev_life is not None: - frame_delta = (idx0 - track.prev_frame_idx) & 0xF - life_delta = (life - track.prev_life) & 0xFFF - same_incarnation = life_delta == 2 * frame_delta - - if not same_incarnation: - # The CAN identity remains the externally-visible key, but a lifecycle discontinuity starts a - # new incarnation and must not inherit the previous object's range-rate history. - track.samples.clear() - track.last_trusted_vrel = None - track.last_trusted_vrel_nanos = None - self.pts.pop(track_id, None) - - v0 = self.rcp.vl[BOSCH_A_MAIN_IDS[slot][0]] - range_raw = int(v0['RANGE_RAW']) - angle_raw = int(v0['AZIMUTH_RAW']) - dRel = BOSCH_A_RANGE_SCALE_M * range_raw + BOSCH_A_RANGE_OFFSET_M - azimuth_rad = BOSCH_A_AZIMUTH_SCALE_RAD * (angle_raw - BOSCH_A_AZIMUTH_CENTER) - # The firmware's internal geometry path uses tan(angle) for a forward-axis distance. The - # Bosch object range is consumed as that forward-axis quantity here, so use the same projection - # rather than treating it as radial/slant range. Keep the switch explicit while the final - # output bridge remains under static review. - # - # Sign: AZIMUTH_RAW > center (positive azimuth_rad) is a LEFT-of-center detection in car frame's - # y axis (left is positive), matching the Nidec RadarPoint.yRel contract. Confirmed against real - # captures 2026-08-22: a stationary cluster of queued vehicles visible on the left in the road - # camera was rendering on the right in both the Qt and raylib radar-track overlays with the - # previously-flipped sign. - lateral_projection = math.tan if BOSCH_A_USE_TAN_LATERAL_PROJECTION else math.sin - yRel = dRel * lateral_projection(azimuth_rad) - - now_s = now * 1e-9 - direct_vrel_raw = observation['direct_vrel_raw'] - direct_vrel_uncertainty_raw = observation['direct_vrel_uncertainty_raw'] - direct_vrel = _bosch_a_direct_vrel(direct_vrel_raw, direct_vrel_uncertainty_raw) - live_direct_vrel = _bosch_a_direct_vrel(direct_vrel_raw) - range_ratio_raw = observation['range_ratio_raw'] - - # A live U11 rejected solely by the conservative U10 qualification threshold is neither an - # unavailable velocity nor permission to synthesize a one-sweep range derivative. It is also - # NOT permission to skip range-innovation checking below: u10 correlates with range_sigma in - # replay data, so a high-u10 sweep is exactly the condition where a bad/discontinuous range - # (slot migration, reset) is most likely, not less likely. Range acceptance is therefore - # decided on the same terms as every other sweep first; only once the range clears that gate - # does a high-u10 U11 fall back to coasting instead of publishing a synthesized derivative. - high_u10_live_vrel = (direct_vrel is None and live_direct_vrel is not None and - direct_vrel_uncertainty_raw is not None and - direct_vrel_uncertainty_raw > BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW) - - # Validate against the previous accepted range. Qualified U11 and the range-ratio field are - # independent corroboration paths; high-U10 U11 is deliberately excluded from this decision. - previous_sample = track.samples[-1] if track.samples else None - fallback_vrel = 0.0 - ratio_vrel = None - range_rejected = False - degraded = _bosch_a_measurement_degraded( - observation['range_sigma_raw'], observation['existence_raw'], direct_vrel_uncertainty_raw, - ) - if previous_sample is not None: - previous_time, previous_range = previous_sample - dt = now_s - previous_time - if dt <= 0.0: - range_rejected = True - else: - fallback_vrel = (dRel - previous_range) / dt - ratio = _bosch_a_range_ratio(range_ratio_raw) - ratio_vrel = _bosch_a_range_ratio_vrel(range_ratio_raw, dRel, dt) - - residuals_m = [] - if direct_vrel is not None: - residuals_m.append(abs(dRel - (previous_range + direct_vrel * dt))) - if ratio is not None: - residuals_m.append(abs(previous_range - dRel * ratio)) - - if residuals_m: - innovation_m = min(residuals_m) - range_rejected = ( - innovation_m > BOSCH_A_RANGE_INNOVATION_HARD_MAX_M or - (degraded and innovation_m > BOSCH_A_RANGE_INNOVATION_MAX_M) - ) - else: - range_rejected = abs(fallback_vrel) > BOSCH_A_FALLBACK_RANGE_RATE_MAX_MPS - - if range_rejected: - # Keep the last accepted point briefly as an unmeasured coast. The rejected geometry is not - # published and never becomes the baseline for a later derivative. - accepted_fresh = previous_sample is not None and now_s - previous_sample[0] <= BOSCH_A_STALE_S - point = self.pts.get(track_id) - if accepted_fresh and point is not None: - point.deprecated.measured = False - else: - self.pts.pop(track_id, None) - track.prev_frame_idx = idx0 - track.prev_life = life - track.last_seen_nanos = now - track.wire_slot = slot - for old_slot, old_id in enumerate(self._slot_track_ids): - if old_slot != slot and old_id == track_id: - self._slot_track_ids[old_slot] = None - self._slot_track_ids[slot] = track_id - continue - - if high_u10_live_vrel: - # The range cleared innovation checking above, so geometry here is trustworthy; only vRel is - # in question. Preserve current geometry but coast only a recent authoritative motion - # estimate without a KF update, rather than publishing a one-sweep-derivative synthesis. - trusted_fresh = (track.last_trusted_vrel is not None and track.last_trusted_vrel_nanos is not None and - (now - track.last_trusted_vrel_nanos) * 1e-9 <= BOSCH_A_STALE_S) - if trusted_fresh: - point = self.pts.get(track_id) - if point is not None: - point.dRel = dRel - point.yRel = yRel - point.vRel = track.last_trusted_vrel - point.deprecated.measured = False - else: - track.last_trusted_vrel = None - track.last_trusted_vrel_nanos = None - self.pts.pop(track_id, None) - - # Do not let a coast-only range observation become a future derivative baseline. - track.prev_frame_idx = idx0 - track.prev_life = life - track.last_seen_nanos = now - track.wire_slot = slot - for old_slot, old_id in enumerate(self._slot_track_ids): - if old_slot != slot and old_id == track_id: - self._slot_track_ids[old_slot] = None - self._slot_track_ids[slot] = track_id - continue - - # Native U11 is unavailable here (sentinel/out-of-range -- the high-u10-but-live case above - # already returned before this point) and the range-ratio field is unusable or degraded too. - # The remaining option is the raw one-sweep (dRel-previous_range)/dt derivative, which is - # structurally the same hazard the high-u10 case guards against: a synthesized rate becoming an - # authoritative measurement. Coast a recent trusted velocity instead, on the same terms as above, - # rather than publish it. - # A true birth observation (no previous accepted range yet) can never mature into a published - # point this cycle regardless of vRel source -- `matured` below requires a second sample -- so - # only intercept once a fallback derivative would actually have something to poison. - u11_and_ratio_unavailable = (direct_vrel is None and (ratio_vrel is None or degraded) and - previous_sample is not None) - if u11_and_ratio_unavailable: - trusted_fresh = (track.last_trusted_vrel is not None and track.last_trusted_vrel_nanos is not None and - (now - track.last_trusted_vrel_nanos) * 1e-9 <= BOSCH_A_STALE_S) - if trusted_fresh: - point = self.pts.get(track_id) - if point is not None: - point.dRel = dRel - point.yRel = yRel - point.vRel = track.last_trusted_vrel - point.deprecated.measured = False - else: - track.last_trusted_vrel = None - track.last_trusted_vrel_nanos = None - self.pts.pop(track_id, None) - - # Do not let a coast-only range observation become a future derivative baseline. - track.prev_frame_idx = idx0 - track.prev_life = life - track.last_seen_nanos = now - track.wire_slot = slot - for old_slot, old_id in enumerate(self._slot_track_ids): - if old_slot != slot and old_id == track_id: - self._slot_track_ids[old_slot] = None - self._slot_track_ids[slot] = track_id - continue - - track.samples.append((now_s, dRel)) - sample_count = len(track.samples) - - # Prefer qualified native U11; otherwise the range-ratio field. The raw one-sweep derivative is - # never published as a measurement -- see the coast/drop branch above, which intercepts before - # this point whenever neither native U11 nor the ratio field is usable. - if direct_vrel is not None: - vRel = direct_vrel - else: - vRel = ratio_vrel - trustworthy_vrel = True - - # A birth observation has no range-rate yet. Keep it as history, but do not publish a RadarPoint - # until a second coherent observation of the same CAN identity supplies a finite derivative. - matured = sample_count >= 2 and math.isfinite(vRel) - if trustworthy_vrel: - track.last_trusted_vrel = vRel - track.last_trusted_vrel_nanos = now - if matured and track_id not in self.pts: - self.pts[track_id] = structs.RadarData.RadarPoint() - self.pts[track_id].trackId = track_id - self.pts[track_id].deprecated.aRel = float('nan') - self.pts[track_id].deprecated.yvRel = float('nan') - - if matured: - self.pts[track_id].dRel = dRel - self.pts[track_id].yRel = yRel - self.pts[track_id].vRel = vRel - self.pts[track_id].deprecated.measured = True - else: - self.pts.pop(track_id, None) - - track.prev_frame_idx = idx0 - track.prev_life = life - track.last_seen_nanos = now - track.wire_slot = slot - for old_slot, old_id in enumerate(self._slot_track_ids): - if old_slot != slot and old_id == track_id: - self._slot_track_ids[old_slot] = None - self._slot_track_ids[slot] = track_id - - ret.points = [self.pts[track_id] for track_id in sorted(self.pts)] - return ret - def _update_nidec(self, updated_messages): ret = structs.RadarData() diff --git a/opendbc/car/honda/tests/test_bosch_a_decoder.py b/opendbc/car/honda/tests/test_bosch_a_decoder.py new file mode 100644 index 00000000000..cf893f0a539 --- /dev/null +++ b/opendbc/car/honda/tests/test_bosch_a_decoder.py @@ -0,0 +1,2329 @@ +"""Bosch-A decoder tests, ported from Honda-PTM-Testing (tests/test_bosch_a_decoder.py) with the decoder. + +Adapted to nrdr: the decoder is opendbc.car.honda.bosch_a_radar.BoschARadar (RadarInterface(CP, CP_SP).bosch_a), point +flags are in RadarPoint.deprecated, and there is no BoschARadar params toggle. Tests of what the port leaves out (the +NORMALIZED_CLOSING outputs, RadarPoint.existence/recovered, the DBC CHECKSUM/COUNTER and the wider Bosch-A car gate) +are removed; the newborn-publish tests switch it on explicitly, since it is off on 7.0. +""" +import math +import random + +import pytest + +from opendbc.can.dbc import DBC as DbcFile +from opendbc.can.parser import get_raw_value +from opendbc.car import structs +from opendbc.car.can_definitions import CanData +from opendbc.car.honda.hondacan import CanBus +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.bosch_a_radar import ( + BOSCH_A_REANCHOR_MIN_SPAN_S, + BOSCH_A_AZIMUTH_SCALE_RAD, + BOSCH_A_AUX_IDS, + BOSCH_A_DBC_NAME, + BOSCH_A_DIRECT_VREL_INVALID, + BOSCH_A_DIRECT_VREL_MAX_RAW, + BOSCH_A_DIRECT_VREL_MIN_RAW, + BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW, + BOSCH_A_DIRECT_VREL_RAIL_BOUND_MPS, + BOSCH_A_FALLBACK_RANGE_RATE_MAX_MPS, + BOSCH_A_FREQ_HZ, + BOSCH_A_LIFE_SATURATED, + BOSCH_A_MAIN_IDS, + BOSCH_A_NUM_SLOTS, + BOSCH_A_RANGE_RATIO_INVALID, + BOSCH_A_RANGE_OFFSET_M, + BOSCH_A_COAST_REVERSING_MARGIN_MPS, + BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS, + BOSCH_A_RANGE_SCALE_M, + BOSCH_A_STALE_S, + BOSCH_A_SWEEP_END_MSG, + BOSCH_A_TRIGGER_MSG, + _bosch_a_aux_id, + _bosch_a_direct_vrel, + _bosch_a_direct_vrel_interval, + _bosch_a_main_base, + _bosch_a_range_ratio, + _bosch_a_range_ratio_vrel, + bosch_a_range_offset_m, +) +import opendbc.car.honda.bosch_a_radar as radar_interface_module +from opendbc.car.honda.values import CAR +from opendbc.sunnypilot.car.tests.runtime_config import make_test_car_config + + +RADAR_CONFIG = make_test_car_config(bosch_a_radar=True) +CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC_BOSCH, RADAR_CONFIG) +CP_SP = structs.CarParamsSP() +BUS = CanBus(CP).camera + + +# --- synthetic frame builders (inverse of the spec's raw-byte formulas) ----------------------------- + +def make_f0(frame_idx=0, status=0x7, range_raw=0, angle_raw=0, range_sigma_raw=1): + # inverse of raw_angle = (B4 << 3) | (B5 >> 5): B4 carries the top 8 bits, B5's top 3 bits carry the + # bottom 3 bits of the 11-bit angle (the decoder ignores B5's low 5 bits entirely). + B0 = (range_sigma_raw & 0x7F) << 1 + B3 = (frame_idx & 0xF) | ((range_raw & 0xF) << 4) + B2 = (range_raw >> 4) & 0xFF + B1 = (status & 0xF) << 4 + B5 = (angle_raw & 0x7) << 5 + B4 = (angle_raw >> 3) & 0xFF + return bytes([B0, B1, B2, B3, B4, B5, 0, 0]) + + +def make_f1(frame_idx=0, existence_raw=126): + return bytes([0, 0, 0, (frame_idx & 0xF) << 1, 0, existence_raw & 0x7F, 0, 0]) + + +def make_f2(frame_idx=0, life=0, nc_raw=512, nc_sigma_raw=0): + B1 = (frame_idx & 0xF) | ((life & 0xF) << 4) + B0 = (life >> 4) & 0xFF + B2 = (nc_raw >> 2) & 0xFF + B3 = (nc_raw & 0x3) << 6 + B5 = nc_sigma_raw & 0x7F + return bytes([B0, B1, B2, B3, 0, B5, 0, 0]) + + +def make_f3(frame_idx=0, edge_a_raw=0, edge_b_raw=0, sigma_a_raw=0, track_id=0xFF): + B0 = (edge_a_raw >> 3) & 0xFF + B1 = ((edge_a_raw & 0x7) << 5) | ((frame_idx & 0xF) << 1) + B2 = (edge_b_raw >> 3) & 0xFF + B3 = (edge_b_raw & 0x7) << 5 + B4 = (sigma_a_raw >> 2) & 0xFF + B5 = (sigma_a_raw & 0x3) << 6 + return bytes([B0, B1, B2, B3, B4, B5, track_id & 0xFF, 0]) + + +def make_aux(frame_idx=0, rawc9=0, rawca=0, direct_vrel_raw=BOSCH_A_DIRECT_VREL_INVALID, + direct_vrel_uncertainty_raw=0x3FF): + B0 = (direct_vrel_raw >> 3) & 0xFF + B1 = ((direct_vrel_raw & 0x7) << 5) | ((frame_idx & 0xF) << 1) + B2 = (direct_vrel_uncertainty_raw >> 2) & 0xFF + B3 = (direct_vrel_uncertainty_raw & 0x3) << 6 + B5 = (rawc9 & 0x3) << 6 + B4 = (rawc9 >> 2) & 0xFF + B7 = (rawca & 0x3) << 6 + B6 = (rawca >> 2) & 0xFF + return bytes([B0, B1, B2, B3, B4, B5, B6, B7]) + + +def make_main_frames(slot, frame_idx, status, range_raw, angle_raw, life, track_id=1, + range_sigma_raw=1, existence_raw=126, nc_raw=512, nc_sigma_raw=0): + f0, f1, f2, f3 = BOSCH_A_MAIN_IDS[slot] + return [ + CanData(f0, make_f0(frame_idx, status, range_raw, angle_raw, range_sigma_raw), BUS), + CanData(f1, make_f1(frame_idx, existence_raw), BUS), + CanData(f2, make_f2(frame_idx, life, nc_raw, nc_sigma_raw), BUS), + CanData(f3, make_f3(frame_idx, track_id=track_id), BUS), + ] + + +def make_radar_interface(): + _reset_test_counters() + return CarInterface.RadarInterface(CP, CP_SP).bosch_a + + +# The DBC now declares the Honda CHECKSUM (last byte bits 3:0) and the 2-bit rolling COUNTER +# (bits 5:4), so opendbc enforces both. Synthesized frames must therefore carry real framing or +# every message is rejected before any signal reaches the parser. Verified on real captures: +# 361,360/361,360 checksum pass and 214,400/214,400 sequential counter transitions. +_TEST_COUNTERS: dict[int, int] = {} + + +def _reset_test_counters(): + _TEST_COUNTERS.clear() + + +def _stamp(frame: CanData) -> CanData: + """Give one synthesized frame a valid rolling counter and Honda checksum.""" + data = bytearray(frame.dat) + counter = (_TEST_COUNTERS.get(frame.address, -1) + 1) & 0x3 + _TEST_COUNTERS[frame.address] = counter + data[7] = (data[7] & 0xC0) | (counter << 4) + checksum = 0 + address = frame.address + while address > 0: + checksum += address & 0xF + address >>= 4 + for i, b in enumerate(data): + if i == len(data) - 1: + b >>= 4 + checksum += (b >> 4) + (b & 0xF) + data[7] |= (8 - checksum) & 0xF + return CanData(frame.address, bytes(data), frame.src) + + +def sweep(slot, frame_idx, status, range_raw, angle_raw, life, t_nanos, with_aux=False, aux_frame_idx=None, + rawc9=0, rawca=BOSCH_A_RANGE_RATIO_INVALID, extra_slots=(), track_id=1, direct_vrel_raw=BOSCH_A_DIRECT_VREL_INVALID, + direct_vrel_uncertainty_raw=0x3FF, range_sigma_raw=1, existence_raw=126, nc_raw=512, nc_sigma_raw=0): + """Build one update() input: a full main-frame set for `slot` (+ optional aux), plus the trigger + frame (slot 15's f3) so update() always processes the cycle unless the caller is testing slot 15 + itself or an incomplete-frame scenario via extra_slots.""" + f0, f1, f2, f3 = BOSCH_A_MAIN_IDS[slot] + aux = BOSCH_A_AUX_IDS[slot] + frames = make_main_frames( + slot, frame_idx, status, range_raw, angle_raw, life, track_id, + range_sigma_raw=range_sigma_raw, existence_raw=existence_raw, nc_raw=nc_raw, nc_sigma_raw=nc_sigma_raw, + ) + if with_aux: + frames.append(CanData(aux, make_aux(aux_frame_idx if aux_frame_idx is not None else frame_idx, rawc9, rawca, + direct_vrel_raw, direct_vrel_uncertainty_raw), BUS)) + if slot != BOSCH_A_NUM_SLOTS - 1: + _, _, _, trig_f3 = BOSCH_A_MAIN_IDS[BOSCH_A_NUM_SLOTS - 1] + frames.append(CanData(trig_f3, make_f3(frame_idx), BUS)) + for extra in extra_slots: + frames.append(extra) + return [(t_nanos, [_stamp(f) for f in frames])] + + +# --- 1. all 16 slot/frame ID mappings --------------------------------------------------------------- + +def test_all_16_slot_main_ids(): + for slot in range(16): + base = 0x280 + 4 * slot if slot < 4 else 0x2D0 + 4 * (slot - 4) + assert _bosch_a_main_base(slot) == base + assert BOSCH_A_MAIN_IDS[slot] == [base, base + 1, base + 2, base + 3] + + +def test_all_16_slot_aux_ids(): + expected = { + 0: 0x2C8, 1: 0x2C9, 2: 0x2CA, 3: 0x2CB, 4: 0x2CC, 5: 0x2CD, 6: 0x2CE, 7: 0x2CF, + 8: 0x290, 9: 0x291, 10: 0x292, 11: 0x293, 12: 0x294, 13: 0x295, 14: 0x296, 15: 0x297, + } + for slot, addr in expected.items(): + assert _bosch_a_aux_id(slot) == addr + assert BOSCH_A_AUX_IDS[slot] == addr + + +def test_no_duplicate_can_ids_across_80_messages(): + all_ids = [addr for ids in BOSCH_A_MAIN_IDS for addr in ids] + BOSCH_A_AUX_IDS + assert len(all_ids) == 80 + assert len(set(all_ids)) == 80 + + +# --- 2. DBC bit geometry matches the spec's raw-byte formulas exactly (section 16) ------------------- + +class TestDbcBitGeometry: + dbc = DbcFile(BOSCH_A_DBC_NAME) + + def test_f0_fields(self): + msg = self.dbc.msgs[0x280] + rng = random.Random(0) + for _ in range(500): + b = [rng.randint(0, 255) for _ in range(8)] + dat = bytes(b) + assert get_raw_value(dat, msg.sigs['STATUS']) == (b[1] >> 4) & 0x0F + assert get_raw_value(dat, msg.sigs['FRAME_IDX']) == b[3] & 0x0F + assert get_raw_value(dat, msg.sigs['RANGE_RAW']) == (b[2] << 4) | (b[3] >> 4) + assert get_raw_value(dat, msg.sigs['AZIMUTH_RAW']) == (b[4] << 3) | (b[5] >> 5) + + def test_f1_frame_idx(self): + msg = self.dbc.msgs[0x281] + rng = random.Random(1) + for _ in range(500): + b = [rng.randint(0, 255) for _ in range(8)] + assert get_raw_value(bytes(b), msg.sigs['FRAME_IDX']) == (b[3] >> 1) & 0x0F + + def test_f2_fields(self): + msg = self.dbc.msgs[0x282] + rng = random.Random(2) + for _ in range(500): + b = [rng.randint(0, 255) for _ in range(8)] + dat = bytes(b) + assert get_raw_value(dat, msg.sigs['FRAME_IDX']) == b[1] & 0x0F + assert get_raw_value(dat, msg.sigs['LIFECYCLE_RAW']) == (b[0] << 4) | (b[1] >> 4) + + def test_f3_fields(self): + msg = self.dbc.msgs[0x283] + rng = random.Random(3) + for _ in range(500): + b = [rng.randint(0, 255) for _ in range(8)] + dat = bytes(b) + assert get_raw_value(dat, msg.sigs['AZIMUTH_EDGE_A_RAW']) == (b[0] << 3) | (b[1] >> 5) + assert get_raw_value(dat, msg.sigs['FRAME_IDX']) == (b[1] >> 1) & 0x0F + assert get_raw_value(dat, msg.sigs['AZIMUTH_EDGE_B_RAW']) == (b[2] << 3) | (b[3] >> 5) + assert get_raw_value(dat, msg.sigs['AZIMUTH_EDGE_SIGMA_A_RAW']) == (b[4] << 2) | (b[5] >> 6) + + def test_track_id_is_f3_byte6_and_does_not_overlap_existing_fields(self): + def affected_bits(signal): + bits = set() + for byte in range(8): + for bit in range(8): + data = bytearray(8) + data[byte] = 1 << bit + if get_raw_value(bytes(data), signal) != 0: + bits.add((byte, bit)) + return bits + + expected_bits = {(6, bit) for bit in range(8)} + f3_ids = [BOSCH_A_MAIN_IDS[slot][3] for slot in range(BOSCH_A_NUM_SLOTS)] + for message_id in f3_ids: + msg = self.dbc.msgs[message_id] + track_id = msg.sigs['TRACK_ID'] + assert track_id.start_bit == 55 + assert track_id.size == 8 + assert track_id.is_little_endian is False + assert affected_bits(track_id) == expected_bits + + existing_bits = set() + for name, signal in msg.sigs.items(): + if name != 'TRACK_ID': + existing_bits |= affected_bits(signal) + assert not existing_bits & expected_bits + + rng = random.Random(33) + msg = self.dbc.msgs[BOSCH_A_MAIN_IDS[0][3]] + for _ in range(500): + data = bytes(rng.randint(0, 255) for _ in range(8)) + assert get_raw_value(data, msg.sigs['TRACK_ID']) == data[6] + + def test_aux_fields(self): + msg = self.dbc.msgs[0x2C8] + rng = random.Random(4) + for _ in range(500): + b = [rng.randint(0, 255) for _ in range(8)] + dat = bytes(b) + assert get_raw_value(dat, msg.sigs['FRAME_IDX']) == (b[1] >> 1) & 0x0F + assert get_raw_value(dat, msg.sigs['REL_VELOCITY_RAW']) == (b[0] << 3) | (b[1] >> 5) + assert get_raw_value(dat, msg.sigs['REL_VELOCITY_UNCERTAINTY_RAW']) == (b[2] << 2) | (b[3] >> 6) + assert get_raw_value(dat, msg.sigs['FW_LID_00C9_RAW']) == (b[4] << 2) | (b[5] >> 6) + assert get_raw_value(dat, msg.sigs['FW_LID_00CA_RAW']) == (b[6] << 2) | (b[7] >> 6) + assert get_raw_value(dat, msg.sigs['AZIMUTH_EDGE_SIGMA_B_RAW']) == (b[4] << 2) | (b[5] >> 6) + assert get_raw_value(dat, msg.sigs['RANGE_RATIO_RAW']) == (b[6] << 2) | (b[7] >> 6) + + def test_semantic_quality_aliases_exist_on_every_slot(self): + for slot in range(BOSCH_A_NUM_SLOTS): + f0, f1, f2, _ = BOSCH_A_MAIN_IDS[slot] + aux = BOSCH_A_AUX_IDS[slot] + assert self.dbc.msgs[f0].sigs['RANGE_SIGMA_RAW'].size == 7 + assert self.dbc.msgs[f1].sigs['OBJECT_EXISTENCE_PROBABILITY_RAW'].size == 7 + assert self.dbc.msgs[f1].sigs['ANGULAR_WIDTH_RAW'].size == 11 + assert self.dbc.msgs[f2].sigs['NORMALIZED_CLOSING_RAW'].size == 10 + assert self.dbc.msgs[f2].sigs['NORMALIZED_CLOSING_SIGMA_RAW'].size == 7 + assert self.dbc.msgs[aux].sigs['AZIMUTH_EDGE_SIGMA_B_RAW'].size == 10 + assert self.dbc.msgs[aux].sigs['RANGE_RATIO_RAW'].size == 10 + + def test_descriptor_backed_raw_fields_cover_all_unmapped_main_bits(self): + f0_ids = [ + ['25', '26', '28', '29'], ['34', '35', '37', '38'], ['43', '44', '46', '47'], + ['52', '53', '55', '56'], ['61', '62', '64', '65'], ['70', '71', '73', '74'], + ['7F', '80', '82', '83'], ['8E', '8F', '91', '92'], ['9D', '9E', 'A0', 'A1'], + ['AC', 'AD', 'AF', 'B0'], ['BB', 'BC', 'BE', 'BF'], ['CA', 'CB', 'CD', 'CE'], + ['D9', 'DA', 'DC', 'DD'], ['E8', 'E9', 'EB', 'EC'], ['F7', 'F8', 'FA', 'FB'], + ['06', '07', '09', '0A'], + ] + f1_ids = [ + ['2B', '18', '2C', '19', '2D'], ['3A', '23', '3B', '24', '3C'], ['49', '2E', '4A', '2F', '4B'], + ['58', '39', '59', '3A', '5A'], ['67', '44', '68', '45', '69'], ['76', '4F', '77', '50', '78'], + ['85', '5A', '86', '5B', '87'], ['94', '65', '95', '66', '96'], ['A3', '70', 'A4', '71', 'A5'], + ['B2', '7B', 'B3', '7C', 'B4'], ['C1', '86', 'C2', '87', 'C3'], ['D0', '91', 'D1', '92', 'D2'], + ['DF', '9C', 'E0', '9D', 'E1'], ['EE', 'A7', 'EF', 'A8', 'F0'], ['FD', 'B2', 'FE', 'B3', 'FF'], + ['0C', 'BD', '0D', 'BE', '0E'], + ] + f2_ids = [ + ['2F', '1A', '30', '1B', '1C'], ['3E', '25', '3F', '26', '27'], ['4D', '30', '4E', '31', '32'], + ['5C', '3B', '5D', '3C', '3D'], ['6B', '46', '6C', '47', '48'], ['7A', '51', '7B', '52', '53'], + ['89', '5C', '8A', '5D', '5E'], ['98', '67', '99', '68', '69'], ['A7', '72', 'A8', '73', '74'], + ['B6', '7D', 'B7', '7E', '7F'], ['C5', '88', 'C6', '89', '8A'], ['D4', '93', 'D5', '94', '95'], + ['E3', '9E', 'E4', '9F', 'A0'], ['F2', 'A9', 'F3', 'AA', 'AB'], ['01', 'B4', '02', 'B5', 'B6'], + ['10', 'BF', '11', 'C0', 'C1'], + ] + positions = { + 'F0': [(44, 4), (11, 2), (7, 7), (55, 8)], + 'F1': [(46, 7), (23, 11), (15, 8), (39, 9), (7, 6)], + 'F2': [(29, 1), (23, 10), (46, 7), (39, 9), (55, 9)], + } + ids_by_frame = {'F0': f0_ids, 'F1': f1_ids, 'F2': f2_ids} + for slot in range(BOSCH_A_NUM_SLOTS): + for frame in ('F0', 'F1', 'F2'): + msg = self.dbc.msgs[BOSCH_A_MAIN_IDS[slot][int(frame[-1])]] + for logical_id, (start_bit, size) in zip(ids_by_frame[frame][slot], positions[frame], strict=True): + signal = msg.sigs[f'FW_LID_{logical_id}_RAW'] + assert signal.start_bit == start_bit + assert signal.size == size + assert signal.is_little_endian is False + + def affected_bits(signal): + bits = set() + for byte in range(8): + for bit in range(8): + data = bytearray(8) + data[byte] = 1 << bit + if get_raw_value(bytes(data), signal) != 0: + bits.add((byte, bit)) + return bits + + fields = [msg.sigs[f'FW_LID_{logical_id}_RAW'] for logical_id in ids_by_frame[frame][slot]] + covered = set() + for signal in fields: + bits = affected_bits(signal) + assert not covered & bits + covered |= bits + + semantic_aliases = { + 'RANGE_SIGMA_RAW', 'OBJECT_EXISTENCE_PROBABILITY_RAW', + 'ANGULAR_WIDTH_RAW', 'ANGULAR_WIDTH_DEG', + 'NORMALIZED_CLOSING_RAW', 'NORMALIZED_CLOSING', 'NORMALIZED_CLOSING_SIGMA_RAW', + } + existing_bits = set() + for name, signal in msg.sigs.items(): + if not name.startswith('FW_LID_') and name not in semantic_aliases: + existing_bits |= affected_bits(signal) + assert not existing_bits & covered + + +# --- 3. range / azimuth extraction + invalid sentinels ------------------------------------------------ + +class TestRangeAzimuth: + def test_direct_vrel_rails_publish_the_bound(self): + """raw 0 / 1728 are saturation rails, and a rail must still be published as a bound. + + A stationary car approached above 12.0 m/s rails on EVERY sweep, so treating a rail as "no + measurement" made the coast permanent, outlive BOSCH_A_STALE_S and delete the radar point. + Measured on route 000001f9 at 29:52: 88 of 88 active frames railed on two stopped cars, the + radar lead was dropped, and the planner commanded 0.00 while closing at 76 m with a 6.6 s TTC. + Publishing -12.0 understates the closing rate but still brakes; deleting the object does not. + """ + assert _bosch_a_direct_vrel(BOSCH_A_DIRECT_VREL_MIN_RAW) == pytest.approx(-12.0) + assert _bosch_a_direct_vrel(BOSCH_A_DIRECT_VREL_MAX_RAW) == pytest.approx(12.0) + assert _bosch_a_direct_vrel(BOSCH_A_DIRECT_VREL_MIN_RAW + 1) == pytest.approx(-12.0 + 1 / 72) + assert _bosch_a_direct_vrel(BOSCH_A_DIRECT_VREL_MAX_RAW - 1) == pytest.approx(12.0 - 1 / 72) + + def test_railed_stopped_car_keeps_its_radar_point(self): + """The regression that motivated the above: a permanently railed object must not be deleted.""" + ri = make_radar_interface() + rr = None + for i, raw in enumerate((1700, 1680, 1660, 1640, 1620, 1600, 1580)): + rr = ri.update(sweep(0, i & 0xF, 0x7, raw, 1024, 1 + 2 * i, i * 70_000_000, with_aux=True, + direct_vrel_raw=BOSCH_A_DIRECT_VREL_MIN_RAW, direct_vrel_uncertainty_raw=90)) + assert len(rr.points) == 1 + # BOSCH_A_U11_FLOOR_UNMEASURED: kept, but published unmeasured with its own range fit (-1.25 m per 70 ms), not -12. + assert rr.points[0].vRel == pytest.approx(-20 * BOSCH_A_RANGE_SCALE_M / 0.07) + assert not rr.points[0].deprecated.measured + + def test_range_scale_matches_firmware_q16_conversion(self): + """The range scale is firmware-derived, not capture-fitted. + + Stock 36802TBA AC004 converts the internal object range with (q16 - n) / 128, and the + wire-to-internal scaling is q16 = sat16(round(8 * raw_range)). Composing the two gives + + range_m = (8 * raw_range - n) / 128 = raw_range / 16 - n / 128 + + so the scale is exactly 1/16 m per raw count and the offset term is a per-unit + calibration value, not a constant. + + This test exists because the previous 0.05712 was 16 * 0.00357, and that 0.00357 was + solved from a single tape measurement with the offset assumed -- one equation, two + unknowns -- which read progressively short with distance. Pin the scale so a future + fit against a vision or capture reference cannot silently reintroduce that error. + """ + Q16_PER_RAW_COUNT = 8 # q16 = 8 * raw_range + FIRMWARE_Q7_DIVISOR = 128 # AC004's (q16 - n) / 128; Q7 is this firmware's unit + assert BOSCH_A_RANGE_SCALE_M == Q16_PER_RAW_COUNT / FIRMWARE_Q7_DIVISOR + + # The *8 exists so a full-scale 12-bit wire value exactly fills the internal int16. + # If this ever fails the q16 relation above is wrong and the scale must be re-derived. + assert Q16_PER_RAW_COUNT * 0xFFF <= 0x7FFF + + def test_range_scale_and_offset(self): + ri = make_radar_interface() + raw_range = 1000 + ri.update(sweep(0, 0, 0x7, raw_range, 1024, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, raw_range, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert rr.points[0].dRel == pytest.approx(BOSCH_A_RANGE_SCALE_M * raw_range + BOSCH_A_RANGE_OFFSET_M) + + def test_range_invalid_sentinel_0xfff(self): + ri = make_radar_interface() + rr = ri.update(sweep(0, 0, 0x7, 0xFFF, 1024, 1, 0)) + assert len(rr.points) == 0 + + def test_azimuth_invalid_sentinel_0x7ff(self): + ri = make_radar_interface() + rr = ri.update(sweep(0, 0, 0x7, 1000, 0x7FF, 1, 0)) + assert len(rr.points) == 0 + + def test_yrel_sign_right_of_center_is_negative(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024 - 100, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1000, 1024 - 100, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) # raw_angle < center -> right of center + d = BOSCH_A_RANGE_SCALE_M * 1000 + BOSCH_A_RANGE_OFFSET_M + expected_y = d * math.tan(-100.0 / 2048.0) + assert rr.points[0].yRel == pytest.approx(expected_y) + assert rr.points[0].yRel < 0 + + def test_yrel_sign_left_of_center_is_positive(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024 + 100, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1000, 1024 + 100, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) # raw_angle > center -> left of center + assert rr.points[0].yRel > 0 + + def test_yrel_zero_on_boresight(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1000, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert rr.points[0].yRel == pytest.approx(0.0, abs=1e-9) + + +# --- 4. status / life invalid sentinels -> conservative object_valid --------------------------------- + +class TestObjectValid: + def test_status_invalid_0xf(self): + ri = make_radar_interface() + rr = ri.update(sweep(0, 0, 0xF, 1000, 1024, 1, 0)) + assert len(rr.points) == 0 + + def test_life_invalid_0xfff(self): + ri = make_radar_interface() + rr = ri.update(sweep(0, 0, 0x7, 1000, 1024, 0xFFF, 0)) + assert len(rr.points) == 0 + + def test_first_valid_sample_is_withheld(self): + ri = make_radar_interface() + rr = ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + assert len(rr.points) == 0 + + def test_invalid_observation_does_not_mature_or_preserve_birth_history(self): + ri = make_radar_interface() + rr = ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + assert len(rr.points) == 0 + rr = ri.update(sweep(0, 1, 0xF, 1000, 1024, 3, 50_000_000)) + assert len(rr.points) == 0 + rr = ri.update(sweep(0, 2, 0x7, 1000, 1024, 1, 100_000_000)) + assert len(rr.points) == 0 + + def test_incomplete_observation_does_not_mature_or_add_history(self): + ri = make_radar_interface() + rr = ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + assert len(rr.points) == 0 + + f0, f1, f2, f3 = BOSCH_A_MAIN_IDS[0] + _, _, _, trig_f3 = BOSCH_A_MAIN_IDS[15] + frames = [ + CanData(f0, make_f0(1, 0x7, 1010, 1024), BUS), + CanData(f1, make_f1(1), BUS), + CanData(f2, make_f2(1, 3), BUS), + CanData(trig_f3, make_f3(1), BUS), + ] + rr = ri.update([(50_000_000, [_stamp(f) for f in frames])]) + assert len(rr.points) == 0 + + rr = ri.update(sweep(0, 2, 0x7, 1020, 1024, 5, 100_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert len(rr.points) == 1 + assert math.isfinite(rr.points[0].vRel) + + def test_second_same_incarnation_sample_without_u11_or_ratio_does_not_publish(self): + # Neither native U11 nor the ratio field is available on either sample (no aux at all). The raw + # one-sweep range derivative that would previously have matured into a measured=True point is now + # never published: with no prior trusted velocity to coast, the point is withheld entirely. + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000)) + assert len(rr.points) == 0 + + +# --- 5. lifecycle continuity --------------------------------------------------------------------------- + +class TestLifecycle: + def test_normal_plus_2_continuity_keeps_trackid(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + t0 = rr.points[0].trackId + rr = ri.update(sweep(0, 2, 0x7, 1020, 1024, 5, 100_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert rr.points[0].trackId == t0 + + def test_continuity_across_dropped_sweeps(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + t0 = rr.points[0].trackId + # 3 sweeps missed: frame_idx jumps from 0 to 4, life must jump by 2*4=8 to stay the same incarnation + rr = ri.update(sweep(0, 4, 0x7, 1040, 1024, 9, 200_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert rr.points[0].trackId == t0 + + def test_frame_idx_wraps_mod_16(self): + ri = make_radar_interface() + ri.update(sweep(0, 14, 0x7, 1000, 1024, 1, 0)) + rr = ri.update(sweep(0, 15, 0x7, 1005, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + t0 = rr.points[0].trackId + # frame_idx wraps 14 -> 1 (delta = (1-14)&0xF = 3), life must advance by 6 + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 7, 150_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert rr.points[0].trackId == t0 + + def test_life_wraps_mod_4096_stays_same_incarnation(self): + ri = make_radar_interface() + ri.update(sweep(0, 14, 0x7, 1000, 1024, 4094, 0)) + rr = ri.update(sweep(0, 15, 0x7, 1005, 1024, 0, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + t0 = rr.points[0].trackId + # frame_idx 14 -> 0 (delta=2), life 4094 -> 2 ((2-4094)&0xFFF == 4 == 2*2) + rr = ri.update(sweep(0, 0, 0x7, 1010, 1024, 2, 100_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert rr.points[0].trackId == t0 + + def test_in_place_replacement_no_invalid_gap_resets_history_but_keeps_can_id(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + t0 = rr.points[0].trackId + # frame_idx advances normally (+1) but life jumps by an unrelated odd amount -> NOT +2*frame_delta + rr = ri.update(sweep(0, 2, 0x7, 50, 1024, 7, 100_000_000)) + assert len(rr.points) == 0 # replacement birth sample is withheld + rr = ri.update(sweep(0, 3, 0x7, 50, 1024, 9, 150_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert len(rr.points) == 1 + assert rr.points[0].trackId == t0 + + def test_replacement_does_not_assume_life_restarts_at_1_3_5(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + t0 = rr.points[0].trackId + # a "replacement" that happens to restart life at a big/even value must still be treated as a + # replacement (not death) because it fails the frame/life identity, regardless of the new value's parity + rr = ri.update(sweep(0, 2, 0x7, 50, 1024, 4000, 100_000_000)) + assert len(rr.points) == 0 # replacement birth sample is withheld + rr = ri.update(sweep(0, 3, 0x7, 50, 1024, 4002, 150_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert rr.points[0].trackId == t0 + assert rr.points[0].vRel == 0.0 + assert len(rr.points) == 1 + + def _drive_held_life(self, held_life, sweeps=12): + """Two clean sweeps into `held_life`, then `sweeps` more with the counter not advancing. + + Returns the per-sweep published point count after the counter stops moving. + """ + ri = make_radar_interface() + kw = dict(with_aux=True, direct_vrel_raw=800, direct_vrel_uncertainty_raw=40) + ri.update(sweep(0, 0, 0x7, 1000, 1024, held_life - 2, 0, **kw)) + ri.update(sweep(0, 1, 0x7, 997, 1024, held_life, 70_000_000, **kw)) + published = [] + for i in range(2, 2 + sweeps): + rr = ri.update(sweep(0, i % 16, 0x7, 1000 - 3 * i, 1024, held_life, i * 70_000_000, **kw)) + published.append(len(rr.points)) + return ri, published + + def test_saturated_lifecycle_counter_keeps_publishing(self): + """LIFECYCLE_RAW pins at 0xFFE after ~137 s of tracking and cannot advance again. + + Treating each of those sweeps as a new incarnation deletes a still-visible object for as long + as it stays visible: measured at 121.8 s of continuous suppression of the followed lead on + 00000232--fc8dad0d18 (track 37, last published at dRel 38.9 m, yRel -0.1 m). + """ + ri, published = self._drive_held_life(BOSCH_A_LIFE_SATURATED) + assert all(n == 1 for n in published), published + assert len(ri._tracks[1].samples) > 2 # range history survives, so vRel stays derivable + assert ri.pts[1].deprecated.measured + + def test_a_stuck_but_unsaturated_counter_is_still_a_lifecycle_break(self): + """D-009 negative control: the carve-out is for the saturation value only. + + A counter frozen anywhere else is a real discontinuity and must still clear the history -- + otherwise this fix would silently disable incarnation detection everywhere. + """ + _ri, published = self._drive_held_life(4000) + assert all(n == 0 for n in published), published + + def test_death_then_rebirth_reuses_can_id_with_clean_history(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + t0 = rr.points[0].trackId + rr = ri.update(sweep(0, 1, 0xF, 1000, 1024, 3, 50_000_000)) # death + assert len(rr.points) == 0 + rr = ri.update(sweep(0, 2, 0x7, 1000, 1024, 1, 100_000_000)) # rebirth, first sample withheld + assert len(rr.points) == 0 + rr = ri.update(sweep(0, 3, 0x7, 1000, 1024, 3, 150_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert len(rr.points) == 1 + assert rr.points[0].trackId == t0 + + +# --- 6. CAN track identity is the RadarPoint identity ----------------------------------------------- + +def test_trackid_comes_from_can_and_is_not_synthetic(): + ri = make_radar_interface() + seen_ids = set() + t = 0 + for i in range(10): + frame_idx = (2 * i) % 16 + can_track_id = i + 1 + ri.update(sweep(0, frame_idx, 0x7, 1000, 1024, 1, t, track_id=can_track_id)) + t += 50_000_000 + rr = ri.update(sweep(0, (frame_idx + 1) % 16, 0x7, 1000, 1024, 3, t, track_id=can_track_id, + with_aux=True, direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + t += 50_000_000 + assert can_track_id in {point.trackId for point in rr.points} + seen_ids.add(can_track_id) + assert seen_ids == set(range(1, 11)) + + +# --- 7. vRel derivative sign ------------------------------------------------------------------------- + +class TestU11Scale72: + """D-074: U11 is 1/72 m/s per count, built in. There is no 1/64 path and no param.""" + + @staticmethod + def _publish(ri, raw=800, sweeps=2, start=0): + rr = None + for i in range(start, start + sweeps): + rr = ri.update(sweep(0, i % 16, 0x7, 1000 + 10 * i, 1024, 1 + 2 * i, i * 50_000_000, with_aux=True, + direct_vrel_raw=raw, direct_vrel_uncertainty_raw=80)) + return rr + + def test_decode_is_1_72(self): + ri = make_radar_interface() + assert ri.u11_counts_per_mps == 72 + assert self._publish(ri).points[0].vRel == pytest.approx((800 - 864) / 72.0) + + def test_full_raw_domain_and_rails(self): + for raw in range(BOSCH_A_DIRECT_VREL_MIN_RAW, BOSCH_A_DIRECT_VREL_MAX_RAW + 1): + assert _bosch_a_direct_vrel(raw) == (raw - 864) / 72.0 + assert _bosch_a_direct_vrel(0) == -12.0 + assert _bosch_a_direct_vrel(1728) == 12.0 + assert _bosch_a_direct_vrel(BOSCH_A_DIRECT_VREL_INVALID) is None + assert _bosch_a_direct_vrel(1729) is None + + def test_rail_interval_uses_the_1_72_rails(self): + low, high = _bosch_a_direct_vrel(0), _bosch_a_direct_vrel(1728) + assert _bosch_a_direct_vrel_interval(low) == (-BOSCH_A_DIRECT_VREL_RAIL_BOUND_MPS, low) + assert _bosch_a_direct_vrel_interval(high) == (high, BOSCH_A_DIRECT_VREL_RAIL_BOUND_MPS) + inside = _bosch_a_direct_vrel(1) + assert _bosch_a_direct_vrel_interval(inside) == (inside, inside) + + +class TestRangeOffsetFirmwareDefault: + """D-076: fixed firmware ROM-default offset per car, -346/128 Civic (cfg 5530) and -341/128 CR-V. No toggle, no CAN 0x669.""" + + def test_constants(self): + assert BOSCH_A_RANGE_OFFSET_M == -346.0 / 128.0 == -2.703125 + assert bosch_a_range_offset_m(CAR.HONDA_CIVIC_BOSCH) == -2.703125 + assert bosch_a_range_offset_m(CAR.HONDA_CRV_5G) == -2.6640625 + assert bosch_a_range_offset_m(CAR.HONDA_CRV_HYBRID) == -2.6640625 + assert bosch_a_range_offset_m() == -2.703125 + + def test_interface_uses_the_cars_offset(self): + assert make_radar_interface().range_offset_m == -2.703125 + + def test_0x669_is_not_a_parser_message(self): + # A required 0x669 would raise canError whenever the camera does not send it. + ri = make_radar_interface() + assert 0x669 not in ri.rcp.message_states + assert not hasattr(radar_interface_module, "bosch_a_range_offset_fallback_enabled") + +class TestVrel: + def test_direct_aux_vrel_is_preferred_over_range_derivative(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, + direct_vrel_raw=800, direct_vrel_uncertainty_raw=80)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=800, direct_vrel_uncertainty_raw=80)) + assert rr.points[0].vRel == pytest.approx((800 - 864) / 72.0) + + def test_direct_aux_vrel_domain_and_sentinel(self): + # The domain endpoints are saturation rails, published as bounds -- see + # test_direct_vrel_rails_publish_the_bound. + assert _bosch_a_direct_vrel(0) == pytest.approx(-12.0) + assert _bosch_a_direct_vrel(BOSCH_A_DIRECT_VREL_MAX_RAW) == pytest.approx(12.0) + assert _bosch_a_direct_vrel(1) == pytest.approx(-12.0 + 1 / 72) + assert _bosch_a_direct_vrel(BOSCH_A_DIRECT_VREL_MAX_RAW - 1) == pytest.approx(12.0 - 1 / 72) + assert _bosch_a_direct_vrel(1729) is None + assert _bosch_a_direct_vrel(BOSCH_A_DIRECT_VREL_INVALID) is None + assert _bosch_a_direct_vrel(0x7FF) is None + assert _bosch_a_direct_vrel(None) is None + assert _bosch_a_direct_vrel(0, BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW) == pytest.approx(-12.0) + assert _bosch_a_direct_vrel(0, BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW + 1) is None + assert _bosch_a_direct_vrel(864, 0x3FE) is None + assert _bosch_a_direct_vrel(864, 0x3FF) is None + + def test_high_u10_live_vrel_without_trust_is_withheld(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=False)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=0, direct_vrel_uncertainty_raw=1023)) + assert len(rr.points) == 0 + assert len(ri._tracks[1].samples) == 1 + + def test_fast_clean_range_rate_without_u11_or_ratio_withholds_rather_than_synthesizes(self): + # This range rate (~-46 m/s) is well outside native U11's representable domain ([-12.0, 12.0] + # m/s), so it can only ever have come from the raw one-sweep derivative -- which is exactly the + # synthesized-measurement hazard this fix closes. With no aux at all (no U11, no ratio) and no + # prior trusted velocity to coast, the point is correctly withheld rather than published at a + # fabricated, out-of-band rate. + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 2000, 1024, 1, 0, with_aux=False)) + ri.update(sweep(0, 1, 0x7, 1945, 1024, 3, 70_000_000, with_aux=False)) + rr = ri.update(sweep(0, 2, 0x7, 1889, 1024, 5, 140_000_000, with_aux=False)) + assert len(rr.points) == 0 + assert len(ri._tracks[1].samples) == 1 # only the birth sample; later cycles coast, not append + + def test_high_u10_live_vrel_coasts_recent_trusted_motion(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1724, 1024, 1, 0, with_aux=True, + direct_vrel_raw=696, direct_vrel_uncertainty_raw=84)) + rr = ri.update(sweep(0, 1, 0x7, 1724, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=696, direct_vrel_uncertainty_raw=84)) + assert rr.points[0].vRel == pytest.approx((696 - 864) / 72.0) + # R146/S21 ID50's 19-count / 68.962 ms high-U10 step would have synthesized about -15.74 m/s. + rr = ri.update(sweep(0, 2, 0x7, 1705, 1024, 5, 118_962_000, with_aux=True, + direct_vrel_raw=585, direct_vrel_uncertainty_raw=744)) + assert len(rr.points) == 1 + assert rr.points[0].dRel == pytest.approx(1705 * BOSCH_A_RANGE_SCALE_M + BOSCH_A_RANGE_OFFSET_M) + assert rr.points[0].vRel == pytest.approx((696 - 864) / 72.0) + assert not rr.points[0].deprecated.measured + assert len(ri._tracks[1].samples) == 2 + + def test_qualified_u11_recovers_immediately_after_high_u10_coast(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, + direct_vrel_raw=800, direct_vrel_uncertainty_raw=84)) + ri.update(sweep(0, 1, 0x7, 1000, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=800, direct_vrel_uncertainty_raw=84)) + ri.update(sweep(0, 2, 0x7, 981, 1024, 5, 100_000_000, with_aux=True, + direct_vrel_raw=585, direct_vrel_uncertainty_raw=744)) + rr = ri.update(sweep(0, 3, 0x7, 980, 1024, 7, 150_000_000, with_aux=True, + direct_vrel_raw=760, direct_vrel_uncertainty_raw=84)) + assert rr.points[0].vRel == pytest.approx((760 - 864) / 72.0) + assert rr.points[0].deprecated.measured + assert ri._tracks[1].last_trusted_vrel == pytest.approx((760 - 864) / 72.0) + + def test_high_u10_coast_cannot_cross_lifecycle_incarnation(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, + direct_vrel_raw=800, direct_vrel_uncertainty_raw=84)) + ri.update(sweep(0, 1, 0x7, 1000, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=800, direct_vrel_uncertainty_raw=84)) + rr = ri.update(sweep(0, 2, 0x7, 981, 1024, 99, 100_000_000, with_aux=True, + direct_vrel_raw=585, direct_vrel_uncertainty_raw=744)) + assert len(rr.points) == 0 + assert ri._tracks[1].last_trusted_vrel is None + + def test_high_u10_coast_keeps_the_point_with_a_stale_velocity(self): + # A coast means the velocity is doubtful, not that the object left. Object existence is decided + # earlier by STATUS/existence/range validity. Dropping the point here is the 000001f9 failure: + # a stopped car was deleted, radard fell back to vision, and the planner commanded 0.00 at 76 m. + # The geometry must keep publishing; the stale velocity is flagged by measured=False. + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, + direct_vrel_raw=800, direct_vrel_uncertainty_raw=84)) + ri.update(sweep(0, 1, 0x7, 1000, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=800, direct_vrel_uncertainty_raw=84)) + ri.update(sweep(0, 2, 0x7, 981, 1024, 5, 100_000_000, with_aux=True, + direct_vrel_raw=585, direct_vrel_uncertainty_raw=744)) + rr = ri.update(sweep(0, 3, 0x7, 980, 1024, 7, 300_000_000, with_aux=True, + direct_vrel_raw=585, direct_vrel_uncertainty_raw=744)) + assert len(rr.points) == 1 + point = rr.points[0] + # fresh geometry, held velocity, explicitly unmeasured + assert point.dRel == pytest.approx(980 * BOSCH_A_RANGE_SCALE_M + BOSCH_A_RANGE_OFFSET_M) + assert point.vRel == pytest.approx((800 - 864) / 72.0) + assert not point.deprecated.measured + + def test_coast_still_retires_when_the_radar_stops_reporting(self): + # The complement of the above: holding a coasting point must not leak a track that the radar + # has actually dropped. _bosch_a_retire_stale_tracks still owns that, via last_seen_nanos. + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, + direct_vrel_raw=800, direct_vrel_uncertainty_raw=84)) + ri.update(sweep(0, 1, 0x7, 1000, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=800, direct_vrel_uncertainty_raw=84)) + rr = ri.update(sweep(0, 2, 0x7, 981, 1024, 5, 100_000_000, with_aux=True, + direct_vrel_raw=585, direct_vrel_uncertainty_raw=744)) + assert len(rr.points) == 1 + # radar stops reporting this identity: a later cycle past BOSCH_A_STALE_S must retire it + rr = ri.update(sweep(1, 0, 0x7, 900, 1024, 1, 1_000_000_000, with_aux=True, track_id=2, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert all(p.trackId != 1 for p in rr.points) + + def test_range_ratio_conversion_and_sentinel(self): + assert _bosch_a_range_ratio(500) == pytest.approx(1.0) + assert _bosch_a_range_ratio(509) == pytest.approx(1.009) + assert _bosch_a_range_ratio(BOSCH_A_RANGE_RATIO_INVALID) is None + assert _bosch_a_range_ratio(None) is None + assert _bosch_a_range_ratio_vrel(600, 10.0, 0.1) == pytest.approx(-10.0) + + @pytest.mark.parametrize( + ('direct_raw', 'range_raw', 'rawca', 'expected'), + [(0, 974, 528, -12.0), (BOSCH_A_DIRECT_VREL_MAX_RAW, 1027, 472, 12.0)], + ) + def test_range_ratio_does_not_extend_direct_vrel_rails(self, direct_raw, range_raw, rawca, expected): + """At a rail the bound is published, and the ratio field must not be used to exceed it. + + The range-ratio channel is the only one that keeps moving past a rail, but it under-reports by + ~25% against clean U11 (slope 0.72-0.84 over 5011 comparisons), so it cannot be trusted to + manufacture a larger closing rate. Recovering the true value past a rail needs the range + channel and a separate validated change. + + The low rail is that change (BOSCH_A_U11_FLOOR_UNMEASURED): raw 0 publishes the D-043 range fit, unmeasured; a + two-sweep floor birth has no fit yet, so it publishes the floor itself as the bound, unmeasured. The ratio still + never sets it. + """ + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, + direct_vrel_raw=direct_raw, direct_vrel_uncertainty_raw=80, rawca=500)) + rr = ri.update(sweep(0, 1, 0x7, range_raw, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=direct_raw, direct_vrel_uncertainty_raw=80, rawca=rawca)) + if expected is None: + assert len(rr.points) == 0 + return + assert len(rr.points) == 1 + assert rr.points[0].vRel == pytest.approx(expected) + assert rr.points[0].deprecated.measured == (direct_raw != 0) + + def test_first_sighting_vrel_is_zero(self): + ri = make_radar_interface() + rr = ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + assert len(rr.points) == 0 + + def test_decreasing_range_gives_negative_vrel(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 2000, 1024, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1990, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=800, direct_vrel_uncertainty_raw=0)) + assert rr.points[0].vRel < 0 + + def test_increasing_range_gives_positive_vrel(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=928, direct_vrel_uncertainty_raw=0)) + assert rr.points[0].vRel > 0 + + def test_vrel_reflects_native_u11_not_raw_range_derivative(self): + # The raw one-sweep (dRel-previous_range)/dt derivative is no longer an authoritative vRel source + # (see TestFallbackNeverPublishes below) -- confirm the published value tracks native U11 exactly, + # not the range-implied rate, even when the two would clearly disagree. + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + d1 = BOSCH_A_RANGE_SCALE_M * 1000 + BOSCH_A_RANGE_OFFSET_M + d2 = BOSCH_A_RANGE_SCALE_M * 1010 + BOSCH_A_RANGE_OFFSET_M + range_implied_vrel = (d2 - d1) / 0.05 + assert range_implied_vrel != pytest.approx(0.0) + assert rr.points[0].vRel == pytest.approx(0.0) # native U11 (864 -> 0 m/s), not the range rate + + def test_incarnation_does_not_carry_velocity_across_lifecycle_break(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 2000, 1024, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1990, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=800, direct_vrel_uncertainty_raw=0)) + assert rr.points[0].vRel < 0 + # replacement: life breaks identity -> fresh incarnation; its first sample is withheld. + rr = ri.update(sweep(0, 2, 0x7, 500, 1024, 99, 100_000_000)) + assert len(rr.points) == 0 + rr = ri.update(sweep(0, 3, 0x7, 500, 1024, 101, 150_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert rr.points[0].vRel == 0.0 + + def test_discontinuous_range_coasts_last_accepted_point_unmeasured(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=80, rawca=500)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=80, rawca=500)) + assert len(rr.points) == 1 + accepted_drel = rr.points[0].dRel + + rr = ri.update(sweep(0, 2, 0x7, 100, 1024, 5, 100_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=80, rawca=509, + range_sigma_raw=4, existence_raw=0)) + assert len(rr.points) == 1 + assert rr.points[0].dRel == pytest.approx(accepted_drel) + assert rr.points[0].deprecated.measured is False + assert len(ri._tracks[1].samples) == 2 + + def test_gross_velocity_range_disagreement_coasts(self): + """A velocity that flatly contradicts the measured range over several sweeps is not published. + + Modelled on Peter route 000001eb at 6:59, where U11 reported -10.58 m/s closing while the range + was actually opening at +0.46 m/s across a track-identity change. The per-sweep innovation gate + cannot see this: over one ~70 ms sweep even a 3 m/s error moves the range only 0.2 m. + """ + ri = make_radar_interface() + # Four sweeps of a steadily OPENING range, with a believable velocity, to build history. + for i, raw in enumerate((800, 810, 820, 830)): + ri.update(sweep(0, i, 0x7, raw, 1024, 1 + 2 * i, i * 70_000_000, with_aux=True, + direct_vrel_raw=864 + 40, direct_vrel_uncertainty_raw=0)) + # Fifth sweep: range keeps opening, but U11 now claims hard closing. Contradiction ~11 m/s. + rr = ri.update(sweep(0, 4, 0x7, 840, 1024, 9, 280_000_000, with_aux=True, + direct_vrel_raw=864 - 11 * 72, direct_vrel_uncertainty_raw=0)) + assert len(rr.points) == 1 + # Coasted, not the contradictory value, and flagged unmeasured. + assert rr.points[0].vRel == pytest.approx(40 / 72.0) + assert not rr.points[0].deprecated.measured + # Geometry is still live -- only the velocity was in question. + assert rr.points[0].dRel == pytest.approx(840 * BOSCH_A_RANGE_SCALE_M + BOSCH_A_RANGE_OFFSET_M) + + def test_exact_peter_reset_sequence_never_rebases_on_rejected_ranges(self): + ri = make_radar_interface() + rows = [ + # range_raw, U11, U10, range sigma, existence, 00CA + (203, 713, 136, 2, 3, 512), + (198, 740, 92, 2, 76, 510), + (64, 463, 612, 4, 0, 509), + (82, 554, 437, 4, 0, 508), + (96, 727, 101, 2, 0, 508), + (102, 795, 31, 1, 0, 508), + ] + times = [0, 70_696_000, 120_413_000, 200_552_000, 260_482_000, 331_286_000] + + rr = None + for i, ((range_raw, u11, u10, sigma, existence, rawca), t_nanos) in enumerate(zip(rows, times, strict=True)): + rr = ri.update(sweep( + 0, (11 + i) & 0xF, 0x9, range_raw, 1024, 5 + 2 * i, t_nanos, + with_aux=True, direct_vrel_raw=u11, direct_vrel_uncertainty_raw=u10, + rawca=rawca, range_sigma_raw=sigma, existence_raw=existence, track_id=23, + )) + + assert rr is not None + assert len(rr.points) == 0 + + # The point of this sequence: the 64/82/96/102 reset ranges are rejected and must never enter + # the accepted history, so they can never become the baseline for a later derivative. Asserted + # by content rather than by sample count, which also depends on the u10 gate -- row 0 (u10=136) + # now coasts on uncertainty, which is the intended stricter behaviour, not a regression. + accepted = [sample[1] for sample in ri._tracks[23].samples] + rejected = [raw * BOSCH_A_RANGE_SCALE_M + BOSCH_A_RANGE_OFFSET_M for raw in (64, 82, 96, 102)] + assert not any(any(abs(a - r) < 1e-6 for r in rejected) for a in accepted) + assert accepted[-1] == pytest.approx(198 * BOSCH_A_RANGE_SCALE_M + BOSCH_A_RANGE_OFFSET_M) + + +# --- 7a. D-054: a velocity coast must not freeze the range-innovation baseline ---------------------- + +class TestCoastAdvancesTheRangeGate: + """Modelled on 00000232--3a01619ce5 track 43 at t=1258.7 s, the followed lead pulling away 78.5 -> + 84.5 m with a degraded range sigma of 7. U11 lagged the opening, the D-043 rate check coasted, and + the gate kept predicting from the last ACCEPTED sample with that lagging U11 across a growing gap: + the error grew 0.86 / 1.12 / 1.53 / 1.87 / 2.11 m, the degraded 2.0 m limit rejected the lead, and + radar did not publish it again for 15 s while it closed to 27 m.""" + OPEN_RAW_PER_SWEEP = 7 # 0.4375 m per 70 ms sweep: receding at 6.25 m/s + U11_TRUE = 864 + 400 # +6.25 m/s, what the range shows + U11_LAGGING = 864 + 64 # +1.0 m/s: more than 3 m/s short of the range rate, so D-043 coasts + DT_NANOS = 70_000_000 + + def _drive(self, ri, i, u11, range_raw=None, existence_raw=126): + raw = 800 + self.OPEN_RAW_PER_SWEEP * i if range_raw is None else range_raw + rr = ri.update(sweep(0, i & 0xF, 0x7, raw, 1024, 1 + 2 * i, i * self.DT_NANOS, with_aux=True, + direct_vrel_raw=u11, direct_vrel_uncertainty_raw=0, range_sigma_raw=7, + existence_raw=existence_raw)) + return rr, raw * BOSCH_A_RANGE_SCALE_M + BOSCH_A_RANGE_OFFSET_M + + def _history_then_coast(self, ri, coasts): + for i in range(4): + rr, _ = self._drive(ri, i, self.U11_TRUE) + assert rr.points[0].deprecated.measured is True + history = list(ri._tracks[1].samples) + d_rel = None + for i in range(4, 4 + coasts): + rr, d_rel = self._drive(ri, i, self.U11_LAGGING) + assert len(rr.points) == 1 and rr.points[0].deprecated.measured is False + return history, rr, d_rel + + def test_lagging_u11_coast_keeps_publishing_a_receding_lead(self): + ri = make_radar_interface() + history, _, _ = self._history_then_coast(ri, coasts=0) + # 25 coasted sweeps (1.75 s): the old baseline would have rejected the lead from the sixth. + for i in range(4, 29): + rr, d_rel = self._drive(ri, i, self.U11_LAGGING) + assert len(rr.points) == 1, f"lead deleted at sweep {i}" + assert rr.points[0].dRel == pytest.approx(d_rel) + assert rr.points[0].vRel == pytest.approx(400 / 72.0) + assert rr.points[0].deprecated.measured is False + # Coasted ranges moved only the gate's baseline. The velocity history is still accepted-only. + assert list(ri._tracks[1].samples) == history + rr, d_rel = self._drive(ri, 29, self.U11_TRUE) + assert rr.points[0].deprecated.measured is True + assert rr.points[0].dRel == pytest.approx(d_rel) + + def test_rejection_right_after_a_coast_holds_the_point_instead_of_deleting_it(self): + # The last ACCEPTED sample is 0.35 s old here, but the last range that passed the gate is one sweep + # old. A single bad sweep must hold the point unmeasured (D-041/D-042), not delete the lead. + ri = make_radar_interface() + _, _, last_d_rel = self._history_then_coast(ri, coasts=4) + rr, _ = self._drive(ri, 8, self.U11_LAGGING, range_raw=800 + 7 * 8 - 128) # 8 m short + assert len(rr.points) == 1 + assert rr.points[0].dRel == pytest.approx(last_d_rel) + assert rr.points[0].deprecated.measured is False + rr, d_rel = self._drive(ri, 9, self.U11_LAGGING) + assert len(rr.points) == 1 and rr.points[0].dRel == pytest.approx(d_rel) + + def test_reset_ranges_after_a_coast_never_publish_or_enter_history(self): + # Negative control for the change above. The recorded reset shape (see + # test_exact_peter_reset_sequence_never_rebases_on_rejected_ranges): a large drop that walks back + # 1.5 / 1.0 / 0.45 m per sweep, existence 0. A coast must not let any of it become a baseline. + ri = make_radar_interface() + history, _, _ = self._history_then_coast(ri, coasts=4) + lead_raw = 800 + 7 * 8 + reset_raws = [lead_raw - 128, lead_raw - 104, lead_raw - 88, lead_raw - 81, lead_raw - 81, lead_raw - 81] + reset_ranges = [raw * BOSCH_A_RANGE_SCALE_M + BOSCH_A_RANGE_OFFSET_M for raw in reset_raws] + for k, raw in enumerate(reset_raws): + rr, _ = self._drive(ri, 8 + k, self.U11_LAGGING, range_raw=raw, existence_raw=0) + assert not any(any(abs(p.dRel - r) < 1e-6 for r in reset_ranges) for p in rr.points) + assert len(rr.points) == 0 # held no longer than BOSCH_A_STALE_S past the last trusted range + assert list(ri._tracks[1].samples) == history + assert not any(abs(ri._tracks[1].range_anchor[1] - r) < 1e-6 for r in reset_ranges) + + def test_a_persistent_range_step_is_still_rejected_and_not_re_anchored(self): + # D-054 does NOT re-anchor on a real, lasting step: replay found returning excursions just as + # self-consistent as lasting ones, and no lasting >= 5 m step on the lead. Recorded as open. + ri = make_radar_interface() + history, _, _ = self._history_then_coast(ri, coasts=0) + for i in range(4, 14): + rr, _ = self._drive(ri, i, self.U11_TRUE, range_raw=800 + 7 * i + 128, existence_raw=126) + assert len(rr.points) == 0 + assert list(ri._tracks[1].samples) == history + + # 00000237--77313c5a66 track 12 from t=7649.64 s: (ms, range m, U11 m/s, range sigma, existence, u10). + # A new object whose range walked out 25.4 -> 28.1 m while U11 said -2 m/s, then closed to 19 m. + RECORDED_237_TRACK_12 = [ + (0, 25.44, -1.94, 10, 1, 233), (70, 26.38, -1.98, 8, 57, 184), (140, 26.75, -2.08, 8, 81, 166), + (210, 27.31, -1.45, 7, 99, 106), (270, 27.81, -1.50, 6, 84, 93), (350, 28.06, -1.80, 6, 98, 112), + (400, 25.31, -2.69, 5, 86, 123), (470, 23.38, -3.06, 4, 122, 94), (540, 22.12, -1.81, 4, 125, 52), + (610, 21.31, -1.83, 3, 125, 43), (680, 20.69, -1.88, 3, 125, 30), (750, 20.12, -1.94, 3, 125, 33), + (820, 19.62, -1.95, 3, 125, 38), (890, 19.25, -1.98, 2, 110, 37), + ] + + def test_a_coasted_range_walk_does_not_lock_out_the_real_ranges(self): + # The mirror of the 232 lockout: here the RANGE walked and U11 was right. The rate check coasted + # the walk (27.81, 28.06 m); measured against that walk alone, the real 25.31 m misses by 2.6 m + # and the anchor-only draft rejected this object for 53 s. It is consistent with the last + # accepted sample (27.31 m), so it must stay published. + ri = make_radar_interface() + for i, (ms, d, v, sigma, existence, u10) in enumerate(self.RECORDED_237_TRACK_12): + raw = round((d - BOSCH_A_RANGE_OFFSET_M) / BOSCH_A_RANGE_SCALE_M) + rr = ri.update(sweep(0, i & 0xF, 0x7, raw, 1024, 1 + 2 * i, ms * 1_000_000, with_aux=True, + direct_vrel_raw=864 + round(v * 72), direct_vrel_uncertainty_raw=u10, + range_sigma_raw=sigma, existence_raw=existence)) + if i in (4, 5): + assert len(rr.points) == 1 and rr.points[0].deprecated.measured is False + if i >= 6: + assert len(rr.points) == 1, f"real range {d} m rejected at sweep {i}" + assert rr.points[0].dRel == pytest.approx(raw * BOSCH_A_RANGE_SCALE_M + BOSCH_A_RANGE_OFFSET_M) + + def test_a_high_u10_birth_coast_does_not_root_the_gate(self): + # A coast only advances a gate that already exists. A high-u10 birth was never gated or published; + # rooting the gate on it rejected every later sweep 8 m away, as in the first D-054 replay on + # 00000239, where the pre-D-054 parser accepted and published them. + ri = make_radar_interface() + rr = ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=1023)) + assert len(rr.points) == 0 and len(ri._tracks[1].samples) == 0 + assert ri._tracks[1].range_anchor is None + for i in range(1, 4): + rr = ri.update(sweep(0, i, 0x7, 1128 + i, 1024, 1 + 2 * i, i * self.DT_NANOS, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0, range_sigma_raw=7)) + assert len(rr.points) == 1 and rr.points[0].deprecated.measured is True + assert rr.points[0].dRel == pytest.approx(1131 * BOSCH_A_RANGE_SCALE_M + BOSCH_A_RANGE_OFFSET_M) + + def test_lifecycle_break_clears_the_range_anchor(self): + ri = make_radar_interface() + self._history_then_coast(ri, coasts=3) + assert ri._tracks[1].range_anchor is not None + rr = ri.update(sweep(0, 7, 0x7, 900, 1024, 40, 7 * self.DT_NANOS, with_aux=True, + direct_vrel_raw=self.U11_TRUE, direct_vrel_uncertainty_raw=0, range_sigma_raw=7)) + assert len(rr.points) == 0 + assert ri._tracks[1].range_anchor == (pytest.approx(7 * self.DT_NANOS * 1e-9), + pytest.approx(900 * BOSCH_A_RANGE_SCALE_M + BOSCH_A_RANGE_OFFSET_M)) + assert len(ri._tracks[1].samples) == 1 + + +# --- 7a'. D-055: an invalid slot must not hide an identity that is valid elsewhere ------------ + +class TestInvalidSlotDoesNotHideAMigratedIdentity: + """Modelled on 00000237--77313c5a66 track 9 (the followed lead, 21.2 s dark) and 00000232 track 43. + The radar moved the object to another wire slot in the same sweep its old slot went invalid. The + invalid-observation hide deleted the point, the D-043 rate check then coasted the valid observation, + and a coast only updates an existing point, so the lead stayed dark until an accept.""" + DT_NANOS = 70_000_000 + U11 = 864 # 0 m/s + + def _history(self, ri, slot=0): + for i in range(4): + rr = ri.update(sweep(slot, i, 0x7, 1000, 1024, 1 + 2 * i, i * self.DT_NANOS, with_aux=True, + direct_vrel_raw=self.U11, direct_vrel_uncertainty_raw=0)) + assert len(rr.points) == 1 and rr.points[0].deprecated.measured is True + + def _migrate(self, ri, i, *, u10, invalid_track_id=0xFF, valid_elsewhere=True): + invalid_old_slot = make_main_frames(0, i, 0xF, 1000, 1024, 1 + 2 * i, track_id=invalid_track_id) + if valid_elsewhere: + return ri.update(sweep(1, i, 0x7, 1000, 1024, 1 + 2 * i, i * self.DT_NANOS, with_aux=True, + direct_vrel_raw=self.U11, direct_vrel_uncertainty_raw=u10, + extra_slots=invalid_old_slot)) + return ri.update(sweep(2, i, 0x7, 1200, 1024, 1, i * self.DT_NANOS, track_id=2, + extra_slots=invalid_old_slot)) + + @pytest.mark.parametrize("invalid_track_id", [0xFF, 1]) + def test_a_coasted_identity_that_migrated_slots_keeps_its_point(self, invalid_track_id): + ri = make_radar_interface() + self._history(ri) + # u10 above the qualified limit: the valid observation in slot 1 coasts. + rr = self._migrate(ri, 4, u10=1023, invalid_track_id=invalid_track_id) + assert [p.trackId for p in rr.points] == [1] + assert rr.points[0].deprecated.measured is False + assert rr.points[0].vRel == pytest.approx(0.0) + + def test_an_accepted_identity_that_migrated_slots_still_publishes_measured(self): + ri = make_radar_interface() + self._history(ri) + rr = self._migrate(ri, 4, u10=0) + assert [p.trackId for p in rr.points] == [1] and rr.points[0].deprecated.measured is True + + def test_negative_control_an_invalid_slot_still_hides_an_identity_seen_nowhere_else(self): + ri = make_radar_interface() + self._history(ri) + rr = self._migrate(ri, 4, u10=0, valid_elsewhere=False) + assert 1 not in [p.trackId for p in rr.points] + assert 1 in ri._tracks # hidden, not retired: the history survives to its stale deadline + + +# --- 7a''. D-057: a lasting, clean range step re-anchors instead of locking the lead out ---- + +class TestLastingCleanStepReAnchors: + """Modelled on 00000236--60bfb34cb1 track 38 at t=1703.6 s and 00000237 track 31 at t=761.5 s. A new + object's birth ranges were accepted, then its range settled several metres closer and every later + sweep contradicted both baselines. The lead stayed dark for 27.4 s / 20.4 s while it closed to 38 m / + 20 m, with non-degraded ranges moving at the U11 rate.""" + DT_NANOS = 70_000_000 + CLOSING_RAW = 2.24 # 2 m/s over a 70 ms sweep + + def _drive(self, ri, i, raw, u11_mps, sigma=1, existence=126): + return ri.update(sweep(0, i & 0xF, 0x7, round(raw), 1024, (1 + 2 * i) & 0xFFF, i * self.DT_NANOS, with_aux=True, + direct_vrel_raw=864 + round(u11_mps * 72), direct_vrel_uncertainty_raw=0, + range_sigma_raw=sigma, existence_raw=existence)) + + def _birth(self, ri): + raw = 1650 # ~103 m + for i in range(6): + raw -= self.CLOSING_RAW + rr = self._drive(ri, i, raw, -2.0) + assert rr.points[0].deprecated.measured is True + return raw + + def test_a_clean_lasting_step_at_the_u11_rate_is_published_again(self): + ri = make_radar_interface() + raw = self._birth(ri) - 136 # settles 8.5 m closer + published_at = None + for i in range(6, 60): + raw -= self.CLOSING_RAW + rr = self._drive(ri, i, raw, -2.0) + if rr.points and rr.points[0].deprecated.measured and published_at is None: + published_at = i + assert rr.points[0].dRel == pytest.approx(round(raw) * BOSCH_A_RANGE_SCALE_M + BOSCH_A_RANGE_OFFSET_M) + assert published_at is not None + assert (published_at - 6) * self.DT_NANOS * 1e-9 >= BOSCH_A_REANCHOR_MIN_SPAN_S + assert (published_at - 6) * self.DT_NANOS * 1e-9 <= BOSCH_A_REANCHOR_MIN_SPAN_S + 0.15 + + # Range sigma 20 is above the D-089 range-scaled limit at ~94 m (0.15 x 94 = 14.1), so it stays degraded. + @pytest.mark.parametrize("sigma,existence", [(20, 126), (1, 0)]) + def test_negative_control_a_degraded_lasting_step_never_re_anchors(self, sigma, existence): + ri = make_radar_interface() + raw = self._birth(ri) - 136 + for i in range(6, 60): + raw -= self.CLOSING_RAW + rr = self._drive(ri, i, raw, -2.0, sigma=sigma, existence=existence) + assert not any(p.deprecated.measured for p in rr.points) + + def test_negative_control_a_clean_step_that_contradicts_u11_never_re_anchors(self): + ri = make_radar_interface() + # The range holds still while U11 claims 4 m/s closing. The step is 25 m so the anchor's U11 + # extrapolation cannot catch the range inside this window: a smaller step lets the existing + # stale-anchor "join" publish (D-054 residual finding), which would mask what D-057 does. + raw = self._birth(ri) - 400 + for i in range(6, 60): + rr = self._drive(ri, i, raw, -4.0) + assert not any(p.deprecated.measured for p in rr.points) + + +class TestAJoinWaitsForAFreshRateFit: + """D-059 (STATUS item 22.5). A range-rejection run ends when a later range passes the gate against the stale anchor's + U11 extrapolation (a "join"). D-043 then fitted the pre-gap samples plus the joined range, so the gap, + not the object, set the fitted rate. Replayed on 7 routes (232/236/237/239/23a/23b/23e): measured vRel + in the second after a join over-closed the next-1 s range slope by >3 m/s on 21.0 % of sweeps + (33.1 % where the run's own slope contradicted U11) against 4.6 % for reference points. Measured vRel resumes only once a fit over the post-join ranges + alone also agrees.""" + DT_NANOS = 70_000_000 + CLOSING_RAW = 2.24 # 2 m/s over a 70 ms sweep + + def _drive(self, ri, i, raw, u11_mps): + return ri.update(sweep(0, i & 0xF, 0x7, round(raw), 1024, (1 + 2 * i) & 0xFFF, i * self.DT_NANOS, with_aux=True, + direct_vrel_raw=864 + round(u11_mps * 72), direct_vrel_uncertainty_raw=0, + range_sigma_raw=1, existence_raw=126)) + + def _birth(self, ri): + raw = 1650 + for i in range(6): + raw -= self.CLOSING_RAW + rr = self._drive(ri, i, raw, -2.0) + assert rr.points[0].deprecated.measured is True + return raw + + def test_negative_control_a_join_that_contradicts_u11_is_published_unmeasured(self): + ri = make_radar_interface() + # The range settles 8.5 m closer and holds still while U11 claims 4 m/s closing. The anchor's U11 + # extrapolation meets it after ~0.84 s. Before 22.5 that join published vRel -4.0 as measured. + raw = self._birth(ri) - 136 + joined_d_rel = round(raw) * BOSCH_A_RANGE_SCALE_M + BOSCH_A_RANGE_OFFSET_M + joined = False + for i in range(6, 60): + rr = self._drive(ri, i, raw, -4.0) + assert not any(p.deprecated.measured for p in rr.points), f"measured at sweep {i}" + if rr.points and rr.points[0].dRel == pytest.approx(joined_d_rel): + joined = True + elif joined: + pytest.fail(f"joined point deleted at sweep {i}") + assert joined + + def test_a_walk_that_returns_resumes_measured_after_a_fresh_fit(self): + ri = make_radar_interface() + raw = self._birth(ri) + for i in range(6, 20): # ~1 s walk 8 m out; the object keeps closing underneath it + raw -= self.CLOSING_RAW + rr = self._drive(ri, i, raw + 128, -2.0) + assert not any(p.deprecated.measured for p in rr.points) # held, then deleted past BOSCH_A_STALE_S (unchanged) + measured_at = None + for i in range(20, 40): + raw -= self.CLOSING_RAW + rr = self._drive(ri, i, raw, -2.0) + assert len(rr.points) == 1, f"point deleted at sweep {i}" + if rr.points[0].deprecated.measured and measured_at is None: + measured_at = i + assert measured_at is not None + assert (measured_at - 20) * self.DT_NANOS * 1e-9 <= 0.5 + + +class TestRateCheckCoastReRoots: + """D-062 (STATUS 69/70). Modelled on 00000258--626242f48b track 13 at 43:29-43:46: the lead was opening, then + braked hard. The D-043 rate check fits `samples`, which only accepted sweeps extend. Coasts had left that + history stale, so the fit read the old opening rate, the first rejection froze it there, and from then on + U11 minus the stale average rate grows with the lead's deceleration: every later U11 was rejected while + the gated range closed from 124 m to 31 m at the U11 rate. The point coasted vRel -0.45 for 12.7 s and + the planner FCW fired. A lasting, clean run of rate-check coasts whose ranges move at the U11 rate now + re-roots `samples` on the run, as D-057 does for range rejection.""" + DT = 0.07 + RANGE_M_PER_RAW = 0.0625 + HOLD = 36 # ~2.5 s of high-u10 coast while the lead starts braking: `samples` stay on the opening ranges + + def _drive(self, ri, i, d, u11_mps, uncertainty=0, sigma=1): + return ri.update(sweep(0, i & 0xF, 0x7, round(d / self.RANGE_M_PER_RAW), 1024, (1 + 2 * i) & 0xFFF, + round(i * self.DT * 1e9), with_aux=True, + # Clamped to the rails as the radar does: past -12.0 m/s an unclamped raw would go negative + # and the 11-bit packer would wrap it to a valid-looking positive U11. + direct_vrel_raw=min(max(864 + round(u11_mps * 72), BOSCH_A_DIRECT_VREL_MIN_RAW), + BOSCH_A_DIRECT_VREL_MAX_RAW), + direct_vrel_uncertainty_raw=uncertainty, range_sigma_raw=sigma, existence_raw=126)) + + def _run(self, ri, vrel, *, sigma=1, u11=None): + """Opening at +2.8 m/s for 12 sweeps, then vRel(t) from `vrel`; yields (sweep, true vRel, result).""" + d = 87.0 + for i in range(12): + d += 2.8 * self.DT + rr = self._drive(ri, i, d, 2.8) + assert rr.points[0].deprecated.measured is True + for i in range(12, 12 + self.HOLD + 60): + v = vrel((i - 11) * self.DT) + d += v * self.DT + u = v if u11 is None else u11 + rr = self._drive(ri, i, d, u, uncertainty=600 if i < 12 + self.HOLD else 0, sigma=sigma) + yield i, v, rr + + def test_a_lead_braking_after_a_coast_is_measured_again(self): + ri = make_radar_interface() + first = 12 + self.HOLD + measured_at = None + for i, v, rr in self._run(ri, lambda t: 2.8 - 3.0 * t): + assert len(rr.points) == 1, f"point deleted at sweep {i}" # D-041: coast, never delete + if i >= first and rr.points[0].deprecated.measured and measured_at is None: + measured_at = i + assert rr.points[0].vRel == pytest.approx(v, abs=0.1) + assert measured_at is not None + assert (measured_at - first) * self.DT <= BOSCH_A_REANCHOR_MIN_SPAN_S + 0.15 + + def test_negative_control_u11_over_closing_an_opening_range_stays_coasted(self): + ri = make_radar_interface() + for i, _, rr in self._run(ri, lambda t: 2.8, u11=-6.0): + assert len(rr.points) == 1, f"point deleted at sweep {i}" + assert rr.points[0].deprecated.measured is False, f"over-closing U11 admitted at sweep {i}" + + def test_negative_control_a_degraded_run_never_re_roots(self): + ri = make_radar_interface() + for i, _, rr in self._run(ri, lambda t: 2.8 - 3.0 * t, sigma=7): + if i >= 12: + assert not any(p.deprecated.measured for p in rr.points), f"measured at sweep {i}" + + +# --- 7b. residual vRel-authority fix: raw one-sweep fallback never becomes a published measurement ---- +# (U11 genuinely unavailable -- sentinel/out-of-range -- is a different path than the high-u10-but-live +# case above; see radar_interface.py's u11_and_ratio_unavailable branch.) + +class TestFallbackNeverPublishes: + def test_invalid_u11_and_ratio_with_trusted_history_coasts_not_derivative(self): + """TEST 1: invalid U11 + unusable ratio + a recent trusted velocity on record. A large range jump + that would imply a big closing velocity via the raw derivative must not reach vRel/measured/KF -- + it should coast the trusted value instead.""" + ri = make_radar_interface() + # Establish trust: two qualified-U11 samples at a steady range. + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + rr = ri.update(sweep(0, 1, 0x7, 1000, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert rr.points[0].deprecated.measured is True + assert rr.points[0].vRel == pytest.approx(0.0) + + # Third sweep: U11 sentinel (unavailable), ratio invalid, and a range jump that -- via the raw + # (dRel-previous_range)/dt derivative -- would imply a closing velocity of about -17 m/s over 50ms. + # Deliberately kept under the 50 m/s generic range-rejection ceiling so this exercises the new + # u11_and_ratio_unavailable coast path specifically, not the pre-existing range_rejected path. + rr = ri.update(sweep(0, 2, 0x7, 985, 1024, 5, 100_000_000, with_aux=True, + direct_vrel_raw=BOSCH_A_DIRECT_VREL_INVALID, direct_vrel_uncertainty_raw=0, + rawca=BOSCH_A_RANGE_RATIO_INVALID)) + implied_fallback_vrel = (985 - 1000) * BOSCH_A_RANGE_SCALE_M / 0.05 + # Confirms the setup would poison if reached, while staying under the generic ceiling so this + # exercises the coast path and not range_rejected. Stated as bounds, not a magic number, so a + # range-scale change cannot silently invalidate the scenario. + assert implied_fallback_vrel < -15.0 + assert abs(implied_fallback_vrel) < BOSCH_A_FALLBACK_RANGE_RATE_MAX_MPS + assert abs(implied_fallback_vrel) < BOSCH_A_FALLBACK_RANGE_RATE_MAX_MPS # and clears range_rejected + assert len(rr.points) == 1 + assert rr.points[0].deprecated.measured is False + assert rr.points[0].vRel == pytest.approx(0.0) # coasted trusted value, not the -17 m/s derivative + # The KF-facing measurement update never happened: last_trusted_vrel/age is untouched by this cycle. + assert ri._tracks[1].last_trusted_vrel == pytest.approx(0.0) + assert ri._tracks[1].last_trusted_vrel_nanos == 50_000_000 + + def test_invalid_u11_and_ratio_without_trusted_history_withholds_point(self): + """TEST 2: invalid U11 + unusable ratio + no prior trusted velocity. No fabricated point.""" + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, + direct_vrel_raw=BOSCH_A_DIRECT_VREL_INVALID, direct_vrel_uncertainty_raw=0)) + rr = ri.update(sweep(0, 1, 0x7, 985, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=BOSCH_A_DIRECT_VREL_INVALID, direct_vrel_uncertainty_raw=0, + rawca=BOSCH_A_RANGE_RATIO_INVALID)) + assert len(rr.points) == 0 + + def test_invalid_u11_with_valid_ratio_still_publishes_via_ratio(self): + """TEST 3: invalid U11 + a valid ratio field. Untouched by this fix -- still publishes via + ratio_vrel, exactly as before. rawca=520 is chosen so the ratio-implied residual (~1.08m) clears + innovation checking, so this exercises the ratio_vrel publish path and not range_rejected.""" + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, + direct_vrel_raw=BOSCH_A_DIRECT_VREL_INVALID, direct_vrel_uncertainty_raw=0, + rawca=500)) + rr = ri.update(sweep(0, 1, 0x7, 1000, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=BOSCH_A_DIRECT_VREL_INVALID, direct_vrel_uncertainty_raw=0, + rawca=520)) + d = BOSCH_A_RANGE_SCALE_M * 1000 + BOSCH_A_RANGE_OFFSET_M + ratio = 0.5 + 0.001 * 520 + residual = abs(d - d * ratio) + assert residual < 2.0 # clears innovation checking regardless of degraded status + assert len(rr.points) == 1 + assert rr.points[0].deprecated.measured is True + expected = d * (1.0 - ratio) / 0.05 + assert rr.points[0].vRel == pytest.approx(expected) + + +# --- 8. auxiliary tag join: enrichment only, never gates validity ------------------------------------- + +class TestAuxiliary: + def test_aux_matching_cycle_is_attached(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, aux_frame_idx=0, rawc9=123, rawca=456)) + st = ri._slots[0] + assert st.logical_00c9_raw == 123 + assert st.logical_00ca_raw == 456 + + def test_aux_mismatched_cycle_not_attached_and_point_withheld_without_vrel_source(self): + # A mismatched aux frame_idx means the AUX payload (U11, ratio, and the c9/ca enrichment fields) + # is never attached for that cycle -- it's as if aux were absent. With no vRel source and no prior + # trusted velocity, the point is correctly withheld rather than synthesized from the range alone. + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, aux_frame_idx=5, rawc9=123, rawca=456)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, aux_frame_idx=5, rawc9=123, rawca=456)) + assert len(rr.points) == 0 + st = ri._slots[0] + assert math.isnan(st.logical_00c9_raw) and math.isnan(st.logical_00ca_raw) # never attached + + def test_aux_absent_without_trust_withholds_point(self): + # Aux absence alone no longer implies "fall back to the raw range derivative" -- with no U11, no + # ratio, and no prior trusted velocity to coast, the point is withheld rather than fabricated. + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=False)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=False)) + assert len(rr.points) == 0 + + def test_aux_absent_does_not_suppress_point_once_trust_is_established(self): + # A subsequent aux-absent sweep does NOT suppress a point once a recent trusted velocity exists -- + # it coasts that trusted value (measured=False) rather than withholding the point outright. + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + ri.update(sweep(0, 1, 0x7, 1000, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + rr = ri.update(sweep(0, 2, 0x7, 1000, 1024, 5, 100_000_000, with_aux=False)) + assert len(rr.points) == 1 + assert rr.points[0].vRel == pytest.approx(0.0) + assert rr.points[0].deprecated.measured is False + + def test_aux_param_invalid_sentinel(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, aux_frame_idx=0, rawc9=1, rawca=0x3FF)) + st = ri._slots[0] + assert st.logical_00c9_raw == 1 + assert math.isnan(st.logical_00ca_raw) + + def test_sigma_does_not_share_aux_param_invalid_sentinel(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, with_aux=True, + aux_frame_idx=0, rawc9=0x3FF, rawca=500)) + st = ri._slots[0] + assert st.logical_00c9_raw == 0x3FF + assert st.logical_00ca_raw == 500 + + +# --- 9. missing CAN frame within a cycle does not kill an existing point ------------------------------- + +def test_incomplete_main_frame_set_leaves_existing_point_untouched(): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert len(rr.points) == 1 + t0 = rr.points[0].trackId + + # Next cycle: only f0/f1/f2 arrive for slot 0 (f3 missing) -- still send the trigger so update() runs. + f0, f1, f2, f3 = BOSCH_A_MAIN_IDS[0] + _, _, _, trig_f3 = BOSCH_A_MAIN_IDS[15] + frames = [ + CanData(f0, make_f0(1, 0x7, 1000, 1024), BUS), + CanData(f1, make_f1(1), BUS), + CanData(f2, make_f2(1, 3), BUS), + CanData(trig_f3, make_f3(1), BUS), + ] + rr = ri.update([(50_000_000, [_stamp(f) for f in frames])]) + assert len(rr.points) == 1 # untouched, not dropped + assert rr.points[0].trackId == t0 + + +def test_incoherent_frame_index_across_main_frames_is_skipped(): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + t0 = rr.points[0].trackId + + f0, f1, f2, f3 = BOSCH_A_MAIN_IDS[0] + _, _, _, trig_f3 = BOSCH_A_MAIN_IDS[15] + frames = [ + CanData(f0, make_f0(1, 0x7, 1000, 1024), BUS), + CanData(f1, make_f1(1), BUS), + CanData(f2, make_f2(2, 3), BUS), # frame idx mismatch vs f0/f1 + CanData(f3, make_f3(1), BUS), + CanData(trig_f3, make_f3(1), BUS), + ] + rr = ri.update([(50_000_000, [_stamp(f) for f in frames])]) + assert len(rr.points) == 1 # unchanged from before, not re-derived, not dropped + assert rr.points[0].trackId == t0 + + +# --- 10. staleness gate ----------------------------------------------------------------------------- + +def test_stale_bus_clears_points_and_flags_temporary_unavailable(): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert len(rr.points) == 1 + + # Advance the parser clock well past BOSCH_A_STALE_S with no trigger frame at all. + f0, f1, f2, f3 = BOSCH_A_MAIN_IDS[0] + frames = [CanData(f0, make_f0(1, 0x7, 1000, 1024), BUS)] # not a full/coherent set, and no trigger + rr = ri.update([(300_000_000, [_stamp(f) for f in frames])]) # +300ms, no trigger msg present + assert rr is not None + assert len(rr.points) == 0 + assert rr.errors.radarUnavailableTemporary is True + + +def test_stale_bus_clears_pending_birth_history_before_maturity(): + ri = make_radar_interface() + rr = ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0)) + assert len(rr.points) == 0 + assert len(ri._tracks[1].samples) == 1 + + # The birth has started, but no RadarPoint exists yet. A silent bus must still clear the pending + # lifecycle/history state rather than allowing it to survive indefinitely. + f0, _, _, _ = BOSCH_A_MAIN_IDS[0] + rr = ri.update([(300_000_000, [CanData(f0, make_f0(1, 0x7, 1000, 1024), BUS)])]) + assert rr is not None + assert len(rr.points) == 0 + assert rr.errors.radarUnavailableTemporary is True + assert not ri._tracks + + +# --- 11. persistent CAN identity is separate from wire-slot assembly ------------------------------- + +class TestPersistentCanIdentity: + def test_same_slot_same_can_identity_is_stable(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, track_id=42)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, track_id=42, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert len(rr.points) == 1 + assert rr.points[0].trackId == 42 + assert set(ri._tracks) == {42} + + def test_slot_migration_preserves_identity_and_ols_history(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, track_id=23)) + rr = ri.update(sweep(1, 1, 0x7, 1010, 1024, 3, 50_000_000, track_id=23, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert len(rr.points) == 1 + assert rr.points[0].trackId == 23 + assert len(ri._tracks[23].samples) == 2 + assert ri._tracks[23].wire_slot == 1 + + def test_slot_zero_one_zero_keeps_one_logical_track(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, track_id=17)) + ri.update(sweep(1, 1, 0x7, 1010, 1024, 3, 50_000_000, track_id=17, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + rr = ri.update(sweep(0, 2, 0x7, 1020, 1024, 5, 100_000_000, track_id=17, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert len(rr.points) == 1 + assert [point.trackId for point in rr.points] == [17] + assert len(ri._tracks) == 1 + assert len(ri._tracks[17].samples) == 3 + assert ri._tracks[17].wire_slot == 0 + + def test_same_slot_replacement_preserves_returning_identity_history(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, track_id=2)) + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, track_id=2, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert [point.trackId for point in rr.points] == [2] + assert len(ri._tracks[2].samples) == 2 + + # Bosch can put a different persistent object in the same wire slot while the slot's lifecycle + # counter continues. The old logical track is not dead: retain its state until its own staleness + # deadline so it can resume without a synthetic birth or an OLS reset. + rr = ri.update(sweep(0, 2, 0x7, 1500, 1024, 5, 100_000_000, track_id=63)) + assert set(ri._tracks) == {2, 63} + assert len(ri._tracks[2].samples) == 2 + assert len(rr.points) == 0 + assert 2 not in ri.pts + + rr = ri.update(sweep(0, 3, 0x7, 1020, 1024, 7, 150_000_000, track_id=2, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert [point.trackId for point in rr.points] == [2] + assert len(ri._tracks[2].samples) == 3 + assert math.isfinite(rr.points[0].vRel) + + def test_two_wire_slots_with_different_ids_publish_two_points(self): + ri = make_radar_interface() + first_extra = make_main_frames(1, 0, 0x7, 1400, 1024, 11, track_id=22) + ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, extra_slots=first_extra, track_id=11)) + second_extra = make_main_frames(1, 1, 0x7, 1410, 1024, 13, track_id=22) + [ + CanData(BOSCH_A_AUX_IDS[1], make_aux(1, direct_vrel_raw=864, direct_vrel_uncertainty_raw=0), BUS), + ] + rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, + extra_slots=second_extra, track_id=11, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert {point.trackId for point in rr.points} == {11, 22} + assert len(rr.points) == 2 + + @pytest.mark.parametrize('track_id', [0, 0xFF, 0x40, 0xA5]) + def test_invalid_can_identity_never_creates_logical_track(self, track_id): + ri = make_radar_interface() + rr = ri.update(sweep(0, 0, 0x7, 1000, 1024, 1, 0, track_id=track_id)) + assert len(rr.points) == 0 + assert not ri._tracks + + def test_track_id_reuse_after_death_starts_with_clean_history(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 2000, 1024, 1, 0, track_id=9)) + rr = ri.update(sweep(0, 1, 0x7, 1990, 1024, 3, 50_000_000, track_id=9, with_aux=True, + direct_vrel_raw=800, direct_vrel_uncertainty_raw=0)) + assert rr.points[0].trackId == 9 + rr = ri.update(sweep(0, 2, 0xF, 1900, 1024, 5, 100_000_000, track_id=9)) + assert len(rr.points) == 0 + rr = ri.update(sweep(0, 3, 0x7, 1000, 1024, 1, 150_000_000, track_id=9)) + assert len(rr.points) == 0 + rr = ri.update(sweep(0, 4, 0x7, 1000, 1024, 3, 200_000_000, track_id=9, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert len(rr.points) == 1 + assert rr.points[0].trackId == 9 + assert rr.points[0].vRel == 0.0 + + def test_lifecycle_discontinuity_keeps_can_id_but_clears_derivative(self): + ri = make_radar_interface() + ri.update(sweep(0, 0, 0x7, 2000, 1024, 1, 0, track_id=31)) + rr = ri.update(sweep(0, 1, 0x7, 1990, 1024, 3, 50_000_000, track_id=31, with_aux=True, + direct_vrel_raw=800, direct_vrel_uncertainty_raw=0)) + assert rr.points[0].vRel < 0 + rr = ri.update(sweep(0, 2, 0x7, 500, 1024, 99, 100_000_000, track_id=31)) + assert len(rr.points) == 0 + rr = ri.update(sweep(0, 3, 0x7, 500, 1024, 101, 150_000_000, track_id=31, with_aux=True, + direct_vrel_raw=864, direct_vrel_uncertainty_raw=0)) + assert len(rr.points) == 1 + assert rr.points[0].trackId == 31 + assert rr.points[0].vRel == 0.0 + + +def test_no_can_data_returns_none_without_crash(): + ri = make_radar_interface() + assert ri.update([]) is None + + +def test_bosch_a_azimuth_scale_is_exact_firmware_scale(): + assert BOSCH_A_AZIMUTH_SCALE_RAD == pytest.approx(1.0 / 2048.0) + + +def test_bosch_a_observed_sweep_end_and_publish_trigger_are_distinct(): + # 0x297 is the observed final object-family frame, but 0x2FF intentionally + # remains the publish trigger while companion data is optional. + assert BOSCH_A_TRIGGER_MSG == 0x2FF + assert BOSCH_A_SWEEP_END_MSG == 0x297 + + +def test_bosch_a_timing_contract(): + assert BOSCH_A_FREQ_HZ == pytest.approx(14.35) + assert BOSCH_A_STALE_S == pytest.approx(0.20) + + +# --- misc integration: DBC wiring ------------------------------------------------------------------- + +def test_honda_bosch_a_radar_dbc_wired_and_available(): + assert CP.radarUnavailable is False + ri = make_radar_interface() + assert ri.rcp is not None + + +def test_civic_bosch_object_feed_uses_camera_side_acc_can(): + ri = make_radar_interface() + can = CanBus(CP) + + assert ri.rcp.bus == can.camera + assert ri.rcp.bus != can.radar + + +# Computed once at collection time, like CP above -- the openpilot_function_fixture in the root +# conftest.py gives every test function its own fresh, isolated OpenpilotPrefix(), so a +# get_non_essential_params() call made from *inside* a test body runs in an environment where the +# BoschARadar=True set at module import time (before any fixture exists) was never written. +# Computing these at module scope, in the same environment CP uses, sidesteps that entirely. + +# radarless and CANFD Bosch platforms respectively -- same HONDA_BOSCH family as Civic Bosch, but +# excluded from HONDA_BOSCH_A, so the gate must stay closed regardless of the tester toggle. + + +# --- D-063: the rail interval, built in (was BoschARailInterval) ------------------------------- + +def test_rail_interval_gate_reads_rail_as_bound_only_when_asked(): + from opendbc.car.honda.bosch_a_radar import (_bosch_a_range_innovation_rejected, _bosch_a_direct_vrel_interval, + BOSCH_A_DIRECT_VREL_RAIL_BOUND_MPS) + low_rail = _bosch_a_direct_vrel_interval(-12.0)[1] + assert _bosch_a_direct_vrel_interval(low_rail) == (-BOSCH_A_DIRECT_VREL_RAIL_BOUND_MPS, low_rail) + assert _bosch_a_direct_vrel_interval(low_rail, exact=True) == (low_rail, low_rail) + assert _bosch_a_direct_vrel_interval(-5.0) == (-5.0, -5.0) + # A lead closing at 20 m/s while U11 sits on the -12.0 rail: 100 m -> 80 m in 1 s. Exact gate rejects + # (8.0 m short of the rail's 88 m prediction, past the 5 m hard max); the interval gate accepts (on the + # rail-to-20 m/s band's edge, residual 0). + assert _bosch_a_range_innovation_rejected((0.0, 100.0), 1.0, 80.0, low_rail, None, False, exact=True) + assert not _bosch_a_range_innovation_rejected((0.0, 100.0), 1.0, 80.0, low_rail, None, False, exact=False) + # Past the physical cap the interval rejects too: 100 m -> 74 m in 1 s is 26 m/s, 6 m outside the band. + assert _bosch_a_range_innovation_rejected((0.0, 100.0), 1.0, 74.0, low_rail, None, False, exact=False) + # An unrailed U11 is exact either way. + assert (_bosch_a_range_innovation_rejected((0.0, 100.0), 1.0, 91.0, -5.0, None, False, exact=False) == + _bosch_a_range_innovation_rejected((0.0, 100.0), 1.0, 91.0, -5.0, None, False, exact=True)) + + +def test_rail_interval_and_coast_range_bound_built_in(): + ri = make_radar_interface() + assert ri.rail_interval is True + assert ri.coast_range_bound is True + + +class TestRailIntervalBoundsTheCoast: + """D-063 addendum (STATUS 92). With BoschARailInterval on, a coast is bounded to within 3 m/s of a fresh + fit over the coast's own ranges (rejoin_samples / inconsistent_run). Modelled on 0000025e 6:43 track 48: + the rail interval admitted a range walk, the D-059 hold then coasted the -13.5 rail (1/64 units) for 1.5 s while its + own post-join fit read +0.8 m/s. Off, the coast is the pre-D-063 verbatim last trusted vRel.""" + DT_NANOS = 70_000_000 + RAIL_RAW = 0 # U11 low rail: true closing >= 12.0 m/s + RAIL_MPS = -12.0 + + # These exercise the D-063 low-rail machinery with raw U11 0, which BOSCH_A_U11_FLOOR_UNMEASURED now takes off that + # path (the machinery still serves the raw 1728 rail). Run them on the decoder as it was before that change. + def setup_method(self, method=None): + self._floor_unmeasured = radar_interface_module.BOSCH_A_U11_FLOOR_UNMEASURED + radar_interface_module.BOSCH_A_U11_FLOOR_UNMEASURED = False + + def teardown_method(self, method=None): + radar_interface_module.BOSCH_A_U11_FLOOR_UNMEASURED = self._floor_unmeasured + + def _drive(self, ri, i, d, u11_raw, uncertainty=0): + return ri.update(sweep(0, i & 0xF, 0x7, round((d - BOSCH_A_RANGE_OFFSET_M) / BOSCH_A_RANGE_SCALE_M), 1024, + (1 + 2 * i) & 0xFFF, i * self.DT_NANOS, with_aux=True, direct_vrel_raw=u11_raw, + direct_vrel_uncertainty_raw=uncertainty, range_sigma_raw=1, existence_raw=126)) + + def _railed_birth_then_walk(self, ri): + """6 sweeps closing at the rail rate (measured -12.0), then the range steps 8 m closer and holds still + with U11 still railed. The interval gate joins the settled range ~0.2 s later (rail_admitted), the exact + gate only after ~0.6 s. Yields (sweep, result) over the 2 s that follow the step.""" + d = 60.0 + for i in range(6): + d += self.RAIL_MPS * self.DT_NANOS * 1e-9 + rr = self._drive(ri, i, d, self.RAIL_RAW) + assert rr.points[0].deprecated.measured is True and rr.points[0].vRel == pytest.approx(self.RAIL_MPS, abs=0.05) + d -= 8.0 + for i in range(6, 6 + 30): + yield i, self._drive(ri, i, d, self.RAIL_RAW) + + def test_on_a_rejoin_hold_coast_is_bounded_by_the_post_join_fit(self): + ri = make_radar_interface() + ri.rail_interval = True + bounded_at = None + for i, rr in self._railed_birth_then_walk(ri): + for p in rr.points: + if not p.deprecated.measured: + if bounded_at is None and p.vRel > self.RAIL_MPS + 0.01: + bounded_at = i + # The post-join ranges hold still: fit 0 m/s, so the coast may not over-close it by more than 3 m/s. + assert p.vRel == pytest.approx(-BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS, abs=0.2) + elif bounded_at is not None: + assert p.vRel > self.RAIL_MPS + 0.01, f"rail re-coasted at sweep {i}" + assert bounded_at is not None + # Bounded within the first second after the step (join ~0.2 s + 4 fresh samples over >= 0.25 s). + assert (bounded_at - 6) * self.DT_NANOS * 1e-9 <= 1.0 + + def test_off_the_same_coast_is_the_verbatim_last_trusted_vrel(self): + ri = make_radar_interface() + ri.rail_interval = False # set explicitly: the toggle test above writes the shared params store under xdist + ri.coast_range_bound = False + coasts = 0 + for i, rr in self._railed_birth_then_walk(ri): + for p in rr.points: + if not p.deprecated.measured: + coasts += 1 + assert p.vRel == pytest.approx(self.RAIL_MPS, abs=0.05), f"coast changed at sweep {i} (toggle off)" + assert coasts > 0 + + def test_range_derived_vrel_alone_never_softens_an_over_closing_coast(self): + """STATUS 111: without the rail interval the bound is one-sided. The same railed coast whose fresh fit reads + 0 m/s keeps the rail: making a coast LESS closing is the rail-interval path's job, not this one's.""" + ri = make_radar_interface() + ri.rail_interval = False + ri.coast_range_bound = True + coasts = 0 + for i, rr in self._railed_birth_then_walk(ri): + for p in rr.points: + if not p.deprecated.measured: + coasts += 1 + assert p.vRel == pytest.approx(self.RAIL_MPS, abs=0.05), f"coast softened at sweep {i}" + assert coasts > 0 + + def _coasts(self, ri): + return [(i, p.vRel) for i, rr in self._railed_birth_then_walk(ri) for p in rr.points if not p.deprecated.measured] + + def test_a_coast_that_implies_a_reversing_lead_is_bounded_outside_a_rail_hold(self): + """STATUS 179, route 00000287 2:39: a stale over-closing coast outside any rail hold that says the car ahead + is reversing. At 10 m/s the -12.0 coast means vLead -2.0. Before a fresh fit exists it is floored at + vLead = -margin; once the fit (0 m/s here) exists it is pulled to within 3 m/s of it. The first sweeps + after the 8 m step are range-rejected: that path keeps the whole last point (geometry too) untouched and + is out of scope here.""" + ri = make_radar_interface() + ri.rail_interval = False + ri.coast_range_bound = True + ri.v_ego = 10.0 + coasts = self._coasts(ri) + rejected = [(i, v) for i, v in coasts if i < 8] + coasts = [(i, v) for i, v in coasts if i >= 8] + assert rejected and all(v == pytest.approx(self.RAIL_MPS, abs=0.05) for _, v in rejected) + assert coasts + floor = -(10.0 + BOSCH_A_COAST_REVERSING_MARGIN_MPS) + assert all(v >= floor - 1e-6 for _, v in coasts) + assert coasts[0][1] == pytest.approx(floor, abs=0.05) + assert coasts[-1][1] == pytest.approx(-BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS, abs=0.2) + + @pytest.mark.parametrize("v_ego", [None, 12.0, 20.0], ids=["unknown", "stationary", "slower_car"]) + def test_a_coast_that_does_not_imply_reversing_keeps_the_one_sided_bound(self, v_ego): + """-12.0 at 12 m/s is a stopped car and at 20 m/s a slower car: STATUS 129's protected + over-closing coasts. Neither, nor an unknown ego speed, is softened.""" + ri = make_radar_interface() + ri.rail_interval = False + ri.coast_range_bound = True + ri.v_ego = v_ego + coasts = self._coasts(ri) + assert coasts and all(v == pytest.approx(self.RAIL_MPS, abs=0.05) for _, v in coasts) + + def test_both_toggles_off_the_reversing_coast_is_verbatim(self): + ri = make_radar_interface() + ri.rail_interval = False + ri.coast_range_bound = False + ri.v_ego = 10.0 + coasts = self._coasts(ri) + assert coasts and all(v == pytest.approx(self.RAIL_MPS, abs=0.05) for _, v in coasts) + + def test_on_with_fewer_than_four_fresh_samples_the_coast_is_unchanged(self): + ri = make_radar_interface() + ri.rail_interval = True + first_coast = None + for i, rr in self._railed_birth_then_walk(ri): + for p in rr.points: + if not p.deprecated.measured and first_coast is None: + first_coast = (i, p.vRel) + assert first_coast is not None and first_coast[1] == pytest.approx(self.RAIL_MPS, abs=0.05) + + def _stale_opening_then_closing(self, ri): + """Opening at +2.8 m/s for 12 sweeps, then closing at 5 m/s with U11 agreeing. Yields each coasted point + until the first measured one.""" + dt = self.DT_NANOS * 1e-9 + d = 87.0 + for i in range(12): + d += 2.8 * dt + rr = self._drive(ri, i, d, 864 + round(2.8 * 72)) + assert rr.points[0].deprecated.measured is True + for i in range(12, 12 + 12): + d += -5.0 * dt + rr = self._drive(ri, i, d, 864 + round(-5.0 * 72)) + assert len(rr.points) == 1 + if rr.points[0].deprecated.measured: + return + yield i, rr.points[0] + + def test_off_an_inconsistent_coast_holds_the_stale_opening_vrel(self): + """Both toggles off: the pre-STATUS 92 behaviour, the route 00000268 9:52 shape (+4.06 coasted 2.35 s).""" + ri = make_radar_interface() + ri.rail_interval = False + ri.coast_range_bound = False + coasts = [p.vRel for _, p in self._stale_opening_then_closing(ri)] + assert coasts and all(v == pytest.approx(2.8, abs=0.05) for v in coasts) + + @pytest.mark.parametrize("rail_interval, coast_range_bound", [(True, False), (False, True)], + ids=["BoschARailInterval", "RangeDerivedVrel"]) + def test_on_an_inconsistent_coast_of_a_stale_opening_vrel_is_pulled_toward_the_closing_fit( + self, rail_interval, coast_range_bound): + """D-062 shape: opening at +2.8 m/s, then the lead brakes to 5 m/s closing with U11 agreeing. The stale + `samples` fit rejects that U11 (one-sided, > 3 m/s below +2.8) and the coast holds +2.8 until D-062 + re-roots after 1.5 s. On, the fresh inconsistent_run fit (-5) bounds the coast to -2 within 5 sweeps. + Either toggle turns the bound on (STATUS 111 split it out of the rail interval for route 00000268).""" + ri = make_radar_interface() + ri.rail_interval = rail_interval + ri.coast_range_bound = coast_range_bound + dt = self.DT_NANOS * 1e-9 + d = 87.0 + for i in range(12): + d += 2.8 * dt + rr = self._drive(ri, i, d, 864 + round(2.8 * 72)) + assert rr.points[0].deprecated.measured is True + pulled_at = None + for i in range(12, 12 + 12): + d += -5.0 * dt + rr = self._drive(ri, i, d, 864 + round(-5.0 * 72)) + assert len(rr.points) == 1 + p = rr.points[0] + if p.deprecated.measured: + break + if pulled_at is None and p.vRel < 2.8 - 0.01: + pulled_at = i + assert p.vRel == pytest.approx(-5.0 + BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS, abs=0.2) + assert pulled_at is not None and pulled_at - 12 <= 5 + + +class TestRailIntervalExactOnDegradedSweeps: + """STATUS 129 (item 92's next step). With BoschARailInterval on, a DEGRADED sweep reads a railed U11 exactly in + the D-054 gate unless the range itself (a fit over the rejected run plus this sweep) moves at a railed speed. + Modelled on 0000025e 402.912 s, track 48: a degraded sweep one sweep after its baseline, exact residual 2.32 m + (over the degraded 2.0 m limit), interval residual 1.93 m, no rejected run to fit. The interval admitted it, + and the walk it started coasted a stale -13.5 rail (1/64 units) on an opening range.""" + DT_NANOS = 70_000_000 + RAIL_RAW = 0 + + # These exercise the D-063 low-rail machinery with raw U11 0, which BOSCH_A_U11_FLOOR_UNMEASURED now takes off that + # path (the machinery still serves the raw 1728 rail). Run them on the decoder as it was before that change. + def setup_method(self, method=None): + self._floor_unmeasured = radar_interface_module.BOSCH_A_U11_FLOOR_UNMEASURED + radar_interface_module.BOSCH_A_U11_FLOOR_UNMEASURED = False + + def teardown_method(self, method=None): + radar_interface_module.BOSCH_A_U11_FLOOR_UNMEASURED = self._floor_unmeasured + + def _drive(self, ri, i, d, range_sigma_raw): + return ri.update(sweep(0, i & 0xF, 0x7, round((d - BOSCH_A_RANGE_OFFSET_M) / BOSCH_A_RANGE_SCALE_M), 1024, + (1 + 2 * i) & 0xFFF, i * self.DT_NANOS, with_aux=True, direct_vrel_raw=self.RAIL_RAW, + direct_vrel_uncertainty_raw=0, range_sigma_raw=range_sigma_raw, existence_raw=126)) + + def _step(self, ri, range_sigma_raw): + """6 sweeps at the rail rate, then one sweep 2.3 m short of the rail's prediction (exact residual 2.3 m; + interval residual 2.3 - 6.5 * 0.07 = 1.85 m). Returns (published point, stepped range, previous range).""" + dt = self.DT_NANOS * 1e-9 + d = 60.0 + for i in range(6): + d += -12.0 * dt + rr = self._drive(ri, i, d, 1) + assert rr.points[0].deprecated.measured is True + d_prev = rr.points[0].dRel + d_step = d_prev - 12.0 * dt - 2.3 + rr = self._drive(ri, 6, d_step, range_sigma_raw) + assert len(rr.points) == 1, "the point must be kept either way (D-041)" + return rr.points[0], rr.points[0].dRel, d_step, d_prev + + @pytest.mark.parametrize("rail_interval", [False, True], ids=["off", "on"]) + def test_degraded_sweep_is_gated_on_the_exact_rail(self, rail_interval): + ri = make_radar_interface() + ri.rail_interval = rail_interval + ri.coast_range_bound = False + p, published, d_step, d_prev = self._step(ri, range_sigma_raw=7) + # Rejected: the point coasts at the last accepted range, and nothing starts a rail-admitted hold. + assert p.deprecated.measured is False + assert published == pytest.approx(d_prev, abs=0.07) + assert ri._tracks[1].rejoin_samples is None + + def test_clean_sweep_still_uses_the_interval(self): + """The same step on a clean sweep: under the 5 m hard limit, so it passes either way (the exact gate + has headroom there); the interval never tightens anything.""" + ri = make_radar_interface() + ri.rail_interval = True + ri.coast_range_bound = False + p, published, d_step, _ = self._step(ri, range_sigma_raw=1) + assert published == pytest.approx(d_step, abs=0.07) + + def test_without_the_degraded_rule_the_interval_admits_it(self, monkeypatch): + """Negative control: the item 92 behaviour. The degraded step is admitted through the interval.""" + from opendbc.car.honda import bosch_a_radar as HRI + monkeypatch.setattr(HRI, "BOSCH_A_RAIL_INTERVAL_EXACT_WHEN_DEGRADED", False) + ri = make_radar_interface() + ri.rail_interval = True + ri.coast_range_bound = False + p, published, d_step, _ = self._step(ri, range_sigma_raw=7) + assert published == pytest.approx(d_step, abs=0.07) + assert p.deprecated.measured is False + assert ri._tracks[1].rejoin_samples is not None + + +class TestRailIntervalDownSideOnlyInsideARailHold: + """STATUS 129. With BoschARailInterval on, the coast bound's LESS-closing side acts only inside a hold that a + rail-interval admission started. Modelled on 00000266 795.4: a stale over-closing coast (-8.2 held while the + range sat still) on a track the interval never touched. Applied to every coast, the down side softened that + protected brake; now it gets the one-sided bound, as with RangeDerivedVrel alone.""" + DT_NANOS = 70_000_000 + + def _drive(self, ri, i, d, u11_mps): + return ri.update(sweep(0, i & 0xF, 0x7, round((d - BOSCH_A_RANGE_OFFSET_M) / BOSCH_A_RANGE_SCALE_M), 1024, + (1 + 2 * i) & 0xFFF, i * self.DT_NANOS, with_aux=True, direct_vrel_raw=864 + round(u11_mps * 72), + direct_vrel_uncertainty_raw=0, range_sigma_raw=1, existence_raw=126)) + + def _coasts(self, ri): + dt = self.DT_NANOS * 1e-9 + d = 70.0 + for i in range(12): + d += -8.0 * dt + rr = self._drive(ri, i, d, -8.0) + assert rr.points[0].deprecated.measured is True + out = [] + for i in range(12, 24): + d += -1.0 * dt + rr = self._drive(ri, i, d, -8.0) + assert len(rr.points) == 1 + if not rr.points[0].deprecated.measured: + out.append(rr.points[0].vRel) + return out + + def test_over_closing_coast_outside_a_rail_hold_is_not_softened(self): + ri = make_radar_interface() + ri.rail_interval = True + ri.coast_range_bound = False + coasts = self._coasts(ri) + assert coasts and all(v == pytest.approx(-8.0, abs=0.05) for v in coasts) + assert ri._tracks[1].rail_hold is False + + def test_negative_control_the_item_92_two_sided_clamp_softens_it(self, monkeypatch): + from opendbc.car.honda import bosch_a_radar as HRI + monkeypatch.setattr(HRI, "BOSCH_A_RAIL_INTERVAL_DOWN_SIDE_ONLY_ON_RAIL_HOLD", False) + ri = make_radar_interface() + ri.rail_interval = True + ri.coast_range_bound = False + coasts = self._coasts(ri) + assert any(v > -8.0 + 1.0 for v in coasts) + + +class TestDegradedRailAdmissionNeedsRangeCorroboration: + """STATUS 129. A degraded, railed track re-acquired after a rejected run: admitted through the interval only when + the rejected run's own ranges close at a railed speed. Modelled on 0000025e 12:03 track 59 (degraded on every + sweep, closing ~16-18 m/s: admitted) against 0000025e 6:43 track 48 (a walk 47.9 -> 35.3 m, then the range sat + still while U11 stayed on the rail: not admitted).""" + DT_NANOS = 70_000_000 + + # These exercise the D-063 low-rail machinery with raw U11 0, which BOSCH_A_U11_FLOOR_UNMEASURED now takes off that + # path (the machinery still serves the raw 1728 rail). Run them on the decoder as it was before that change. + def setup_method(self, method=None): + self._floor_unmeasured = radar_interface_module.BOSCH_A_U11_FLOOR_UNMEASURED + radar_interface_module.BOSCH_A_U11_FLOOR_UNMEASURED = False + + def teardown_method(self, method=None): + radar_interface_module.BOSCH_A_U11_FLOOR_UNMEASURED = self._floor_unmeasured + + def _drive(self, ri, i, d, sigma): + return ri.update(sweep(0, i & 0xF, 0x7, round((d - BOSCH_A_RANGE_OFFSET_M) / BOSCH_A_RANGE_SCALE_M), 1024, + (1 + 2 * i) & 0xFFF, i * self.DT_NANOS, with_aux=True, direct_vrel_raw=0, + direct_vrel_uncertainty_raw=0, range_sigma_raw=sigma, existence_raw=126)) + + def _run(self, step_m, after_step_rate): + """Measured at the rail rate, then a step of `step_m` closer, then degraded sweeps moving at `after_step_rate`. + Each geometry is chosen so the exact -12.0 prediction rejects every degraded sweep and the [-20, -12.0] + interval admits some of them; only the rejected run's own range rate separates the two.""" + ri = make_radar_interface() + ri.rail_interval = True + ri.coast_range_bound = False + dt = self.DT_NANOS * 1e-9 + d = 100.0 + for i in range(6): + d += -12.0 * dt + rr = self._drive(ri, i, d, 1) + assert rr.points[0].deprecated.measured is True + d -= step_m + admitted_at, rail_hold_at_admission = None, None + for i in range(6, 6 + 12): + d += after_step_rate * dt + rr = self._drive(ri, i, d, 7) + # A range-rejected run older than BOSCH_A_STALE_S is withheld with the toggle on or off (the dark lead). + if admitted_at is None and len(rr.points) == 1 and rr.points[0].dRel == pytest.approx(d, abs=0.07): + admitted_at, rail_hold_at_admission = i, ri._tracks[1].rail_hold + return rail_hold_at_admission, admitted_at + + def test_a_range_closing_at_a_railed_speed_is_admitted(self): + # 12:03-like: a 2.5 m step, then the range keeps closing at 17 m/s (fit on the rail interval) + rail_hold, admitted_at = self._run(2.5, -17.0) + assert admitted_at is not None + assert rail_hold is True + + def test_a_range_that_stopped_closing_is_not_admitted(self): + # 6:43-like: a 15 m walk, then the range sits still while U11 stays on the rail + _, admitted_at = self._run(15.0, 0.0) + assert admitted_at is None + + def test_negative_control_without_the_rule_the_stopped_range_is_admitted(self, monkeypatch): + from opendbc.car.honda import bosch_a_radar as HRI + monkeypatch.setattr(HRI, "BOSCH_A_RAIL_INTERVAL_EXACT_WHEN_DEGRADED", False) + _, admitted_at = self._run(15.0, 0.0) + assert admitted_at is not None + + +def test_rail_interval_still_admits_a_degraded_railed_lead_across_a_long_gap(): + """STATUS 129, the case D-063 exists for: 0000025e 12:03 track 59, degraded on every sweep, re-acquired against + a baseline 2.16 s old (121.3 m) at 79.5 m while U11 sat on the rail. The exact prediction is 95.4 m (92.1 m at the + 1/64 rail); the interval [-20, -12.0] m/s reaches 78.0 m.""" + from opendbc.car.honda.bosch_a_radar import (_bosch_a_range_innovation_rejected, _bosch_a_direct_vrel, + BOSCH_A_DIRECT_VREL_MIN_RAW) + low_rail = _bosch_a_direct_vrel(BOSCH_A_DIRECT_VREL_MIN_RAW) + baseline, now_s, d = (722.069, 121.31), 724.233, 79.50 + assert _bosch_a_range_innovation_rejected(baseline, now_s, d, low_rail, None, True, exact=True) + assert not _bosch_a_range_innovation_rejected(baseline, now_s, d, low_rail, None, True, exact=False) + + +def test_degraded_rail_corroboration_fits_the_tail_of_the_run_not_the_finished_walk(): + """STATUS 129, 0000025e 6:43 track 48 at 403.383 s: the rejected run walked 44.8 -> 35.3 m and then sat still. + The shortest D-043 tail reads the range as stopped (far off the rail interval); an 8-sweep fit averages the walk + in, reads a railed speed and would have admitted it (negative control).""" + from opendbc.car.honda.bosch_a_radar import (_bosch_a_trailing_fit_window, _bosch_a_fresh_range_rate, + _bosch_a_distance_to_interval, _bosch_a_direct_vrel_interval, + _bosch_a_direct_vrel, BOSCH_A_DIRECT_VREL_MIN_RAW, + BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS) + run = [(402.912, 44.75), (402.973, 42.56), (403.044, 39.12), (403.113, 36.12), (403.173, 35.31), + (403.243, 34.94), (403.313, 35.12), (403.383, 35.31)] + interval = _bosch_a_direct_vrel_interval(_bosch_a_direct_vrel(BOSCH_A_DIRECT_VREL_MIN_RAW)) + tail = _bosch_a_trailing_fit_window(run) + assert len(tail) == 5 + assert _bosch_a_distance_to_interval(_bosch_a_fresh_range_rate(tail), interval) > BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS + assert _bosch_a_distance_to_interval(_bosch_a_fresh_range_rate(run), interval) <= BOSCH_A_VREL_RATE_CHECK_MAX_DISAGREEMENT_MPS + + +# --- NC-at-rail (stopshadow-radar, replay only) ---------------------------------------------------------- + + +def test_newborn_vrel_is_the_range_fit_bounded_at_stationary(): + """BOSCH_A_NEWBORN_RANGE_PUBLISH: the fit rate, bounded at -vEgo; no live U11, no rail floor.""" + from opendbc.car.honda.bosch_a_radar import _bosch_a_newborn_vrel + dt = 1.0 / BOSCH_A_FREQ_HZ + closing = [(i * dt, 116.0 - 20.0 * i * dt) for i in range(6)] + assert _bosch_a_newborn_vrel(closing, None) == pytest.approx(-20.0) + assert _bosch_a_newborn_vrel(closing, 25.0) == pytest.approx(-20.0) + # Faster than a stopped object can close: bounded at -vEgo (well past the U11 low rail). + assert _bosch_a_newborn_vrel(closing, 19.8) == pytest.approx(-19.8) + assert _bosch_a_newborn_vrel(closing, 12.0) == pytest.approx(-12.0) + assert _bosch_a_newborn_vrel(closing[:2], 19.8) is None # too short to fit + noisy = [(i * dt, 100.0 + (3.0 if i % 2 else -3.0)) for i in range(6)] + assert _bosch_a_newborn_vrel(noisy, 19.8) is None # over BOSCH_A_REANCHOR_MAX_RMS_M + + +def test_newborn_history_is_not_seeded_by_default(): + # BOSCH_A_NEWBORN_SEED_HISTORY off: 00000284 22:36.7 track 17's seeded burst held a -10.47 coast for 1.3 s. + from opendbc.car.honda import bosch_a_radar as radar_interface + assert radar_interface.BOSCH_A_NEWBORN_SEED_HISTORY is False + + +class TestNewbornLeadsToggle: + """BOSCH_A_NEWBORN_RANGE_PUBLISH is built in on (the BoschANewbornLeads toggle is removed) and copied into + RadarInterface.newborn_range_publish. ON: a high-u10 newborn is published on its range fit. OFF (replays/tests): + withheld exactly as before the newborn publish.""" + + @staticmethod + def _drive_newborn(ri, sweeps=10): + # A stopped car from 116 m closing at 20 m/s, every sweep with U11 above BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW. + ri.v_ego = 19.8 + dt_nanos = int(1e9 / BOSCH_A_FREQ_HZ) + published = [] + for i in range(sweeps): + d = 116.0 - 20.0 * i / BOSCH_A_FREQ_HZ + rr = ri.update(sweep(0, i % 16, 0x7, int(round(d * 16)), 1024, 1 + 2 * i, i * dt_nanos, with_aux=True, + direct_vrel_raw=600, direct_vrel_uncertainty_raw=BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW + 200)) + if rr is not None: + published.append(list(rr.points)) + return published + + def test_source_default_is_off_on_7_0(self): + # off in the port: it needs Trung's radard NEWBORN_* exemptions, which 7.0's radard does not have + from opendbc.car.honda import bosch_a_radar as radar_interface + assert radar_interface.BOSCH_A_NEWBORN_RANGE_PUBLISH is False + assert not hasattr(radar_interface, "bosch_a_newborn_leads_enabled") + + def test_off_never_publishes_a_newborn(self, monkeypatch): + from opendbc.car.honda import bosch_a_radar as radar_interface + monkeypatch.setattr(radar_interface, "BOSCH_A_NEWBORN_RANGE_PUBLISH", False) + ri = make_radar_interface() + assert ri.newborn_range_publish is False + published = self._drive_newborn(ri) + assert len(published) == 10 + assert all(len(points) == 0 for points in published) + + def test_on_publishes_the_newborn_on_its_range_fit(self): + ri = make_radar_interface() + ri.newborn_range_publish = True + published = self._drive_newborn(ri) + assert any(len(points) for points in published) + first = next(points for points in published if points) + assert first[0].deprecated.measured is False + assert first[0].vRel == pytest.approx(-19.8, abs=0.5) # the range fit (-20), bounded at -vEgo + + +class TestU11FloorUnmeasured: + """BOSCH_A_U11_FLOOR_UNMEASURED: raw U11 0 is never a velocity measurement. Static evidence only.""" + + @staticmethod + def _sweep(ri, i, range_raw, u11): + return ri.update(sweep(0, i & 0xF, 0x7, range_raw, 1024, 1 + 2 * i, i * 70_000_000, with_aux=True, + direct_vrel_raw=u11, direct_vrel_uncertainty_raw=90)) + + def test_floor_born_track_publishes_the_floor_bound_then_the_range_fit(self): + ri = make_radar_interface() + published = [] + for i in range(7): + rr = self._sweep(ri, i, 1700 - 20 * i, BOSCH_A_DIRECT_VREL_MIN_RAW) + published.append([(p.vRel, p.deprecated.measured) for p in rr.points]) + # Same maturity as before (a second sample); until the fit exists (4 samples over 0.25 s) the floor is the bound. + assert published[0] == [] + assert published[1:4] == [[(-12.0, False)]] * 3 + for pts in published[4:]: + assert len(pts) == 1 + assert pts[0][0] == pytest.approx(-20 * BOSCH_A_RANGE_SCALE_M / 0.07) + assert pts[0][1] is False + assert ri._tracks[1].last_trusted_vrel is None + + def test_floor_never_updates_the_trusted_vrel_and_keeps_the_point(self): + ri = make_radar_interface() + for i in range(4): + rr = self._sweep(ri, i, 1000 - 1 * i, 800) + assert rr.points[0].deprecated.measured + trusted = ri._tracks[1].last_trusted_vrel + trusted_nanos = ri._tracks[1].last_trusted_vrel_nanos + assert trusted == pytest.approx((800 - 864) / 72) + for i in range(4, 40): + rr = self._sweep(ri, i, 1000 - 1 * i, BOSCH_A_DIRECT_VREL_MIN_RAW) + assert len(rr.points) == 1, i + assert not rr.points[0].deprecated.measured + assert rr.points[0].vRel != pytest.approx(-12.0) + assert ri._tracks[1].last_trusted_vrel == trusted + assert ri._tracks[1].last_trusted_vrel_nanos == trusted_nanos + # The fit over the floor sweeps' own ranges: -1/16 m per 70 ms. + assert rr.points[0].vRel == pytest.approx(-BOSCH_A_RANGE_SCALE_M / 0.07) + # A real U11 afterwards is measured again at once; the floor never entered the accepted history. + assert len(ri._tracks[1].samples) == 4 + rr = self._sweep(ri, 40, 960, 800) + assert rr.points[0].deprecated.measured + assert rr.points[0].vRel == pytest.approx((800 - 864) / 72) + + def test_fit_is_bounded_at_minus_v_ego(self): + ri = make_radar_interface() + ri.v_ego = 10.0 + for i in range(7): + rr = self._sweep(ri, i, 1700 - 20 * i, BOSCH_A_DIRECT_VREL_MIN_RAW) + assert rr.points[0].vRel == pytest.approx(-10.0) + + def test_one_above_the_floor_is_still_measured(self): + ri = make_radar_interface() + for i in range(3): + rr = self._sweep(ri, i, 1700 - 20 * i, BOSCH_A_DIRECT_VREL_MIN_RAW + 1) + assert rr.points[0].deprecated.measured + assert rr.points[0].vRel == pytest.approx(-12.0 + 1 / 72) + + def test_floor_published_point_survives_its_first_accepted_sample(self): + ri = make_radar_interface() + for i in range(3): + rr = self._sweep(ri, i, 1700 - 20 * i, BOSCH_A_DIRECT_VREL_MIN_RAW) + assert len(rr.points) == 1 + assert len(ri._tracks[1].samples) == 0 + rr = self._sweep(ri, 3, 1640, 0x7FE) # inactive sentinel: no U11, no ratio -> the coast branch + assert len(rr.points) == 1 + assert not rr.points[0].deprecated.measured + assert rr.points[0].vRel == pytest.approx(-12.0) + rr = self._sweep(ri, 4, 1620, 800) # the first accepted sample: not a birth, the point stays + assert len(rr.points) == 1 + assert rr.points[0].deprecated.measured + assert ri._tracks[1].floor_vrel is None + + @pytest.mark.parametrize('floor_on', [True, False]) + def test_range_step_rejected_floor_era_track_stays_published_unmeasured(self, floor_on): + """Route 00000008 track 17 (217.05-223.03 s): a floor-era identity whose range then steps away from the gate's + baseline. The D-054/D-057 gate is unchanged; only its drop outcome is: publish the reported geometry unmeasured.""" + ri = make_radar_interface() + ri.u11_floor_unmeasured = floor_on + for i in range(6): + self._sweep(ri, i, 1600, BOSCH_A_DIRECT_VREL_MIN_RAW) + for i in range(6, 9): + rr = self._sweep(ri, i, 1600, 864) + assert len(rr.points) == 1 and rr.points[0].deprecated.measured + for i in range(9, 20): + rr = self._sweep(ri, i, 1760, 864) # a 10 m step against a U11 of 0: rejected for the whole run + assert ri._tracks[1].rejected_run, i + if floor_on: + assert len(rr.points) == 1, i + assert not rr.points[0].deprecated.measured + if i >= 11: # past BOSCH_A_STALE_S the reported geometry is published, not the stale one + assert rr.points[0].dRel == pytest.approx(1760 * BOSCH_A_RANGE_SCALE_M + BOSCH_A_RANGE_OFFSET_M) + assert rr.points[0].vRel == pytest.approx(0.0, abs=1e-6) # the rejected run's own range fit + if not floor_on: + assert len(rr.points) == 0 # the shipped outcome with the switch off: unchanged + + def test_switch_off_restores_the_measured_floor(self): + ri = make_radar_interface() + ri.u11_floor_unmeasured = False + for i in range(7): + rr = self._sweep(ri, i, 1700 - 20 * i, BOSCH_A_DIRECT_VREL_MIN_RAW) + assert rr.points[0].vRel == pytest.approx(-12.0) + assert rr.points[0].deprecated.measured diff --git a/opendbc/car/honda/tests/test_bosch_a_radar.py b/opendbc/car/honda/tests/test_bosch_a_radar.py index eafd61cd923..04c9e819ee8 100644 --- a/opendbc/car/honda/tests/test_bosch_a_radar.py +++ b/opendbc/car/honda/tests/test_bosch_a_radar.py @@ -10,7 +10,7 @@ from opendbc.car.can_definitions import CanData from opendbc.car.honda.hondacan import CanBus from opendbc.car.honda.interface import CarInterface -from opendbc.car.honda.radar_interface import ( +from opendbc.car.honda.bosch_a_radar import ( BOSCH_A_AZIMUTH_SCALE_RAD, BOSCH_A_AUX_IDS, BOSCH_A_DBC_NAME, @@ -104,7 +104,7 @@ def make_main_frames(slot, frame_idx, status, range_raw, angle_raw, life, track_ def make_radar_interface(): - return CarInterface.RadarInterface(CP, CP_SP) + return CarInterface.RadarInterface(CP, CP_SP).bosch_a def sweep(slot, frame_idx, status, range_raw, angle_raw, life, t_nanos, with_aux=False, aux_frame_idx=None, @@ -164,7 +164,9 @@ class TestDbcBitGeometry: def test_all_slot_range_aliases_match_runtime_scale(self, slot): signal = self.dbc.msgs[BOSCH_A_MAIN_IDS[slot][0]].sigs['RANGE'] assert signal.factor == BOSCH_A_RANGE_SCALE_M == 0.0625 - assert signal.offset == BOSCH_A_RANGE_OFFSET_M == -3.0 + # the DBC offset is documentation only (the parser recomputes range from RANGE_RAW with the per-car ROM offset) + assert signal.offset == -3.0 + assert BOSCH_A_RANGE_OFFSET_M == -346.0 / 128.0 for raw, expected in ((48, 0.0), (208, 10.0), (1000, 59.5), (1648, 100.0), (4094, 252.875)): decoded = get_raw_value(make_f0(range_raw=raw), signal) assert decoded * signal.factor + signal.offset == expected @@ -583,16 +585,16 @@ def test_direct_aux_vrel_is_preferred_over_range_derivative(self): direct_vrel_raw=800, direct_vrel_uncertainty_raw=80)) rr = ri.update(sweep(0, 1, 0x7, 1010, 1024, 3, 50_000_000, with_aux=True, direct_vrel_raw=800, direct_vrel_uncertainty_raw=80)) - assert rr.points[0].vRel == pytest.approx((800 - 864) / 64.0) + assert rr.points[0].vRel == pytest.approx((800 - 864) / 72.0) def test_direct_aux_vrel_domain_and_sentinel(self): - assert _bosch_a_direct_vrel(0) == pytest.approx(-13.5) - assert _bosch_a_direct_vrel(BOSCH_A_DIRECT_VREL_MAX_RAW) == pytest.approx(13.5) + assert _bosch_a_direct_vrel(0) == pytest.approx(-12.0) + assert _bosch_a_direct_vrel(BOSCH_A_DIRECT_VREL_MAX_RAW) == pytest.approx(12.0) assert _bosch_a_direct_vrel(1729) is None assert _bosch_a_direct_vrel(BOSCH_A_DIRECT_VREL_INVALID) is None assert _bosch_a_direct_vrel(0x7FF) is None assert _bosch_a_direct_vrel(None) is None - assert _bosch_a_direct_vrel(0, BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW) == pytest.approx(-13.5) + assert _bosch_a_direct_vrel(0, BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW) == pytest.approx(-12.0) assert _bosch_a_direct_vrel(0, BOSCH_A_DIRECT_VREL_MAX_UNCERTAINTY_RAW + 1) is None assert _bosch_a_direct_vrel(864, 0x3FE) is None assert _bosch_a_direct_vrel(864, 0x3FF) is None @@ -606,7 +608,7 @@ def test_high_u10_live_vrel_without_trust_is_withheld(self): assert len(ri._tracks[1].samples) == 1 def test_fast_clean_range_rate_without_u11_or_ratio_withholds_rather_than_synthesizes(self): - # This range rate (~-46 m/s) is well outside native U11's representable domain ([-13.5, 13.5] + # This range rate (~-46 m/s) is well outside native U11's representable domain ([-12.0, 12.0] # m/s), so it can only ever have come from the raw one-sweep derivative -- which is exactly the # synthesized-measurement hazard this fix closes. With no aux at all (no U11, no ratio) and no # prior trusted velocity to coast, the point is correctly withheld rather than published at a @@ -624,13 +626,13 @@ def test_high_u10_live_vrel_coasts_recent_trusted_motion(self): direct_vrel_raw=696, direct_vrel_uncertainty_raw=84)) rr = ri.update(sweep(0, 1, 0x7, 1724, 1024, 3, 50_000_000, with_aux=True, direct_vrel_raw=696, direct_vrel_uncertainty_raw=84)) - assert rr.points[0].vRel == pytest.approx(-2.625) + assert rr.points[0].vRel == pytest.approx((696 - 864) / 72.0) # R146/S21 ID50's 19-count / 68.962 ms high-U10 step would have synthesized about -15.74 m/s. rr = ri.update(sweep(0, 2, 0x7, 1705, 1024, 5, 118_962_000, with_aux=True, direct_vrel_raw=585, direct_vrel_uncertainty_raw=744)) assert len(rr.points) == 1 assert rr.points[0].dRel == pytest.approx(1705 * BOSCH_A_RANGE_SCALE_M + BOSCH_A_RANGE_OFFSET_M) - assert rr.points[0].vRel == pytest.approx(-2.625) + assert rr.points[0].vRel == pytest.approx((696 - 864) / 72.0) assert not rr.points[0].deprecated.measured assert len(ri._tracks[1].samples) == 2 @@ -644,9 +646,9 @@ def test_qualified_u11_recovers_immediately_after_high_u10_coast(self): direct_vrel_raw=585, direct_vrel_uncertainty_raw=744)) rr = ri.update(sweep(0, 3, 0x7, 980, 1024, 7, 150_000_000, with_aux=True, direct_vrel_raw=760, direct_vrel_uncertainty_raw=84)) - assert rr.points[0].vRel == pytest.approx((760 - 864) / 64.0) + assert rr.points[0].vRel == pytest.approx((760 - 864) / 72.0) assert rr.points[0].deprecated.measured - assert ri._tracks[1].last_trusted_vrel == pytest.approx((760 - 864) / 64.0) + assert ri._tracks[1].last_trusted_vrel == pytest.approx((760 - 864) / 72.0) def test_high_u10_coast_cannot_cross_lifecycle_incarnation(self): ri = make_radar_interface() @@ -669,7 +671,11 @@ def test_high_u10_coast_expires_with_trusted_motion_age(self): direct_vrel_raw=585, direct_vrel_uncertainty_raw=744)) rr = ri.update(sweep(0, 3, 0x7, 980, 1024, 7, 300_000_000, with_aux=True, direct_vrel_raw=585, direct_vrel_uncertainty_raw=744)) - assert len(rr.points) == 0 + # Testing's D-041/D-042: a stale trusted velocity no longer deletes the point; the geometry keeps + # publishing with the held velocity, flagged measured=False. + assert len(rr.points) == 1 + assert rr.points[0].vRel == pytest.approx((800 - 864) / 72.0) + assert not rr.points[0].deprecated.measured def test_range_ratio_conversion_and_sentinel(self): assert _bosch_a_range_ratio(500) == pytest.approx(1.0) @@ -680,7 +686,7 @@ def test_range_ratio_conversion_and_sentinel(self): @pytest.mark.parametrize( ('direct_raw', 'range_raw', 'rawca', 'expected'), - [(0, 974, 528, -13.5), (BOSCH_A_DIRECT_VREL_MAX_RAW, 1027, 472, 13.5)], + [(0, 974, 528, -12.0), (BOSCH_A_DIRECT_VREL_MAX_RAW, 1027, 472, 12.0)], ) def test_range_ratio_does_not_extend_direct_vrel_rails(self, direct_raw, range_raw, rawca, expected): ri = make_radar_interface() @@ -1114,7 +1120,7 @@ def test_bosch_a_observed_sweep_end_and_publish_trigger_are_distinct(): def test_bosch_a_timing_contract(): - assert BOSCH_A_FREQ_HZ == 15 + assert BOSCH_A_FREQ_HZ == pytest.approx(14.35) assert BOSCH_A_STALE_S == pytest.approx(0.20) @@ -1122,9 +1128,10 @@ def test_bosch_a_timing_contract(): def test_civic_bosch_radar_dbc_wired_and_available(): assert CP.radarUnavailable is False - ri = make_radar_interface() + ri = CarInterface.RadarInterface(CP, CP_SP) assert ri.bosch_a_radar is True - assert ri.rcp is not None + assert ri.bosch_a is not None + assert ri.rcp is ri.bosch_a.rcp def test_civic_bosch_object_feed_uses_camera_side_acc_can(): diff --git a/opendbc/car/honda/tests/test_bosch_a_radar_interface.py b/opendbc/car/honda/tests/test_bosch_a_radar_interface.py new file mode 100644 index 00000000000..2fe9ff1eedc --- /dev/null +++ b/opendbc/car/honda/tests/test_bosch_a_radar_interface.py @@ -0,0 +1,147 @@ +"""Radar-interface tests ported from Honda-PTM-Testing (tests/test_bosch_a_radar.py). + +Left out: the card-startup/HondaBoschARadar param test and the ALT_RADAR_STOCK (stock_aol) set checks, which depend on +StarPilot-only modules this repo does not have. The nrdr RadarInterface is still selected by DBC name + CP.radarUnavailable. +""" +import pytest + +from opendbc.car import gen_empty_fingerprint, structs +from opendbc.car.can_definitions import CanData +from opendbc.car.honda.bosch_a_radar import VERIFIED_BOSCH_A_CARS, bosch_a_range_offset_m, bosch_a_radar_platform +from opendbc.car.honda.hondacan import CanBus +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.radar_interface import RadarInterface +from opendbc.car.honda.values import CAR +from opendbc.sunnypilot.car.tests.runtime_config import make_test_car_config + +BOSCH_A_DIRECT_VREL_INVALID = 0x7FE +RADAR_CONFIG = make_test_car_config(bosch_a_radar=True) +CP_SP = structs.CarParamsSP() + + +def make_f0(frame_idx=0, status=7, range_raw=0, angle_raw=0, range_sigma_raw=1): + B0 = (range_sigma_raw & 127) << 1 + B3 = frame_idx & 15 | (range_raw & 15) << 4 + B2 = range_raw >> 4 & 255 + B1 = (status & 15) << 4 + B5 = (angle_raw & 7) << 5 + B4 = angle_raw >> 3 & 255 + return bytes([B0, B1, B2, B3, B4, B5, 0, 0]) + +def make_f1(frame_idx=0, existence_raw=126): + return bytes([0, 0, 0, (frame_idx & 15) << 1, 0, existence_raw & 127, 0, 0]) + +def make_f2(frame_idx=0, life=0): + B1 = frame_idx & 15 | (life & 15) << 4 + B0 = life >> 4 & 255 + return bytes([B0, B1, 0, 0, 0, 0, 0, 0]) + +def make_f3(frame_idx=0, edge_a_raw=0, edge_b_raw=0, sigma_a_raw=0, track_id=255): + B0 = edge_a_raw >> 3 & 255 + B1 = (edge_a_raw & 7) << 5 | (frame_idx & 15) << 1 + B2 = edge_b_raw >> 3 & 255 + B3 = (edge_b_raw & 7) << 5 + B4 = sigma_a_raw >> 2 & 255 + B5 = (sigma_a_raw & 3) << 6 + return bytes([B0, B1, B2, B3, B4, B5, track_id & 255, 0]) + +def make_aux(frame_idx=0, rawc9=0, rawca=0, direct_vrel_raw=BOSCH_A_DIRECT_VREL_INVALID, direct_vrel_uncertainty_raw=1023): + B0 = direct_vrel_raw >> 3 & 255 + B1 = (direct_vrel_raw & 7) << 5 | (frame_idx & 15) << 1 + B2 = direct_vrel_uncertainty_raw >> 2 & 255 + B3 = (direct_vrel_uncertainty_raw & 3) << 6 + B5 = (rawc9 & 3) << 6 + B4 = rawc9 >> 2 & 255 + B7 = (rawca & 3) << 6 + B6 = rawca >> 2 & 255 + return bytes([B0, B1, B2, B3, B4, B5, B6, B7]) + + +def sweep(bus, index, timestamp, with_aux=True, status=7): + frames = [] + for slot in range(16): + base = 0x280 + 4 * slot if slot < 4 else 0x2D0 + 4 * (slot - 4) + valid = slot == 0 + frames.extend((CanData(base, make_f0(index, status if valid else 15, 1000, 1024), bus), + CanData(base + 1, make_f1(index), bus), + CanData(base + 2, make_f2(index, 1 + index * 2), bus), + CanData(base + 3, make_f3(index, track_id=1 if valid else 255), bus))) + if with_aux: + address = 0x2C8 + slot if slot < 8 else 0x290 + slot - 8 + frames.append(CanData(address, make_aux(index, direct_vrel_raw=864, direct_vrel_uncertainty_raw=0), bus)) + return [(timestamp, frames)] + + +@pytest.mark.parametrize("identity", (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G)) +def test_actual_parser_points_silence_retires_and_new_track_recovers(identity): + cp = CarInterface.get_params(identity, gen_empty_fingerprint(), [], False, False, False, RADAR_CONFIG) + assert not cp.radarUnavailable + ri = RadarInterface(cp, CP_SP) + bus = CanBus(cp).camera + first = ri.update(sweep(bus, 0, 1_000_000_000)) + assert first is not None and not first.points + second = ri.update(sweep(bus, 1, 1_066_666_667)) + assert not second.errors.canError + assert len(second.points) == 1 + # firmware range: raw/16 minus the car's ROM offset n0/128 (Civic 346, CR-V 341) + assert second.points[0].dRel == pytest.approx(1000 / 16 + bosch_a_range_offset_m(identity)) + assert second.points[0].yRel == pytest.approx(0.0) + stale = ri.update([(1_400_000_000, [])]) + assert stale is not None and not stale.points and stale.errors.radarUnavailableTemporary + assert not ri.update(sweep(bus, 2, 1_466_666_667)).points + assert len(ri.update(sweep(bus, 3, 1_533_333_334)).points) == 1 + + +@pytest.mark.parametrize("identity", (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G)) +def test_point_status_is_the_status_nibble_of_its_latest_observation(identity): + # route 34 T479: the phantom lead's track was STATUS 9 for its first ~4 s, then 7 + cp = CarInterface.get_params(identity, gen_empty_fingerprint(), [], False, False, False, RADAR_CONFIG) + ri = RadarInterface(cp, CP_SP) + bus = CanBus(cp).camera + assert ri.point_statuses == {} + ri.update(sweep(bus, 0, 1_000_000_000, status=9)) + unconfirmed = ri.update(sweep(bus, 1, 1_066_666_667, status=9)) + assert ri.point_statuses == {p.trackId: 9 for p in unconfirmed.points} and len(unconfirmed.points) == 1 + confirmed = ri.update(sweep(bus, 2, 1_133_333_334)) + assert ri.point_statuses == {p.trackId: 7 for p in confirmed.points} and len(confirmed.points) == 1 + + +def test_point_status_is_none_without_a_bosch_a_radar(): + cp = CarInterface.get_params(CAR.HONDA_CIVIC, gen_empty_fingerprint(), [], False, False, False, RADAR_CONFIG) + assert RadarInterface(cp, CP_SP).point_statuses is None + + +@pytest.mark.parametrize("identity", (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G)) +def test_wrong_bus_cannot_create_radar_points(identity): + cp = CarInterface.get_params(identity, gen_empty_fingerprint(), [], False, False, False, RADAR_CONFIG) + ri = RadarInterface(cp, CP_SP) + for index in range(3): + assert ri.update(sweep(CanBus(cp).camera + 1, index, 1_000_000_000 + index * 66_666_667)) is None + assert not ri.bosch_a.pts + + +def test_verified_bosch_a_cars_are_all_platform_cars(): + expected = frozenset(c for c in CAR if bosch_a_radar_platform(c)) + assert VERIFIED_BOSCH_A_CARS == expected + assert CAR.ACURA_RDX_3G in VERIFIED_BOSCH_A_CARS + assert CAR.HONDA_ACCORD in VERIFIED_BOSCH_A_CARS + assert CAR.HONDA_CIVIC_2022 not in VERIFIED_BOSCH_A_CARS + + +@pytest.mark.parametrize("identity", (CAR.HONDA_CIVIC_2022, CAR.HONDA_ACCORD_11G, CAR.HONDA_HRV_3G, CAR.ACURA_MDX_4G_MMR)) +def test_non_bosch_a_platform_factory_has_no_radar_decoder(identity): + cp = CarInterface.get_params(identity, gen_empty_fingerprint(), [], False, False, False, RADAR_CONFIG) + assert cp.radarUnavailable + assert RadarInterface(cp, CP_SP).rcp is None + + +@pytest.mark.parametrize("candidate", (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G)) +def test_selected_bosch_a_direct_update_uses_its_actual_protocol(candidate): + cp = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], False, False, False, RADAR_CONFIG) + cp.radarUnavailable = False + radar = RadarInterface(cp, CP_SP) + assert radar.bosch_a is not None + assert radar.update([]) is None + result = radar._update([radar.trigger_msg]) + assert result.errors.canError + assert not result.points