From 461f81cddfa96be4fbf2b9c01e821790f8110f27 Mon Sep 17 00:00:00 2001 From: Peter Nguyen Date: Sun, 8 Nov 2026 13:37:35 -0600 Subject: [PATCH 1/2] 2026-11-08 honda: port the current Bosch-A radar parser from Honda-PTM-Testing Moves the Bosch-A decoding into a new bosch_a_radar.py (BoschARadar), copied from Biscuit-Labs Honda-PTM-Testing. RadarInterface(CP, CP_SP) now delegates the Bosch-A path to it and keeps only the Nidec path and nrdr's selection (DBC name plus radarUnavailable). The old inline Bosch-A code is deleted. point_statuses and a settable v_ego are exposed on the wrapper. Behavior now matches Testing: per-car ROM range offset, U11 at 1/72 m/s, 14.35 Hz, life-counter saturation, rail bounds, coast bounds including reversing coast, re-anchor/innovation, vrel-rate check, raw U11 0 = unmeasured. Testing's former toggles are hard-coded to its current defaults; no Params reads, no DBC changes. Tests: ported Testing's decoder tests (139) and radar-interface tests (the portable ones, 14 incl. parametrized); nrdr's 82 tests kept with expectations updated where Testing intentionally changed them (1/72 U11, 14.35 Hz, -346/128 offset, stale velocity no longer deletes the point). Evidence is static tests only; nothing here is road- or replay-validated. Co-Authored-By: Claude Opus 5.5 --- opendbc/car/honda/bosch_a_radar.py | 1458 +++++++++++ opendbc/car/honda/radar_interface.py | 613 +---- .../car/honda/tests/test_bosch_a_decoder.py | 2329 +++++++++++++++++ opendbc/car/honda/tests/test_bosch_a_radar.py | 41 +- .../tests/test_bosch_a_radar_interface.py | 147 ++ 5 files changed, 3979 insertions(+), 609 deletions(-) create mode 100644 opendbc/car/honda/bosch_a_radar.py create mode 100644 opendbc/car/honda/tests/test_bosch_a_decoder.py create mode 100644 opendbc/car/honda/tests/test_bosch_a_radar_interface.py 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 From 427f1986ed241f111d106cc72cdefbda5efe5f7f Mon Sep 17 00:00:00 2001 From: Peter Nguyen Date: Sun, 8 Nov 2026 15:42:54 -0600 Subject: [PATCH 2/2] 2026-11-08 Bosch-A longitudinal: Testing's actuator modules on Bosch-A cars Ports bosch_longitudinal (BoschABrakeEdge), brake_onset, bosch_low_speed_brake and bosch_gas_learner from Biscuit-Labs Honda-PTM-Testing. On qualifying Bosch-A cars they replace nrdr's gas/wind factors and Bosch extra-brake controller. The Civic gas learner (HONDA_CIVIC_BOSCH only, as on Testing) also replaces MVL's learner when MVL is on. The stop counter on this path follows Testing (stopping state alone). Static evidence only. Co-Authored-By: Claude Opus 5.5 --- opendbc/car/honda/bosch_gas_learner.py | 134 ++++++++ opendbc/car/honda/bosch_longitudinal.py | 300 ++++++++++++++++++ opendbc/car/honda/bosch_low_speed_brake.py | 71 +++++ opendbc/car/honda/brake_onset.py | 61 ++++ opendbc/car/honda/carcontroller.py | 137 ++++---- opendbc/car/honda/hondacan.py | 13 +- opendbc/car/honda/mvl_longitudinal.py | 72 +++-- .../honda/tests/test_bosch_a_long_wiring.py | 108 +++++++ .../car/honda/tests/test_bosch_gas_learner.py | 151 +++++++++ .../honda/tests/test_bosch_longitudinal.py | 239 ++++++++++++++ .../honda/tests/test_bosch_low_speed_brake.py | 135 ++++++++ .../car/honda/tests/test_bosch_stopping.py | 7 + opendbc/car/honda/tests/test_brake_onset.py | 126 ++++++++ 13 files changed, 1462 insertions(+), 92 deletions(-) create mode 100644 opendbc/car/honda/bosch_gas_learner.py create mode 100644 opendbc/car/honda/bosch_longitudinal.py create mode 100644 opendbc/car/honda/bosch_low_speed_brake.py create mode 100644 opendbc/car/honda/brake_onset.py create mode 100644 opendbc/car/honda/tests/test_bosch_a_long_wiring.py create mode 100644 opendbc/car/honda/tests/test_bosch_gas_learner.py create mode 100644 opendbc/car/honda/tests/test_bosch_longitudinal.py create mode 100644 opendbc/car/honda/tests/test_bosch_low_speed_brake.py create mode 100644 opendbc/car/honda/tests/test_brake_onset.py diff --git a/opendbc/car/honda/bosch_gas_learner.py b/opendbc/car/honda/bosch_gas_learner.py new file mode 100644 index 00000000000..9d21324ec7f --- /dev/null +++ b/opendbc/car/honda/bosch_gas_learner.py @@ -0,0 +1,134 @@ +"""Civic Bosch live gas learner, ported from openpilot-radar ns-bosch-radar-testing (LongGasLearner). + +What it adds over the shared star7 learner in bosch_longitudinal.py: + - learns against the command from 0.5 s ago, not the current one (the gas pedal lags); + - learns only on quasi-steady commands (|d accel| < 0.3 m/s^3) on flat road (|pitch - 0.013| < 0.02 rad); + - hard range 0.6-1.6, decays back toward 1.0 at 0.01/min while outside 0.8-1.25; + - the applied factor follows the learned one through a 7.5 s filter; + - the gas factor scales the flat-road request only, anchored at the gas-on threshold, and the hill term is added + on top with a 1.1 gain about the mount's 0.013 rad pitch bias. + +Tuned on ns against the Civic gas table [0, 2] -> [0, 1600]. star7 keeps its own Civic table [-0.2, 2] -> [0, 750] +(Peter's call, 2026-10-22). That pairing has never been driven. Evidence here: static and unit tests only. +Not ported: the starpilotCarState.gasLearner* log fields (they need cereal changes). +""" +import math +from collections import deque + +import numpy as np + +from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, DT_CTRL + +# Bump when the learner's meaning changes, so saved factors from older math are dropped once. +LEARN_VERSION = 1 + +LEARNER_DT = 2 * DT_CTRL # update() runs on the 50 Hz gas frames +LAG_TICKS = 25 # 0.5 s +ACCEL_RATE_THRESH = 0.3 # m/s^3 + +HARD_LO = 0.6 +HARD_HI = 1.6 +SOFT_LO = 0.8 +SOFT_HI = 1.25 +DECAY_PER_TICK = 0.01 / 60.0 * LEARNER_DT + +FACTOR_FILTER_RC = 7.5 +FACTOR_FILTER_ALPHA = LEARNER_DT / (FACTOR_FILTER_RC + LEARNER_DT) + +PITCH_DEADBAND = 0.02 # rad +CIVIC_PITCH_BIAS = 0.013 # rad; this device mount, 18/18 ns routes +CIVIC_HILL_GAS_GAIN = 1.1 # ns: offline fit said 1.2, shipped 1.1 after the hill sim doubled crest overshoot at 1.2 +CIVIC_GAS_LEARN_SPEED = 150.0 # the Civic gas pedal reacts slowly +WIND_LEARN_SPEED = 1000.0 + + +def safe_clamp(v: float) -> float: + if not math.isfinite(v): + return 1.0 + return float(np.clip(v, HARD_LO, HARD_HI)) + + +def decay_toward_nominal(v: float) -> float: + if v < SOFT_LO or v > SOFT_HI: + v = min(1.0, v + DECAY_PER_TICK) if v < 1.0 else max(1.0, v - DECAY_PER_TICK) + return v + + +def gas_lookup_accel(gas_pedal_force: float, hill_brake: float, gasfactor: float, min_gas: float) -> float: + """Input to the gas table. The factor scales the flat-road offset from the gas-on threshold; the hill term is + added unscaled, with the Civic hill gain about the pitch bias so flat-road gas is unchanged.""" + hill_level = math.sin(CIVIC_PITCH_BIAS) * ACCELERATION_DUE_TO_GRAVITY + return ((gas_pedal_force - hill_brake - min_gas) * gasfactor + min_gas + hill_brake + + (CIVIC_HILL_GAS_GAIN - 1.0) * (hill_brake - hill_level)) + + +class CivicGasLearner: + def __init__(self, gasfactor: float = 1.0, windfactor: float = 1.0): + self.load(gasfactor, windfactor) + self._accel_deque: deque = deque(maxlen=LAG_TICKS + 1) + self._was_engaged = False + self.learning = False + self.last_gas_error = 0.0 + + def load(self, gasfactor: float, windfactor: float): + """Saved values start both the learned and the applied factor, so there is no startup transient.""" + self.raw_gasfactor = self.gasfactor = safe_clamp(gasfactor) + self.raw_windfactor = self.windfactor = safe_clamp(windfactor) + self.gasfactor_before_maxgas = self.raw_gasfactor + self.windfactor_before_maxgas = self.raw_windfactor + self.windfactor_before_brake = self.raw_windfactor + + def _reset_deque(self, accel_cmd: float): + self._accel_deque.clear() + self._accel_deque.extend([accel_cmd] * (LAG_TICKS + 1)) + + def update(self, accel_cmd: float, a_ego: float, gas_pedal_force: float, wind_brake_ms2: float, engaged: bool, + gas_pressed: bool, brake_pressed: bool, v_ego: float, pitch: float, at_accel_max: bool): + self.learning = False + if (engaged and not self._was_engaged) or gas_pressed: + self._reset_deque(accel_cmd) + self._was_engaged = engaged + self._accel_deque.append(accel_cmd) + + if engaged and not gas_pressed and not brake_pressed and v_ego > 0.0 and len(self._accel_deque) == LAG_TICKS + 1: + lagged = self._accel_deque[0] + quasi_steady = abs(self._accel_deque[-1] - lagged) / (LAG_TICKS * LEARNER_DT) < ACCEL_RATE_THRESH + if quasi_steady and abs(pitch - CIVIC_PITCH_BIAS) < PITCH_DEADBAND: + self.learning = True + gas_error = lagged - a_ego + self.last_gas_error = float(gas_error) + if gas_error != 0.0 and gas_pedal_force > 0.0: + self.raw_gasfactor = float(np.clip(self.raw_gasfactor + gas_error / CIVIC_GAS_LEARN_SPEED * gas_pedal_force, + HARD_LO, HARD_HI)) + if gas_error != 0.0: + wind_adjust = 1.0 + wind_brake_ms2 / WIND_LEARN_SPEED + self.raw_windfactor = float(np.clip(self.raw_windfactor * (wind_adjust if gas_error > 0.0 else 1.0 / wind_adjust), + HARD_LO, HARD_HI)) + + # Anti-windup: braking never lowers the wind factor; at the accel limit neither factor may rise. + if gas_pedal_force <= 0.0: + self.raw_windfactor = max(self.raw_windfactor, self.windfactor_before_brake) + else: + self.windfactor_before_brake = self.raw_windfactor + if at_accel_max: + self.raw_gasfactor = max(HARD_LO, min(self.raw_gasfactor, self.gasfactor_before_maxgas) - DECAY_PER_TICK) + self.raw_windfactor = min(self.raw_windfactor, self.windfactor_before_maxgas) + else: + self.gasfactor_before_maxgas = self.raw_gasfactor + self.windfactor_before_maxgas = self.raw_windfactor + + if not math.isfinite(self.raw_gasfactor): + self.raw_gasfactor = self.gasfactor_before_maxgas = 1.0 + if not math.isfinite(self.raw_windfactor): + self.raw_windfactor = self.windfactor_before_maxgas = self.windfactor_before_brake = 1.0 + + self.raw_gasfactor = safe_clamp(decay_toward_nominal(self.raw_gasfactor)) + self.raw_windfactor = safe_clamp(decay_toward_nominal(self.raw_windfactor)) + + self.gasfactor = FACTOR_FILTER_ALPHA * self.raw_gasfactor + (1.0 - FACTOR_FILTER_ALPHA) * self.gasfactor + self.windfactor = FACTOR_FILTER_ALPHA * self.raw_windfactor + (1.0 - FACTOR_FILTER_ALPHA) * self.windfactor + if not math.isfinite(self.gasfactor): + self.gasfactor = 1.0 + if not math.isfinite(self.windfactor): + self.windfactor = 1.0 + return self.gasfactor, self.windfactor diff --git a/opendbc/car/honda/bosch_longitudinal.py b/opendbc/car/honda/bosch_longitudinal.py new file mode 100644 index 00000000000..422828b825f --- /dev/null +++ b/opendbc/car/honda/bosch_longitudinal.py @@ -0,0 +1,300 @@ +import copy +import math + +import numpy as np + +from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, DT_CTRL, structs +from opendbc.car.honda.bosch_a_radar import VERIFIED_BOSCH_A_CARS +from opendbc.car.honda.brake_onset import BrakeOnsetCompensation +from opendbc.car.honda.bosch_low_speed_brake import LowSpeedBrakeAssist +from opendbc.car.honda.bosch_gas_learner import CivicGasLearner, LEARN_VERSION, gas_lookup_accel +from opendbc.car.honda.values import CAR, HondaFlags + +LongCtrlState = structs.CarControl.Actuators.LongControlState +BOSCH_BRAKE_FORCE_ON = -0.12 +BOSCH_BRAKE_FORCE_RELEASE = -0.02 +# Bosch-A stop-and-go brake cycling, measured on the Civic (STATUS 263/265, route 0000001f--39b2448f7d). At 2-4 m/s with a command of +# -0.1..-0.3 the brake request toggled 15-25 times a minute: brake-on cut the gas from ~30-40 to 0, and every release +# jumped the gas straight to the 60-unit cap. Replay of this class on 1f's logged command, open loop, 50 Hz, with +# long-control drops resetting it, against the old path on the same harness (the state machine reproduces the +# logged BRAKE_REQUEST on 90.4 % of frames; no closed-loop gap or jerk numbers): +# BOSCH_A_BRAKE_FORCE_ON: brake request only below -0.20, the bottom of the gas lookup, so the band between it and +# -0.12 coasts with the gas off instead of braking. The only change that cuts toggles: 3.69 -> 3.17 per minute +# (an earlier sim that ignored long-control drops said 2.11). +# BOSCH_A_BRAKE_MIN_HOLD_S: once on, the request stays on this long unless the command asks for real acceleration +# (above BOSCH_A_BRAKE_HOLD_ESCAPE). Holds under 1.5 s 27.6 % -> 16.7 %; the rest are long-control drops and the +# escape (about 0.2 per minute, each shortening a hold by 0.04-0.4 s). +# BOSCH_A_RELEASE_GAS_START / _RAMP_S: after a release the gas starts at most at 35 and reaches the lookup over 1 s, +# instead of the 60 cap. Gas on the release frame 60 -> 35; mean over the first second 121 -> 81. +# BOSCH_A_LAUNCH_GAS_RAMP_S: a release straight out of stopping (a launch from a stop, not a stop-and-go brake cycle) +# ramps over 0.5 s instead. Route 0000007c--0ea998a750 16:59 (owner bookmark, "didn't want to move, I tapped the +# gas"): openpilot released and sent gas 36 -> 232 over 0.8 s with no motion; clean launches on 7c/72/75/76 take +# 0.9-1.7 s from release to motion. Replay cannot model the car's standstill release, so the gain is unmeasured: +# owner-approved road test 2026-11-03. The brake-cycling releases above keep the 1 s ramp. +# Stopping still forces the request on, and a long-control drop still clears it, exactly as before. +# Runs on every car with the Bosch-A radar harness (bosch_a_harness), owner 2026-10-24: the CR-V 5G is static only. +BOSCH_A_BRAKE_FORCE_ON = -0.20 +BOSCH_A_BRAKE_MIN_HOLD_S = 1.5 +BOSCH_A_BRAKE_HOLD_ESCAPE = 0.2 +BOSCH_A_RELEASE_GAS_START = 35.0 +BOSCH_A_RELEASE_GAS_RAMP_S = 1.0 +BOSCH_A_LAUNCH_GAS_RAMP_S = 0.5 + +CLASSIC_BOSCH_CARS = frozenset(( + CAR.HONDA_NBOX_2G, + CAR.HONDA_ACCORD, + CAR.HONDA_CIVIC_BOSCH, + CAR.HONDA_CIVIC_BOSCH_DIESEL, + CAR.HONDA_CIVIC_2022, + CAR.HONDA_CRV_5G, + CAR.HONDA_CRV_HYBRID, + CAR.HONDA_HRV_3G, + CAR.HONDA_CITY_7G, + CAR.ACURA_RDX_3G, + CAR.ACURA_RDX_3G_MMR, + CAR.HONDA_INSIGHT, + CAR.HONDA_E, + CAR.HONDA_E_ADVANCE, + CAR.ACURA_MDX_4G, + CAR.HONDA_ODYSSEY_5G_MMR, + CAR.ACURA_TLX_2G, + CAR.HONDA_FIT_4G, + CAR.ACURA_INTEGRA, + CAR.ACURA_ADX, +)) +HONDA_BOSCH_RADARLESS = frozenset(( + CAR.HONDA_CIVIC_2022, + CAR.HONDA_HRV_3G, + CAR.HONDA_CITY_7G, + CAR.HONDA_FIT_4G, + CAR.ACURA_INTEGRA, + CAR.ACURA_ADX, +)) + +def update_honda_bosch_braking(braking: bool, gas_pedal_force: float, stopping: bool, long_active: bool) -> bool: + if not long_active: + return False + if stopping: + return True + if braking: + return gas_pedal_force <= BOSCH_BRAKE_FORCE_RELEASE + return gas_pedal_force < BOSCH_BRAKE_FORCE_ON + +class BoschABrakeEdge: + """Bosch-A harness brake request with a deeper on threshold, a minimum hold and a gas ramp after release.""" + def __init__(self, dt: float): + self.dt = dt + self.reset() + + def reset(self): + self.braking = False + self.hold_s = 0.0 + self.release_s = math.inf + self.stopping = False + self.ramp_s = BOSCH_A_RELEASE_GAS_RAMP_S + + def update(self, gas_pedal_force: float, stopping: bool, long_active: bool) -> bool: + if not long_active: + self.reset() + return False + was_braking = self.braking + was_stopping = self.stopping + self.stopping = stopping + if stopping: + self.braking = True + elif self.braking: + held = self.hold_s < BOSCH_A_BRAKE_MIN_HOLD_S and gas_pedal_force <= BOSCH_A_BRAKE_HOLD_ESCAPE + self.braking = held or gas_pedal_force <= BOSCH_BRAKE_FORCE_RELEASE + else: + self.braking = gas_pedal_force < BOSCH_A_BRAKE_FORCE_ON + if self.braking: + self.hold_s = self.hold_s + self.dt if was_braking else 0.0 + self.release_s = math.inf + else: + if was_braking: + self.release_s = 0.0 + self.ramp_s = BOSCH_A_LAUNCH_GAS_RAMP_S if was_stopping else BOSCH_A_RELEASE_GAS_RAMP_S + else: + self.release_s += self.dt + return self.braking + + def limit_gas(self, gas: float) -> float: + """Gas after a brake release rises from BOSCH_A_RELEASE_GAS_START to the lookup over ramp_s (shorter for a launch).""" + if self.braking or self.release_s >= self.ramp_s or gas <= BOSCH_A_RELEASE_GAS_START: + return gas + return BOSCH_A_RELEASE_GAS_START + (gas - BOSCH_A_RELEASE_GAS_START) * self.release_s / self.ramp_s + + +def get_honda_bosch_wind_brake_mps2(v_ego: float) -> float: + return float(np.interp(v_ego, [0.0, 13.4, 22.4, 31.3, 40.2], [0.0, 0.049, 0.136, 0.267, 0.441])) + +def bosch_a_harness(cp): + """A verified classic Bosch car running the Bosch-A radar harness (same car gate as BoschARadar).""" + return (cp.carFingerprint in VERIFIED_BOSCH_A_CARS and not cp.radarUnavailable and + not cp.flags & (HondaFlags.BOSCH_RADARLESS | HondaFlags.BOSCH_CANFD | HondaFlags.BOSCH_ALT_RADAR)) + +def qualified(cp): + return (cp.brand == "honda" and cp.carFingerprint in CLASSIC_BOSCH_CARS and cp.flags & HondaFlags.BOSCH and + not cp.flags & HondaFlags.BOSCH_CANFD and cp.openpilotLongitudinalControl and + not cp.passive and not cp.dashcamOnly and not cp.notCar and + bool(cp.flags & HondaFlags.BOSCH_RADARLESS) == (cp.carFingerprint in HONDA_BOSCH_RADARLESS)) + + +# Port note (nrdr): star7's CarControllerParams gives Bosch cars the gas table [-0.2, 2.0] -> [0, gas_max] +# (values.py CarControllerParams.__init__ in Honda-PTM-Testing). nrdr's table is [0, 2.0] -> [0, 1600] and is shared with +# its non-Bosch-A paths, so BoschLongitudinal gets its own copy of the params with star7's table. Numbers are star7's. +STAR7_GAS_LOOKUP_BP = [-0.2, 2.0] +STAR7_GAS_MAX = {CAR.HONDA_CIVIC_BOSCH: 750, CAR.HONDA_ODYSSEY_5G_MMR: 2000, CAR.ACURA_RDX_3G_MMR: 2000, CAR.ACURA_RDX_3G: 2200} +STAR7_GAS_MAX_DEFAULT = 1600 + + +def star7_params(cp, params): + """A copy of the controller params carrying star7's Bosch gas table for this car.""" + p = copy.copy(params) + p.BOSCH_GAS_LOOKUP_BP = list(STAR7_GAS_LOOKUP_BP) + p.BOSCH_GAS_LOOKUP_V = [0, STAR7_GAS_MAX.get(cp.carFingerprint, STAR7_GAS_MAX_DEFAULT)] + return p + + +class BoschLongitudinal: + def __init__(self, cp, params): + if not qualified(cp): + raise ValueError("Honda road load requires its admitted classic Bosch profile") + self.CP = cp + self.params = params + self.pitch = 0.0 + self.accel = 0.0 + self.gas = 0.0 + self.bosch_braking = False + self.bosch_last_gas = 0.0 + # Civic Bosch runs the ns live gas learner (bosch_gas_learner.py); every other car keeps the shared learner below. + self.civic_learner = CivicGasLearner() if cp.carFingerprint == CAR.HONDA_CIVIC_BOSCH else None + self.learner_version = LEARN_VERSION if self.civic_learner is not None else None + # Bosch-A harness cars only (owner 2026-10-24). The brake_onset.py fit and the edge numbers are from the Civic; + # on the CR-V 5G they are static only. update() runs every other control frame. + bosch_a = bosch_a_harness(cp) + self.brake_onset = BrakeOnsetCompensation(2 * DT_CTRL) if bosch_a else None + self.brake_edge = BoschABrakeEdge(2 * DT_CTRL) if bosch_a else None + self.low_speed_brake = LowSpeedBrakeAssist(2 * DT_CTRL) if bosch_a else None + self.set_factors(1.0, 1.0) + + def set_factors(self, gas, wind): + if self.civic_learner is not None: + self.civic_learner.load(gas, wind) + self.bosch_gas_factor = self.civic_learner.raw_gasfactor + self.bosch_wind_factor = self.civic_learner.raw_windfactor + return + self.bosch_gas_factor = float(gas) if math.isfinite(gas) and 0.01 <= gas <= 3.0 else 1.0 + self.bosch_wind_factor = float(wind) if math.isfinite(wind) and 0.1 <= wind <= 5.0 else 1.0 + self.bosch_wind_factor_before_brake = self.bosch_wind_factor + self.bosch_gas_factor_before_gasmax = self.bosch_gas_factor + self.bosch_wind_factor_before_gasmax = self.bosch_wind_factor + + def observe_orientation(self, orientation): + if len(orientation) == 3 and math.isfinite(orientation[1]): + self.pitch = orientation[1] + + def update(self, CC, CS, accel): + actuators = CC.actuators + hill_brake = math.sin(self.pitch) * ACCELERATION_DUE_TO_GRAVITY + wind_brake_mps2 = get_honda_bosch_wind_brake_mps2(CS.out.vEgo) + min_gas = self.params.BOSCH_GAS_LOOKUP_BP[0] + self.accel = float(np.clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX)) + target = self.accel + if self.brake_onset is not None: + self.accel = self.brake_onset.update(self.accel, active=bool( + CC.longActive and actuators.longControlState == LongCtrlState.pid and + not CS.out.gasPressed and not CS.out.brakePressed)) + if self.low_speed_brake is not None: + lcs = actuators.longControlState + self.accel = self.low_speed_brake.update(target, self.accel, CS.out.aEgo, CS.out.vEgo, + active=bool(CC.longActive and lcs in (LongCtrlState.pid, LongCtrlState.stopping) and + not CS.out.gasPressed and not CS.out.brakePressed), + stopping=lcs == LongCtrlState.stopping) + self.accel = float(np.clip(self.accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX)) + if self.civic_learner is not None: + return self._update_civic(CC, CS, hill_brake, wind_brake_mps2, min_gas) + gas_pedal_force = self.accel + hill_brake + if self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS: + gas_pedal_force += wind_brake_mps2 * self.bosch_wind_factor + if actuators.longControlState == LongCtrlState.pid and (not CS.out.gasPressed): + gas_error = self.accel - CS.out.aEgo + if gas_error != 0.0 and gas_pedal_force > min_gas: + if self.CP.carFingerprint == CAR.HONDA_INSIGHT: + gas_learn_speed = 150.0 + elif self.CP.carFingerprint in (CAR.ACURA_RDX_3G, CAR.ACURA_RDX_3G_MMR): + gas_learn_speed = 300.0 + else: + gas_learn_speed = 50.0 + gas_learn_force = gas_pedal_force + self.bosch_gas_factor = float(np.clip(self.bosch_gas_factor + gas_error / gas_learn_speed * gas_learn_force, 0.1, 3.0)) + if gas_error != 0.0 and (not CS.out.brakePressed) and (CS.out.vEgo > 0.0): + wind_learn_speed = 100.0 if self.CP.carFingerprint in (CAR.ACURA_RDX_3G, CAR.ACURA_RDX_3G_MMR) else 1000.0 + wind_adjust = 1.0 + wind_brake_mps2 / wind_learn_speed + if gas_error > 0.0: + self.bosch_wind_factor = float(np.clip(self.bosch_wind_factor * wind_adjust, 0.1, 3.0)) + else: + self.bosch_wind_factor = float(np.clip(self.bosch_wind_factor / wind_adjust, 0.1, 3.0)) + if gas_pedal_force <= 0.0: + self.bosch_wind_factor = max(self.bosch_wind_factor, self.bosch_wind_factor_before_brake) + else: + self.bosch_wind_factor_before_brake = self.bosch_wind_factor + if gas_pedal_force >= self.params.BOSCH_ACCEL_MAX: + self.bosch_gas_factor = min(self.bosch_gas_factor, self.bosch_gas_factor_before_gasmax) + self.bosch_wind_factor = min(self.bosch_wind_factor, self.bosch_wind_factor_before_gasmax) + else: + self.bosch_gas_factor_before_gasmax = self.bosch_gas_factor + self.bosch_wind_factor_before_gasmax = self.bosch_wind_factor + gas_lookup_input = gas_pedal_force * self.bosch_gas_factor + self.gas = float(np.interp(gas_lookup_input, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V)) + self.gas = min(self.gas, max(60.0, self.bosch_last_gas + 60.0)) + stopping = actuators.longControlState == LongCtrlState.stopping + self.bosch_braking = self._update_braking(gas_pedal_force, stopping, CC.longActive) + self.bosch_last_gas = self.gas + return self.accel, self.gas, gas_pedal_force, self.bosch_braking + + def _update_civic(self, CC, CS, hill_brake, wind_brake_mps2, min_gas): + actuators = CC.actuators + learner = self.civic_learner + gas_pedal_force = self.accel + hill_brake + wind_brake_mps2 * learner.windfactor + learner.update(self.accel, CS.out.aEgo, gas_pedal_force, wind_brake_mps2, + engaged=CC.longActive and actuators.longControlState == LongCtrlState.pid, + gas_pressed=CS.out.gasPressed, brake_pressed=CS.out.brakePressed, v_ego=CS.out.vEgo, + pitch=self.pitch, at_accel_max=gas_pedal_force >= self.params.BOSCH_ACCEL_MAX) + # Saved values are the learned factors; the applied ones trail them through the filter. + self.bosch_gas_factor = learner.raw_gasfactor + self.bosch_wind_factor = learner.raw_windfactor + lookup = gas_lookup_accel(gas_pedal_force, hill_brake, learner.gasfactor, min_gas) + self.gas = float(np.interp(lookup, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V)) + self.gas = min(self.gas, max(60.0, self.bosch_last_gas + 60.0)) + stopping = actuators.longControlState == LongCtrlState.stopping + self.bosch_braking = self._update_braking(gas_pedal_force, stopping, CC.longActive) + self.bosch_last_gas = self.gas + return self.accel, self.gas, gas_pedal_force, self.bosch_braking + + def update_mvl_gas(self, CC, CS, accel, hill_brake, wind_brake_mps2, params): + """MVL on (owner decision 2026-11-08): MVL keeps its planner and actuator path, only the Civic gas learner is star7's. + + Returns (gas_pedal_force, gas) for MVL's own ACC_CONTROL builder; MVL's learned gas factor, gas alpha and wind + factor are not applied on top. params is MVL's table, unchanged. Only valid when civic_learner is not None.""" + actuators = CC.actuators + learner = self.civic_learner + min_gas = params.BOSCH_GAS_LOOKUP_BP[0] + gas_pedal_force = accel + hill_brake + wind_brake_mps2 * learner.windfactor + learner.update(accel, CS.out.aEgo, gas_pedal_force, wind_brake_mps2, + engaged=CC.longActive and actuators.longControlState == LongCtrlState.pid, + gas_pressed=CS.out.gasPressed, brake_pressed=CS.out.brakePressed, v_ego=CS.out.vEgo, + pitch=self.pitch, at_accel_max=gas_pedal_force >= params.BOSCH_ACCEL_MAX) + self.bosch_gas_factor = learner.raw_gasfactor + self.bosch_wind_factor = learner.raw_windfactor + lookup = gas_lookup_accel(gas_pedal_force, hill_brake, learner.gasfactor, min_gas) + gas = float(np.interp(lookup, params.BOSCH_GAS_LOOKUP_BP, params.BOSCH_GAS_LOOKUP_V)) + return gas_pedal_force, gas + + def _update_braking(self, gas_pedal_force, stopping, long_active): + """Brake request; on Bosch-A harness cars also the minimum hold and the gas ramp after release (self.gas).""" + if self.brake_edge is None: + return update_honda_bosch_braking(self.bosch_braking, gas_pedal_force, stopping, long_active) + braking = self.brake_edge.update(gas_pedal_force, stopping, long_active) + self.gas = self.brake_edge.limit_gas(self.gas) + return braking diff --git a/opendbc/car/honda/bosch_low_speed_brake.py b/opendbc/car/honda/bosch_low_speed_brake.py new file mode 100644 index 00000000000..695a66baa17 --- /dev/null +++ b/opendbc/car/honda/bosch_low_speed_brake.py @@ -0,0 +1,71 @@ +"""Civic Bosch low-speed crawl: a small integral "extra brake" below ~3 m/s (7 mph). + +Evidence is replay only (Job, 2026-11-06, bpos + low-speed plant A, stops r99/9b/r96 + pool). +Below about 3 m/s (7 mph) behind a stopping car, the Civic Bosch delivers 0.2-0.4 m/s^2 less +decel than commanded and crawls the last metres instead of stopping cleanly. Ported from +mvl-boston opendbc sp-honda-dev-202608 @770d31735 carcontroller.py L320-326 / L1123-1137 and +tamed per replay arm K05N05R1: 9b_S0 crawl at 0.5-2 m/s cut 12.7 -> 7.2 s, pool unchanged vs +base (2 new/2 removed <-1 episodes, fidelity 0.844 vs 0.845, launch median 1.9 vs 1.8 s). The +replay plant is NOT validated for the final crawl (it cannot reproduce r99's crawl to 7 m), so +the road gain from this change is unmeasured. + +This only ever adds braking, never gas: the output is never above the command it is given. The +stopping guard below only slows how fast the extra brake deepens while stopping. mvl's guard +(and the K05N05R1 replay) rate-limits the whole command, which would also hold back a sudden +deeper planner command in the stopping state; here the planner's own command always passes +(static reasoning; the replay's stopping-ramp arm S was inert, so the two should match there). +""" + +import numpy as np + +# 1/s. mvl uses 1.0; replay sweep K05N05R1 (Job, 2026-11-06) found 0.5 keeps ~85% of mvl's +# crawl cut while being gentler. +K_I = 0.5 +# m/s^2, most extra brake the integrator may add. mvl uses -2.0. Replay: this limit had no +# effect on the stops tested; it only caps a wrong aEgo reading. +NEG_LIMIT = -0.5 +# m/s, upper speed of the low-speed window (mvl). +V_MAX = 3.0 +# m/s, below this the extra brake is dropped at once (R1); replay showed a jerk at the final +# snap to a stop without this reset. +V_RESET = 0.3 +# m/s^2 per s, release rate once out of the window. mvl releases +0.02 per 50 Hz tick. +RELEASE_RATE = 1.0 +# m/s^2 per s. mvl's stopping guard is 0.002 per 50 Hz tick: while stopping, the extra brake +# may deepen the output past its last value at most this fast. Never shallower than the command. +STOP_DEEPEN_RATE = 0.1 + + +class LowSpeedBrakeAssist: + """Integral-only (k_p = 0) extra brake for the Civic Bosch low-speed crawl.""" + + def __init__(self, dt: float): + self.dt = dt + self.reset() + + def reset(self): + self.i = 0.0 + self.last = None + self.extra = 0.0 + + def update(self, target: float, sent: float, a_ego: float, v_ego: float, active: bool, stopping: bool) -> float: + if not active: + self.reset() + return sent + + if v_ego < V_RESET: + self.i = 0.0 + out = sent + elif target < 1e-3 and v_ego < V_MAX: + self.i = float(np.clip(self.i + K_I * (target - a_ego) * self.dt, NEG_LIMIT, 0.0)) + out = min(sent, sent + self.i) + else: + self.i = min(0.0, self.i + RELEASE_RATE * self.dt) + out = min(sent, sent + self.i) + + if stopping and self.last is not None: + out = min(sent, max(out, self.last - STOP_DEEPEN_RATE * self.dt)) + + self.last = out + self.extra = out - sent + return out diff --git a/opendbc/car/honda/brake_onset.py b/opendbc/car/honda/brake_onset.py new file mode 100644 index 00000000000..475b023a3f7 --- /dev/null +++ b/opendbc/car/honda/brake_onset.py @@ -0,0 +1,61 @@ +"""Civic Bosch brake-onset overshoot compensation. + +The Civic's ACC ECU decelerates past the ACCEL_COMMAND while a brake request is +deepening, then settles back near the command. Evidence (log analysis, limited +road evidence), dongle 11c8fa231c0499ed routes 0d/0e/0f/10/13/1a, 2026-10-24: + - 45 brake steps (command falling faster than 2 m/s^3): peak aEgo / peak command + median 1.19 (p25 1.11, p75 1.27), about 0.35 m/s^2 deeper, at every speed band. + - Steady braking after the 0.2 s lag is near 1.0-1.1, inside pitch uncertainty, + so this is a transient, not a gain error. + - Ten flagged brake events at 20 Hz: aEgo(t+0.25 s) ~= u + C*(u_f - LP(u_f)) when + deepening, u_f = min(u, 0), LP first order. Best fit C 0.4..0.5, tau 1.0..1.5 s. + RMSE 0.37 -> 0.27 m/s^2, better on every event (e.g. 0f t=774: 0.63 -> 0.44). + +The compensation inverts that model, so the predicted decel equals the planner's +command: it only reduces a deepening brake command, never adds braking, and never +touches a release. C is set below the fit so an over-predicted overshoot costs at +most the cap. The relief fades out for commands approaching full braking so a +maximum brake request is always sent as asked. +""" + +# Fit C 0.4..0.5; 0.35 keeps a margin toward under-compensation. +OVERSHOOT_GAIN = 0.35 +# Fit tau 1.0..1.5 s; the shorter end limits how long a release-then-rebrake keeps relief. +OVERSHOOT_TAU_S = 1.0 +# Largest command reduction. The measured step excess median is 0.35 m/s^2. +MAX_RELIEF_MPS2 = 0.5 +# Relief is full at or above FADE_START and zero at or below FADE_END (BOSCH_ACCEL_MIN is -3.5). +FADE_START_MPS2 = -3.0 +FADE_END_MPS2 = -3.5 + + +class BrakeOnsetCompensation: + def __init__(self, dt: float): + self.alpha = dt / (OVERSHOOT_TAU_S + dt) + self.reset() + + def reset(self): + self.state = None + self.relief = 0.0 + + def update(self, accel: float, active: bool) -> float: + braking = min(accel, 0.0) + if not active or self.state is None: + self.state = braking + self.relief = 0.0 + return accel + + out = accel + if braking < self.state: + k = OVERSHOOT_GAIN * (1.0 - self.alpha) + inverse = (braking + k * self.state) / (1.0 + k) + relief = min(inverse - braking, MAX_RELIEF_MPS2) + if accel <= FADE_END_MPS2: + relief = 0.0 + elif accel < FADE_START_MPS2: + relief *= (accel - FADE_END_MPS2) / (FADE_START_MPS2 - FADE_END_MPS2) + out = accel + max(relief, 0.0) + self.relief = out - accel + # The filter follows what the ECU was actually asked for. + self.state += self.alpha * (min(out, 0.0) - self.state) + return out diff --git a/opendbc/car/honda/carcontroller.py b/opendbc/car/honda/carcontroller.py index 92697dbc8b7..4cf37f281f7 100644 --- a/opendbc/car/honda/carcontroller.py +++ b/opendbc/car/honda/carcontroller.py @@ -8,6 +8,7 @@ from opendbc.can import CANPacker from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, rate_limit, make_tester_present_msg, structs from opendbc.car.honda import hondacan +from opendbc.car.honda.bosch_longitudinal import BoschLongitudinal, bosch_a_harness, qualified as bosch_long_qualified, star7_params from opendbc.car.honda.values import CAR, HONDA_BOSCH, CarControllerParams, CruiseButtons, CruiseSettings, HondaFlags from opendbc.car.interfaces import CarControllerBase from opendbc.car.common.pid import PIDController @@ -275,6 +276,14 @@ def __init__(self, dbc_names, CP, CP_SP, config: HondaCarConfig | None = None): self.last_torque = 0.0 self.bosch_last_gas = 0 self.last_applied_brake = 0.0 + # Honda-PTM-Testing Bosch-A longitudinal (always on; no toggle). Runs on a qualified car with the Bosch-A radar + # harness active (CP.radarUnavailable False, i.e. nrdr's honda.bosch_a_radar). With MVL off it replaces nrdr's gas/wind + # factors and extra-brake controller; with MVL on it is only the gas learner (Civic Bosch), see mvl_longitudinal. + # Every other car keeps nrdr's path unchanged. The host sets bosch_learning_params (BoschLearningParams) to load and + # persist the learned factors, exactly like Testing's starpilot controller_extensions. + self.bosch_longitudinal = (BoschLongitudinal(CP, star7_params(CP, self.params)) + if bosch_a_harness(CP) and bosch_long_qualified(CP) else None) + self.bosch_learning_params = None self.lkas_button_send_remaining = 0 self.last_lkas_button_frame = 0 @@ -488,6 +497,8 @@ def update(self, CC, CC_SP, CS, now_nanos): self.sat_excess_frames = self.sat_deficit_frames = 0 if not self.mvl_longitudinal and (self.CP.flags & HondaFlags.BOSCH or self.CP_SP.enableGasInterceptor): self.gasfactor, self.windfactor = self.nrdr.longitudinal_factors + if self.bosch_longitudinal is not None: + self.bosch_longitudinal.observe_orientation(CC.orientationNED) gas_pedal_force = 0.0 min_gas = self.params.BOSCH_GAS_LOOKUP_BP[0] actuators = CC.actuators @@ -1275,72 +1286,79 @@ def update(self, CC, CC_SP, CS, now_nanos): ts = self.frame * DT_CTRL if self.CP.flags & HondaFlags.BOSCH: - if not self.nrdr.replaces_longitudinal and (accel < 1e-3) and \ - (CS.out.vEgo < 3.0) and not (-1e-3 < CS.out.vEgo < 1e-3): - brake_addon = self.brake_pid.update(error = accel - CS.out.aEgo, speed = CS.out.vEgo) - targetaccel = min(accel,accel + brake_addon) - elif self.nrdr.replaces_longitudinal: - self.brake_pid.reset() - targetaccel = accel + bosch_braking = None + if self.bosch_longitudinal is not None: + # Testing's BoschLongitudinal replaces the nrdr extra-brake controller, wind/gas factors and learner here + self.accel, self.gas, gas_pedal_force, bosch_braking = self.bosch_longitudinal.update(CC, CS, accel) else: - if self.brake_pid.i < 0.0: - self.brake_pid.i = min(0.0, self.brake_pid.i + 0.02) # release 1m/s2 @ 50hz - else: + if not self.nrdr.replaces_longitudinal and (accel < 1e-3) and \ + (CS.out.vEgo < 3.0) and not (-1e-3 < CS.out.vEgo < 1e-3): + brake_addon = self.brake_pid.update(error = accel - CS.out.aEgo, speed = CS.out.vEgo) + targetaccel = min(accel,accel + brake_addon) + elif self.nrdr.replaces_longitudinal: self.brake_pid.reset() - targetaccel = min(accel,accel + self.brake_pid.i) - if actuators.longControlState == LongCtrlState.stopping: # simulate the old stoppingDecelRate of 0.1 m/s^2/s @ 50hz - targetaccel = max(targetaccel, self.last_applied_brake - 0.002) - self.last_applied_brake = targetaccel - self.accel = float(np.clip(targetaccel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX)) - raw_gas_pedal_force = targetaccel + wind_brake_ms2 * self.windfactor + hill_brake - gas_pedal_force = raw_gas_pedal_force + self.nrdr.gas_alpha - - # live-learn gas pedal adjustments when openpilot is controlling gas - if not self.nrdr.replaces_longitudinal and (actuators.longControlState == LongCtrlState.pid) and (not CS.out.gasPressed): - gas_error = accel - CS.out.aEgo - if gas_error != 0.0 and gas_pedal_force > min_gas: - if self.CP.carFingerprint in (CAR.HONDA_INSIGHT, CAR.HONDA_CIVIC_BOSCH): # gas pedal reacts too slowly - learn_speed = 150 - elif self.CP.carFingerprint in (CAR.ACURA_RDX_3G, CAR.ACURA_RDX_3G_MMR): # Prevent overreacting to turbo lag - learn_speed = 300 + targetaccel = accel + else: + if self.brake_pid.i < 0.0: + self.brake_pid.i = min(0.0, self.brake_pid.i + 0.02) # release 1m/s2 @ 50hz else: - learn_speed = 50 - self.gasfactor = np.clip(self.gasfactor + gas_error / learn_speed * (gas_pedal_force - min_gas), 0.01, 3.0) - if gas_error != 0.0 and (not CS.out.brakePressed) and (CS.out.vEgo > 0.0): - if self.CP.carFingerprint in (CAR.ACURA_RDX_3G, CAR.ACURA_RDX_3G_MMR): # Faster reaction - wind_learn_speed = 100 + self.brake_pid.reset() + targetaccel = min(accel,accel + self.brake_pid.i) + if actuators.longControlState == LongCtrlState.stopping: # simulate the old stoppingDecelRate of 0.1 m/s^2/s @ 50hz + targetaccel = max(targetaccel, self.last_applied_brake - 0.002) + self.last_applied_brake = targetaccel + self.accel = float(np.clip(targetaccel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX)) + raw_gas_pedal_force = targetaccel + wind_brake_ms2 * self.windfactor + hill_brake + gas_pedal_force = raw_gas_pedal_force + self.nrdr.gas_alpha + + # live-learn gas pedal adjustments when openpilot is controlling gas + if not self.nrdr.replaces_longitudinal and (actuators.longControlState == LongCtrlState.pid) and (not CS.out.gasPressed): + gas_error = accel - CS.out.aEgo + if gas_error != 0.0 and gas_pedal_force > min_gas: + if self.CP.carFingerprint in (CAR.HONDA_INSIGHT, CAR.HONDA_CIVIC_BOSCH): # gas pedal reacts too slowly + learn_speed = 150 + elif self.CP.carFingerprint in (CAR.ACURA_RDX_3G, CAR.ACURA_RDX_3G_MMR): # Prevent overreacting to turbo lag + learn_speed = 300 + else: + learn_speed = 50 + self.gasfactor = np.clip(self.gasfactor + gas_error / learn_speed * (gas_pedal_force - min_gas), 0.01, 3.0) + if gas_error != 0.0 and (not CS.out.brakePressed) and (CS.out.vEgo > 0.0): + if self.CP.carFingerprint in (CAR.ACURA_RDX_3G, CAR.ACURA_RDX_3G_MMR): # Faster reaction + wind_learn_speed = 100 + else: + wind_learn_speed = 1000 + wind_adjust = 1 + wind_brake_ms2 / wind_learn_speed + self.windfactor = np.clip(self.windfactor * (wind_adjust if (gas_error > 0) else 1.0/wind_adjust), 0.1, 3.0) + if gas_pedal_force <= min_gas: # don't reduce windfactor while braking, allow increases + self.windfactor = max(self.windfactor, self.windfactor_before_brake) else: - wind_learn_speed = 1000 - wind_adjust = 1 + wind_brake_ms2 / wind_learn_speed - self.windfactor = np.clip(self.windfactor * (wind_adjust if (gas_error > 0) else 1.0/wind_adjust), 0.1, 3.0) - if gas_pedal_force <= min_gas: # don't reduce windfactor while braking, allow increases - self.windfactor = max(self.windfactor, self.windfactor_before_brake) - else: - self.windfactor_before_brake = self.windfactor - if gas_pedal_force >= self.params.BOSCH_ACCEL_MAX: # don't increase gasfactor nor windfactor at accel max, allow decreases - self.gasfactor = min(self.gasfactor, self.gasfactor_before_gasmax) - self.windfactor = min(self.windfactor, self.windfactor_before_gasmax) - else: - self.gasfactor_before_gasmax = self.gasfactor - self.windfactor_before_gasmax = self.windfactor - self.gasfactor, self.windfactor = self.nrdr.update_bosch_learner( - CC, CS, actuators, accel, gas_pedal_force, wind_brake_ms2, self.pitch, self.params, live.live_learning_gas, - ) - self.gas = float(np.interp((gas_pedal_force - min_gas) * self.gasfactor + min_gas, - self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V)) - - # limit gas ramp to 60 units per frame, matches stock. Higher sometimes causes powertrain to ignore gas command. - max_gas = max(60, self.bosch_last_gas + 60) - self.gas = min(self.gas, max_gas) - self.bosch_last_gas = self.gas - - stopping = actuators.longControlState == LongCtrlState.stopping and self.accel <= 0.0 + self.windfactor_before_brake = self.windfactor + if gas_pedal_force >= self.params.BOSCH_ACCEL_MAX: # don't increase gasfactor nor windfactor at accel max, allow decreases + self.gasfactor = min(self.gasfactor, self.gasfactor_before_gasmax) + self.windfactor = min(self.windfactor, self.windfactor_before_gasmax) + else: + self.gasfactor_before_gasmax = self.gasfactor + self.windfactor_before_gasmax = self.windfactor + self.gasfactor, self.windfactor = self.nrdr.update_bosch_learner( + CC, CS, actuators, accel, gas_pedal_force, wind_brake_ms2, self.pitch, self.params, live.live_learning_gas, + ) + self.gas = float(np.interp((gas_pedal_force - min_gas) * self.gasfactor + min_gas, + self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V)) + + # limit gas ramp to 60 units per frame, matches stock. Higher sometimes causes powertrain to ignore gas command. + max_gas = max(60, self.bosch_last_gas + 60) + self.gas = min(self.gas, max_gas) + self.bosch_last_gas = self.gas + # Testing's actuator path counts stopping on the state alone (SOURCE carcontroller l.257); + # nrdr's extra accel <= 0 condition stays for every other Bosch car. + stopping = actuators.longControlState == LongCtrlState.stopping and \ + (self.bosch_longitudinal is not None or self.accel <= 0.0) self.stopping_counter = self.stopping_counter + 1 if stopping else 0 # CAN FD: never overlap the stock radar's own ACC_CONTROL stream; ours starts within a few # frames of the radar going silent (see the deferred radar disable above) if not (self.CP.flags & HondaFlags.BOSCH_CANFD and CS.stock_acc_alive): can_sends.extend(hondacan.create_acc_commands(self.packer, self.CAN, CC.enabled, CC.longActive, self.accel, self.gas, - self.stopping_counter, self.CP, gas_pedal_force)) + self.stopping_counter, self.CP, gas_pedal_force, braking=bosch_braking)) elif not (self.CP_SP.flags & HondaFlagsSP.STOCK_LONGITUDINAL): if self.CP_SP.enableGasInterceptor: brake_fraction = self.nrdr.nidec_brake_command(self.brake_last, wind_brake, live.full_brake_authority and not clarity_pedal) @@ -1597,6 +1615,11 @@ def update(self, CC, CC_SP, CS, now_nanos): if self.mvl_longitudinal: mvl_longitudinal.persist(self, self.nrdr.config.provider) + if self.bosch_longitudinal is not None and self.bosch_learning_params is not None: + self.bosch_learning_params.persist(self.frame) # MVL on: only the Civic gas learner is Testing's + elif self.bosch_longitudinal is not None: + if self.bosch_learning_params is not None: + self.bosch_learning_params.persist(self.frame) elif not self.legacy_pcm and self.frame % 6000 == 0: if self.CP.flags & HondaFlags.BOSCH or self.CP_SP.enableGasInterceptor: self.nrdr.persist() diff --git a/opendbc/car/honda/hondacan.py b/opendbc/car/honda/hondacan.py index bb9b4378311..515d2978c07 100644 --- a/opendbc/car/honda/hondacan.py +++ b/opendbc/car/honda/hondacan.py @@ -80,6 +80,9 @@ def create_brake_command(packer, CAN, apply_brake, pump_on, pcm_override, pcm_ca return packer.make_can_msg("BRAKE_COMMAND", CAN.pt, values) +BOSCH_STAR7_MIN_GAS_ACCEL = -0.2 # star7 BOSCH_GAS_LOOKUP_BP[0] + + def bosch_force_command_state(active: bool, gas_force: float) -> tuple[bool, bool]: """Classify the compensated command once so gas and brake can never overlap.""" if not math.isfinite(gas_force): @@ -87,11 +90,17 @@ def bosch_force_command_state(active: bool, gas_force: float) -> tuple[bool, boo return active and gas_force > 0.0, active and gas_force < 0.0 -def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_counter, CP, gas_force): +def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_counter, CP, gas_force, braking=None): commands = [] control_on = 5 if enabled else 0 - gas_active, braking = bosch_force_command_state(active, gas_force) + if braking is None: + gas_active, braking = bosch_force_command_state(active, gas_force) + else: + # BoschLongitudinal (Bosch-A harness) owns the brake request (hysteresis, hold); gas is on only when it is off + # and the compensated force is above star7's gas floor. Same rule as Honda-PTM-Testing create_acc_commands. + braking = active and bool(braking) + gas_active = active and math.isfinite(gas_force) and gas_force > BOSCH_STAR7_MIN_GAS_ACCEL and not braking gas_command = gas if gas_active else -30000 accel_command = accel if active else 0 braking = int(braking) diff --git a/opendbc/car/honda/mvl_longitudinal.py b/opendbc/car/honda/mvl_longitudinal.py index a8d35f9ca18..c6d2220420d 100644 --- a/opendbc/car/honda/mvl_longitudinal.py +++ b/opendbc/car/honda/mvl_longitudinal.py @@ -956,40 +956,46 @@ def commands(self, CC, CS, accel, gas, brake, wind_brake, hill_brake, prior_wind targetaccel = max(targetaccel, self.last_applied_brake - 0.002) self.last_applied_brake = targetaccel self.accel = float(np.clip(targetaccel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX)) - gas_pedal_force = targetaccel + wind_brake_ms2 * self.windfactor + hill_brake + self.gasalpha - - # live-learn gas pedal adjustments when openpilot is controlling gas - if (actuators.longControlState == LongCtrlState.pid) and (not CS.out.gasPressed): - gas_error = accel - CS.out.aEgo - if self.CP.carFingerprint in (CAR.HONDA_INSIGHT, CAR.HONDA_CIVIC_BOSCH): # gas pedal reacts too slowly - learn_speed = 150 - elif self.CP.carFingerprint in (CAR.ACURA_RDX_3G, CAR.ACURA_RDX_3G_MMR): # Prevent overreacting to turbo lag - learn_speed = 300 - else: - learn_speed = 50 - if gas_error != 0.0 and gas_pedal_force > 0: - self.gasfactor = np.clip(self.gasfactor + gas_error / learn_speed * gas_pedal_force, 0.01, 3.0) - if (-0.5 < gas_pedal_force - self.gasalpha < 0.1) and (CS.out.vEgo > 1.0): - self.gasalpha = float(np.clip(self.gasalpha + gas_error / learn_speed / 10.0, 0.0, 0.4)) - if gas_error != 0.0 and (not CS.out.brakePressed) and (CS.out.vEgo > 0.0): - if self.CP.carFingerprint in (CAR.ACURA_RDX_3G, CAR.ACURA_RDX_3G_MMR): # Faster reaction - wind_learn_speed = 100 + bosch_long = getattr(self, "bosch_longitudinal", None) + if bosch_long is not None and bosch_long.civic_learner is not None: + # Owner decision 2026-11-08: on Bosch-A Civic the gas learner is Honda-PTM-Testing's CivicGasLearner even with MVL + # on. MVL's learned gas factor / gas alpha / wind factor are not applied here, and its learner below is skipped. + gas_pedal_force, self.gas = bosch_long.update_mvl_gas(CC, CS, self.accel, hill_brake, wind_brake_ms2, self.params) + else: + gas_pedal_force = targetaccel + wind_brake_ms2 * self.windfactor + hill_brake + self.gasalpha + + # live-learn gas pedal adjustments when openpilot is controlling gas + if (actuators.longControlState == LongCtrlState.pid) and (not CS.out.gasPressed): + gas_error = accel - CS.out.aEgo + if self.CP.carFingerprint in (CAR.HONDA_INSIGHT, CAR.HONDA_CIVIC_BOSCH): # gas pedal reacts too slowly + learn_speed = 150 + elif self.CP.carFingerprint in (CAR.ACURA_RDX_3G, CAR.ACURA_RDX_3G_MMR): # Prevent overreacting to turbo lag + learn_speed = 300 else: - wind_learn_speed = 1000 - wind_adjust = 1 + wind_brake_ms2 / wind_learn_speed - self.windfactor = np.clip(self.windfactor * (wind_adjust if (gas_error > 0) else 1.0/wind_adjust), 0.1, 3.0) - if gas_pedal_force <= 0.0: # don't reduce windfactor while braking, allow increases - self.windfactor = max(self.windfactor, self.windfactor_before_brake) - else: - self.windfactor_before_brake = self.windfactor - if gas_pedal_force >= self.params.BOSCH_ACCEL_MAX: # don't increase gasfactor nor windfactor at accel max, allow decreases - self.gasfactor = min(self.gasfactor, self.gasfactor_before_gasmax) - self.windfactor = min(self.windfactor, self.windfactor_before_gasmax) - else: - self.gasfactor_before_gasmax = self.gasfactor - self.windfactor_before_gasmax = self.windfactor - self.gas = float(np.interp((gas_pedal_force) * self.gasfactor, - [0, self.params.BOSCH_GAS_LOOKUP_BP[1]], self.params.BOSCH_GAS_LOOKUP_V)) + learn_speed = 50 + if gas_error != 0.0 and gas_pedal_force > 0: + self.gasfactor = np.clip(self.gasfactor + gas_error / learn_speed * gas_pedal_force, 0.01, 3.0) + if (-0.5 < gas_pedal_force - self.gasalpha < 0.1) and (CS.out.vEgo > 1.0): + self.gasalpha = float(np.clip(self.gasalpha + gas_error / learn_speed / 10.0, 0.0, 0.4)) + if gas_error != 0.0 and (not CS.out.brakePressed) and (CS.out.vEgo > 0.0): + if self.CP.carFingerprint in (CAR.ACURA_RDX_3G, CAR.ACURA_RDX_3G_MMR): # Faster reaction + wind_learn_speed = 100 + else: + wind_learn_speed = 1000 + wind_adjust = 1 + wind_brake_ms2 / wind_learn_speed + self.windfactor = np.clip(self.windfactor * (wind_adjust if (gas_error > 0) else 1.0/wind_adjust), 0.1, 3.0) + if gas_pedal_force <= 0.0: # don't reduce windfactor while braking, allow increases + self.windfactor = max(self.windfactor, self.windfactor_before_brake) + else: + self.windfactor_before_brake = self.windfactor + if gas_pedal_force >= self.params.BOSCH_ACCEL_MAX: # don't increase gasfactor nor windfactor at accel max, allow decreases + self.gasfactor = min(self.gasfactor, self.gasfactor_before_gasmax) + self.windfactor = min(self.windfactor, self.windfactor_before_gasmax) + else: + self.gasfactor_before_gasmax = self.gasfactor + self.windfactor_before_gasmax = self.windfactor + self.gas = float(np.interp((gas_pedal_force) * self.gasfactor, + [0, self.params.BOSCH_GAS_LOOKUP_BP[1]], self.params.BOSCH_GAS_LOOKUP_V)) # limit gas ramp to 60 units per frame, matches stock. Higher sometimes causes powertrain to ignore gas command. max_gas = max(60, self.bosch_last_gas + 60) diff --git a/opendbc/car/honda/tests/test_bosch_a_long_wiring.py b/opendbc/car/honda/tests/test_bosch_a_long_wiring.py new file mode 100644 index 00000000000..137892b546d --- /dev/null +++ b/opendbc/car/honda/tests/test_bosch_a_long_wiring.py @@ -0,0 +1,108 @@ +"""Wiring of the Honda-PTM-Testing Bosch-A longitudinal modules into the nrdr CarController (static tests, no road evidence).""" +from dataclasses import replace + +import pytest + +from opendbc.car import structs +from opendbc.car.honda.bosch_longitudinal import BoschLongitudinal +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.values import CAR +from opendbc.sunnypilot.car.tests.runtime_config import make_test_car_config + +PID = structs.CarControl.Actuators.LongControlState.pid + + +def build(car, *, bosch_a_radar=True, mvl=False, alpha_long=True): + config = make_test_car_config(bosch_a_radar=bosch_a_radar) + if mvl: + config = replace(config, honda=replace(config.honda, mvl_longitudinal=True)) + fp = {i: {} for i in range(7)} + cp = CarInterface.get_params(car, fp, [], alpha_long, False, False, config) + sp = CarInterface.get_params_sp(cp, car, fp, [], alpha_long, False, False) + ci = CarInterface(cp, sp, config) + ci.update([]) + ci.CS.out.vEgo = 12.0 + ci.CS.out.cruiseState.speed = 25.0 + ci.CS.out.cruiseState.available = True + return ci + + +def drive(ci, accel, ticks=200): + cc = structs.CarControl(enabled=True, longActive=True, latActive=True) + cc.actuators.accel = accel + cc.actuators.longControlState = PID + sent = [] + for tick in range(ticks): + _, messages = ci.apply(cc.as_reader(), structs.CarControlSP(), tick * 10_000_000) + sent.extend(messages) + return sent + + +@pytest.mark.parametrize("car", [CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G, CAR.HONDA_ACCORD]) +def test_bosch_a_cars_get_the_testing_modules_in_both_mvl_modes(car): + for mvl in (False, True): + ci = build(car, mvl=mvl) + assert isinstance(ci.CC.bosch_longitudinal, BoschLongitudinal) + assert ci.CC.bosch_learning_params is None # the host sets it, like Testing's controller_extensions + assert ci.CC.bosch_longitudinal.params.BOSCH_GAS_LOOKUP_BP == [-0.2, 2.0] + + +def test_star7_gas_table_is_per_car_and_leaves_nrdr_params_alone(): + civic = build(CAR.HONDA_CIVIC_BOSCH) + assert civic.CC.bosch_longitudinal.params.BOSCH_GAS_LOOKUP_V == [0, 750] + # nrdr's own (class-shared, mutated by other cars' interface setup) params object is not the one handed over + assert civic.CC.params is not civic.CC.bosch_longitudinal.params + assert build(CAR.HONDA_CRV_5G).CC.bosch_longitudinal.params.BOSCH_GAS_LOOKUP_V == [0, 1600] + + +@pytest.mark.parametrize("kwargs", [dict(bosch_a_radar=False), dict(alpha_long=False)]) +def test_no_modules_without_the_bosch_a_radar_or_openpilot_long(kwargs): + assert build(CAR.HONDA_CIVIC_BOSCH, **kwargs).CC.bosch_longitudinal is None + + +def test_non_bosch_a_cars_are_untouched(): + assert build(CAR.HONDA_CLARITY).CC.bosch_longitudinal is None + assert build(CAR.HONDA_CIVIC).CC.bosch_longitudinal is None + + +def test_mvl_off_replaces_the_nrdr_brake_pid_and_factors(): + ci = build(CAR.HONDA_CIVIC_BOSCH) + gas0, wind0, applied0 = ci.CC.gasfactor, ci.CC.windfactor, ci.CC.last_applied_brake + sent = drive(ci, -0.6) + assert any(addr == 0x1DF for addr, _, _ in sent) # ACC_CONTROL + # nrdr's own extra-brake controller and its factors are bypassed on this car + assert ci.CC.last_applied_brake == applied0 + assert (ci.CC.gasfactor, ci.CC.windfactor) == (gas0, wind0) + assert ci.CC.bosch_longitudinal.bosch_braking is True + assert ci.CC.accel == pytest.approx(ci.CC.bosch_longitudinal.accel) + + +def test_mvl_off_calls_learning_params_persist_with_the_frame(): + ci = build(CAR.HONDA_CIVIC_BOSCH) + frames = [] + ci.CC.bosch_learning_params = type("L", (), {"persist": lambda self, frame: frames.append(frame)})() + drive(ci, 0.3, ticks=20) + assert frames and frames == sorted(frames) + + +def test_mvl_on_keeps_mvl_path_but_uses_the_testing_civic_gas_learner(): + ci = build(CAR.HONDA_CIVIC_BOSCH, mvl=True) + assert ci.CC.mvl_longitudinal + learner = ci.CC.bosch_longitudinal.civic_learner + assert learner is not None + gas_before, alpha_before = ci.CC.gasfactor, ci.CC.gasalpha + sent = drive(ci, 0.8, ticks=300) + assert any(addr == 0x1DF for addr, _, _ in sent) + # MVL's learned gas factor / alpha are not applied nor learned on this car + assert (ci.CC.gasfactor, ci.CC.gasalpha) == (gas_before, alpha_before) + assert ci.CC.gas > 0.0 + + +def test_mvl_on_non_civic_keeps_mvl_learner(): + ci = build(CAR.HONDA_CRV_5G, mvl=True) + assert ci.CC.bosch_longitudinal.civic_learner is None + # control: MVL's own learner still runs there, so the Civic test above is not passing for lack of learning + ci.CS.out.aEgo = 0.0 + before = ci.CC.gasfactor + drive(ci, 0.8, ticks=300) + assert ci.CC.gasfactor != before diff --git a/opendbc/car/honda/tests/test_bosch_gas_learner.py b/opendbc/car/honda/tests/test_bosch_gas_learner.py new file mode 100644 index 00000000000..520c86b9550 --- /dev/null +++ b/opendbc/car/honda/tests/test_bosch_gas_learner.py @@ -0,0 +1,151 @@ +import math +from types import SimpleNamespace + +import pytest + +from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, gen_empty_fingerprint, structs +from opendbc.car.honda import bosch_gas_learner as gl +from opendbc.car.honda.bosch_longitudinal import BoschLongitudinal, star7_params +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.values import CAR, CarControllerParams +from opendbc.sunnypilot.car.tests.runtime_config import make_test_car_config + +FLAT = gl.CIVIC_PITCH_BIAS + + +def owner(identity=CAR.HONDA_CIVIC_BOSCH): + cp = CarInterface.get_params(identity, gen_empty_fingerprint(), [], True, False, False, make_test_car_config(bosch_a_radar=True)) + return BoschLongitudinal(cp, star7_params(cp, CarControllerParams(cp))) + + +def inputs(speed=20.0, measured=0.0, active=True, state="pid", gas=False, brake=False): + cc = structs.CarControl(longActive=active) + cc.actuators.longControlState = state + cs = SimpleNamespace(out=SimpleNamespace(vEgo=speed, aEgo=measured, gasPressed=gas, brakePressed=brake)) + return cc, cs + + +def drive(controller, accels, measured, pitch=FLAT, speed=20.0): + controller.pitch = pitch + for a in accels: + cc, cs = inputs(speed=speed, measured=measured) + controller.update(cc, cs, a) + + +def test_only_civic_bosch_gets_the_ns_learner(): + assert owner().civic_learner is not None and owner().learner_version == gl.LEARN_VERSION + for car in (CAR.HONDA_ACCORD, CAR.HONDA_INSIGHT, CAR.HONDA_CRV_5G): + other = owner(car) + assert other.civic_learner is None and other.learner_version is None + other.set_factors(0.01, 5.0) # the shared learner keeps its own range + assert (other.bosch_gas_factor, other.bosch_wind_factor) == (0.01, 5.0) + + +def test_civic_saved_factors_use_the_ns_range(): + c = owner() + c.set_factors(3.0, 0.1) + assert (c.bosch_gas_factor, c.bosch_wind_factor) == (gl.HARD_HI, gl.HARD_LO) + assert c.civic_learner.gasfactor == gl.HARD_HI # applied factor starts at the saved value, no transient + c.set_factors(float("nan"), float("inf")) + assert (c.bosch_gas_factor, c.bosch_wind_factor) == (1.0, 1.0) + + +def test_steady_under_delivery_raises_the_gas_factor(): + c = owner() + drive(c, [0.5] * 100, measured=0.3) + assert c.civic_learner.learning + assert c.civic_learner.last_gas_error == pytest.approx(0.2) + assert 1.0 < c.civic_learner.gasfactor < c.bosch_gas_factor # applied trails learned through the filter + + +def test_learner_uses_the_command_from_half_a_second_ago(): + learner = gl.CivicGasLearner() + args = dict(a_ego=0.5, gas_pedal_force=0.5, wind_brake_ms2=0.0, engaged=True, gas_pressed=False, + brake_pressed=False, v_ego=20.0, pitch=FLAT, at_accel_max=False) + learner.update(accel_cmd=0.3, **args) # engagement edge fills the window with 0.3 + for _ in range(gl.LAG_TICKS - 1): + learner.update(accel_cmd=0.3, **args) + learner.update(accel_cmd=0.35, **args) # newest differs, oldest is still 0.3 + assert learner.last_gas_error == pytest.approx(0.3 - 0.5) + + +@pytest.mark.parametrize("pitch", [FLAT + 0.03, FLAT - 0.03]) +def test_no_learning_on_grades(pitch): + c = owner() + drive(c, [0.5] * 100, measured=0.3, pitch=pitch) + assert not c.civic_learner.learning and c.bosch_gas_factor == 1.0 + + +def test_no_learning_while_the_command_ramps(): + c = owner() + drive(c, [i / 100 for i in range(100)], measured=0.0) # 1 m/s^2 over 2 s = 0.5 m/s^3, above the 0.3 gate + assert c.bosch_gas_factor == 1.0 + + +def test_no_learning_with_driver_gas_or_brake_or_standstill(): + for kw in (dict(gas=True), dict(brake=True), dict(speed=0.0)): + c = owner() + c.pitch = FLAT + for _ in range(100): + cc, cs = inputs(measured=0.3, **kw) + c.update(cc, cs, 0.5) + assert c.bosch_gas_factor == 1.0, kw + + +def test_factor_decays_back_inside_the_soft_band(): + learner = gl.CivicGasLearner(1.5, 1.0) + learner.update(0.0, 0.0, 0.0, 0.0, engaged=False, gas_pressed=False, brake_pressed=False, v_ego=0.0, pitch=FLAT, + at_accel_max=False) + assert learner.raw_gasfactor == pytest.approx(1.5 - gl.DECAY_PER_TICK) + inside = gl.CivicGasLearner(1.2, 1.0) + inside.update(0.0, 0.0, 0.0, 0.0, engaged=False, gas_pressed=False, brake_pressed=False, v_ego=0.0, pitch=FLAT, + at_accel_max=False) + assert inside.raw_gasfactor == 1.2 + + +def test_gas_lookup_is_unchanged_on_flat_road_and_gains_on_climbs(): + hill_flat = math.sin(FLAT) * ACCELERATION_DUE_TO_GRAVITY + assert gl.gas_lookup_accel(0.5 + hill_flat, hill_flat, 1.0, -0.2) == pytest.approx(0.5 + hill_flat) + # factor scales the offset from the gas-on threshold, not the hill term + assert gl.gas_lookup_accel(0.5 + hill_flat, hill_flat, 1.2, -0.2) == pytest.approx((0.5 + 0.2) * 1.2 - 0.2 + hill_flat) + hill = 0.8 + assert gl.gas_lookup_accel(hill, hill, 1.0, -0.2) == pytest.approx(hill + 0.1 * (hill - hill_flat)) + + +def test_civic_keeps_star7_gas_table(): + c = owner() + assert c.params.BOSCH_GAS_LOOKUP_BP == [-0.2, 2.0] and c.params.BOSCH_GAS_LOOKUP_V == [0, 750] + + +class FakeParams: + def __init__(self, gas, wind): + self.values = {"HondaGasFactorParams": gas, "HondaWindFactorParams": wind} + + def get(self, key, return_default=False): + return self.values[key] + + def put(self, key, value): + self.values[key] = value + + +def test_old_saved_factors_are_reset_once(tmp_path): + # Needs the openpilot checkout; the feeding side (BoschLearningParams) is tested there. + BoschLearningParams = pytest.importorskip("openpilot.starpilot.car.honda.longitudinal").BoschLearningParams + meta = str(tmp_path / "meta.json") + saved = FakeParams(1.4, 0.7) + first = BoschLearningParams(saved, owner(), meta_path=meta) + assert (first.owner.bosch_gas_factor, first.owner.bosch_wind_factor) == (1.0, 1.0) + assert saved.values == {"HondaGasFactorParams": 1.0, "HondaWindFactorParams": 1.0} + saved.values = {"HondaGasFactorParams": 1.3, "HondaWindFactorParams": 0.9} # learned on the new math + second = BoschLearningParams(saved, owner(), meta_path=meta) + assert (second.owner.bosch_gas_factor, second.owner.bosch_wind_factor) == (1.3, 0.9) + + +def test_other_cars_never_reset_saved_factors(tmp_path): + # Needs the openpilot checkout; the feeding side (BoschLearningParams) is tested there. + BoschLearningParams = pytest.importorskip("openpilot.starpilot.car.honda.longitudinal").BoschLearningParams + meta = tmp_path / "meta.json" + saved = FakeParams(1.4, 0.7) + learning = BoschLearningParams(saved, owner(CAR.HONDA_ACCORD), meta_path=str(meta)) + assert (learning.owner.bosch_gas_factor, learning.owner.bosch_wind_factor) == (1.4, 0.7) + assert not meta.exists() diff --git a/opendbc/car/honda/tests/test_bosch_longitudinal.py b/opendbc/car/honda/tests/test_bosch_longitudinal.py new file mode 100644 index 00000000000..b7020db4ecc --- /dev/null +++ b/opendbc/car/honda/tests/test_bosch_longitudinal.py @@ -0,0 +1,239 @@ +import math +import time +from types import SimpleNamespace + +import pytest + +from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, gen_empty_fingerprint, structs +from opendbc.car.honda.bosch_longitudinal import BoschLongitudinal, bosch_a_harness, qualified, star7_params +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.values import CAR, CarControllerParams +from opendbc.sunnypilot.car.tests.runtime_config import make_test_car_config + + +def owner(identity=CAR.HONDA_ACCORD): + cp = CarInterface.get_params(identity, gen_empty_fingerprint(), [], True, False, False, make_test_car_config(bosch_a_radar=True)) + assert qualified(cp) + return BoschLongitudinal(cp, star7_params(cp, CarControllerParams(cp))) + + +def inputs(speed=0.0, measured=0.0, active=True, state="pid", gas=False, brake=False): + cc = structs.CarControl(longActive=active) + cc.actuators.longControlState = state + cs = SimpleNamespace(out=SimpleNamespace(vEgo=speed, aEgo=measured, gasPressed=gas, brakePressed=brake)) + return cc, cs + + +def test_pitch_observation_retains_odd_frame_sample_for_next_command(): + controller = owner() + controller.observe_orientation((0.0, 0.1, 0.0)) + controller.observe_orientation(()) + cc, cs = inputs(state="off") + result = controller.update(cc, cs, 0.0) + assert result[2] == pytest.approx(math.sin(0.1) * ACCELERATION_DUE_TO_GRAVITY) + controller.observe_orientation((0.0, -0.1, 0.0)) + assert controller.update(cc, cs, 0.0)[2] < 0 + + +def test_disabled_update_preserves_original_gas_ramp_without_requesting_brakes(): + controller = owner() + cc, cs = inputs(active=False, state="off") + controller.observe_orientation((0.0, 0.2, 0.0)) + first = controller.update(cc, cs, 0.0) + second = controller.update(cc, cs, 0.0) + assert first[1] == 60.0 + assert second[1] == 120.0 + assert first[3] is second[3] is False + assert controller.bosch_gas_factor == controller.bosch_wind_factor == 1.0 + + +@pytest.mark.parametrize("identity,rate", ((CAR.HONDA_ACCORD, 50.0), (CAR.HONDA_INSIGHT, 150.0), + (CAR.ACURA_RDX_3G, 300.0), (CAR.ACURA_RDX_3G_MMR, 300.0))) +def test_learning_uses_actual_vehicle_rate_and_driver_gas_override(identity, rate): + controller = owner(identity) + cc, cs = inputs(measured=0.5) + controller.update(cc, cs, 1.0) + assert controller.bosch_gas_factor == pytest.approx(1.0 + 0.5 / rate) + previous = controller.bosch_gas_factor + cs.out.gasPressed = True + controller.update(cc, cs, 1.0) + assert controller.bosch_gas_factor == previous + + +def test_braking_hysteresis_stopping_and_inactive_release(): + # Civic 2022 is not a Bosch-A harness car, so this checks the plain hysteresis without the brake fixes. + controller = owner(CAR.HONDA_CIVIC_2022) + cc, cs = inputs(state="off") + assert controller.update(cc, cs, -0.13)[3] + assert controller.update(cc, cs, -0.03)[3] + assert not controller.update(cc, cs, -0.01)[3] + cc.actuators.longControlState = "stopping" + assert controller.update(cc, cs, 0.1)[3] + cc.longActive = False + assert not controller.update(cc, cs, 0.0)[3] + + +def test_saved_factors_keep_shared_learner_range_and_reject_invalid_values(): + controller = owner() + controller.set_factors(0.01, 5.0) + assert (controller.bosch_gas_factor, controller.bosch_wind_factor) == (0.01, 5.0) + controller.set_factors(float("nan"), float("inf")) + assert (controller.bosch_gas_factor, controller.bosch_wind_factor) == (1.0, 1.0) + + +def test_actual_controller_captures_pitch_on_non_transmit_frame(): + # nrdr adaptation: CarInterface needs CP_SP and the host config. + config, fp = make_test_car_config(bosch_a_radar=True), gen_empty_fingerprint() + cp = CarInterface.get_params(CAR.HONDA_ACCORD, fp, [], True, False, False, config) + sp = CarInterface.get_params_sp(cp, CAR.HONDA_ACCORD, fp, [], True, False, False) + ci = CarInterface(cp, sp, config) + ci.update([]) + command = structs.CarControl() + command.actuators.longControlState = "off" + ci.apply(command.as_reader(), structs.CarControlSP(), 1_000_000_000) + command.orientationNED = [0.0, 0.1, 0.0] + ci.apply(command.as_reader(), structs.CarControlSP(), 1_010_000_000) + command.orientationNED = [] + ci.apply(command.as_reader(), structs.CarControlSP(), 1_020_000_000) + assert ci.CC.bosch_longitudinal.pitch == pytest.approx(0.1) + assert ci.CC.bosch_longitudinal.bosch_last_gas > 0.0 + + +def test_actual_configure_loads_typed_factors_without_startup_rewrite(): + # Needs the openpilot checkout (Params, starpilot.controller_extensions); the feeding side is tested there. + pytest.importorskip("openpilot.starpilot.controller_extensions") + from openpilot.common.params import Params + from openpilot.common.prefix import OpenpilotPrefix + from openpilot.starpilot.controller_extensions import configure_controller + + with OpenpilotPrefix(): + saved = Params() + saved.put("HondaGasFactorParams", 1.25, block=True) + saved.put("HondaWindFactorParams", 0.85, block=True) + cp = CarInterface.get_params(CAR.HONDA_ACCORD, gen_empty_fingerprint(), [], True, False, False, make_test_car_config(bosch_a_radar=True)) + ci = CarInterface(cp) + configure_controller(ci, saved) + learning = ci.CC.bosch_learning_params + assert learning.owner is ci.CC.bosch_longitudinal + assert (learning.owner.bosch_gas_factor, learning.owner.bosch_wind_factor) == (1.25, 0.85) + learning.owner.set_factors(1.1, 0.9) + learning.persist(0) + learning.persist(5999) + assert saved.get("HondaGasFactorParams") == 1.25 + assert saved.get("HondaWindFactorParams") == 0.85 + learning.persist(6000) + deadline = time.monotonic() + 2.0 + while saved.get("HondaWindFactorParams") != 0.9 and time.monotonic() < deadline: + time.sleep(0.01) + assert saved.get("HondaGasFactorParams") == pytest.approx(1.1) + assert saved.get("HondaWindFactorParams") == pytest.approx(0.9) + writes = [] + learning.params = SimpleNamespace(put=lambda *args: writes.append(args)) + learning.persist(12000) + assert writes == [] + learning.owner.set_factors(1.2, 0.9) + learning.persist(18000) + assert writes == [("HondaGasFactorParams", 1.2), ("HondaWindFactorParams", 0.9)] + + +def test_civic_brake_request_starts_deeper_and_coasts_the_band_above(): + controller = owner(CAR.HONDA_CIVIC_BOSCH) + cc, cs = inputs(speed=3.0) + assert not controller.update(cc, cs, -0.15)[3] + assert controller.update(cc, cs, -0.30)[3] + + +def test_civic_brake_request_holds_at_least_its_minimum_unless_accel_is_asked(): + from opendbc.car.honda.bosch_longitudinal import BOSCH_A_BRAKE_MIN_HOLD_S + controller = owner(CAR.HONDA_CIVIC_BOSCH) + cc, cs = inputs(speed=3.0) + assert controller.update(cc, cs, -0.4)[3] + frames = 0 + while controller.update(cc, cs, 0.05)[3]: + frames += 1 + assert frames * controller.brake_edge.dt == pytest.approx(BOSCH_A_BRAKE_MIN_HOLD_S, abs=0.03) + assert controller.update(cc, cs, -0.4)[3] + assert not controller.update(cc, cs, 0.5)[3] # a clear accel request ends the hold at once + + +def test_civic_gas_after_release_ramps_from_35_instead_of_jumping_to_the_cap(): + from opendbc.car.honda.bosch_longitudinal import BOSCH_A_RELEASE_GAS_RAMP_S, BOSCH_A_RELEASE_GAS_START + controller = owner(CAR.HONDA_CIVIC_BOSCH) + cc, cs = inputs(speed=3.0) + for _ in range(100): + controller.update(cc, cs, -0.4) + gases = [] + for _ in range(int(BOSCH_A_RELEASE_GAS_RAMP_S / controller.brake_edge.dt) + 20): + _, gas, _, braking = controller.update(cc, cs, 0.3) + assert not braking + gases.append(gas) + assert gases[0] <= BOSCH_A_RELEASE_GAS_START + assert all(b >= a - 1e-6 for a, b in zip(gases, gases[1:], strict=False)) + assert max(b - a for a, b in zip(gases, gases[1:], strict=False)) < 10.0 + free = owner(CAR.HONDA_CIVIC_BOSCH) + for _ in range(100): + target = free.update(cc, cs, 0.3)[1] + assert gases[-1] == pytest.approx(target, rel=0.05) + + +def test_civic_brake_edge_keeps_stopping_and_long_drop_behavior(): + controller = owner(CAR.HONDA_CIVIC_BOSCH) + cc, cs = inputs(speed=1.0, state="stopping") + assert controller.update(cc, cs, 0.1)[3] + cc.longActive = False + assert not controller.update(cc, cs, -1.0)[3] + + +def test_brake_fixes_run_on_bosch_a_harness_cars_only(): + for identity in (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G, CAR.HONDA_ACCORD): + controller = owner(identity) + assert controller.brake_edge is not None and controller.brake_onset is not None + for identity in CAR: + cp = CarInterface.get_params(identity, gen_empty_fingerprint(), [], True, False, False, make_test_car_config(bosch_a_radar=True)) + if qualified(cp): + controller = BoschLongitudinal(cp, star7_params(cp, CarControllerParams(cp))) + assert (controller.brake_edge is not None) == (controller.brake_onset is not None) == bosch_a_harness(cp), identity + civic_2022 = owner(CAR.HONDA_CIVIC_2022) + assert civic_2022.brake_edge is None and civic_2022.brake_onset is None + cp = CarInterface.get_params(CAR.HONDA_CIVIC_BOSCH, gen_empty_fingerprint(), [], True, False, False, make_test_car_config(bosch_a_radar=True)) + cp.radarUnavailable = True + no_radar = BoschLongitudinal(cp, star7_params(cp, CarControllerParams(cp))) + assert no_radar.brake_edge is None and no_radar.brake_onset is None + + +def test_crv_brake_edge_holds_the_request_and_ramps_gas_after_release(): + from opendbc.car.honda.bosch_longitudinal import BOSCH_A_RELEASE_GAS_RAMP_S, BOSCH_A_RELEASE_GAS_START + controller = owner(CAR.HONDA_CRV_5G) + cc, cs = inputs(speed=3.0) + assert not controller.update(cc, cs, -0.15)[3] + for _ in range(100): + assert controller.update(cc, cs, -0.4)[3] + gases = [] + for _ in range(int(BOSCH_A_RELEASE_GAS_RAMP_S / controller.brake_edge.dt) + 20): + _, gas, _, braking = controller.update(cc, cs, 0.3) + gases.append(gas) + assert not braking and gases[0] <= BOSCH_A_RELEASE_GAS_START + assert all(b >= a - 1e-6 for a, b in zip(gases, gases[1:], strict=False)) + + +def test_brake_edge_launch_from_stopping_ramps_gas_faster_than_a_brake_cycle_release(): + from opendbc.car.honda.bosch_longitudinal import (BOSCH_A_LAUNCH_GAS_RAMP_S, BOSCH_A_RELEASE_GAS_RAMP_S, + BOSCH_A_RELEASE_GAS_START, BoschABrakeEdge) + assert BOSCH_A_LAUNCH_GAS_RAMP_S < BOSCH_A_RELEASE_GAS_RAMP_S + + def gas_after_release(stopping_before): + edge = BoschABrakeEdge(0.02) + for _ in range(100): # 2 s of braking: past the minimum hold + assert edge.update(-0.5, stopping_before, True) + gases = [] + for _ in range(int(BOSCH_A_RELEASE_GAS_RAMP_S / edge.dt) + 5): + assert not edge.update(0.5, False, True) + gases.append(edge.limit_gas(500.)) + return gases + + launch, cycle = gas_after_release(True), gas_after_release(False) + assert launch[0] == cycle[0] == BOSCH_A_RELEASE_GAS_START + n_launch = int(round(BOSCH_A_LAUNCH_GAS_RAMP_S / 0.02)) + assert launch[n_launch] == 500. and cycle[n_launch] < 500. + assert all(a >= b for a, b in zip(launch, cycle, strict=True)) + assert cycle[-1] == 500. diff --git a/opendbc/car/honda/tests/test_bosch_low_speed_brake.py b/opendbc/car/honda/tests/test_bosch_low_speed_brake.py new file mode 100644 index 00000000000..94263d719a5 --- /dev/null +++ b/opendbc/car/honda/tests/test_bosch_low_speed_brake.py @@ -0,0 +1,135 @@ +from types import SimpleNamespace + +import pytest + +from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs +from opendbc.car.honda.bosch_longitudinal import BoschLongitudinal, star7_params +from opendbc.car.honda.bosch_low_speed_brake import (LowSpeedBrakeAssist, NEG_LIMIT, V_MAX, V_RESET, + RELEASE_RATE, STOP_DEEPEN_RATE) +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.values import CAR, CarControllerParams +from opendbc.sunnypilot.car.tests.runtime_config import make_test_car_config + +DT = 2 * DT_CTRL + + +def test_inactive_returns_sent_and_resets(): + assist = LowSpeedBrakeAssist(DT) + assist.i = -0.3 + assist.last = -1.2 + out = assist.update(target=-1.0, sent=-1.0, a_ego=-0.5, v_ego=1.0, active=False, stopping=False) + assert out == -1.0 + assert assist.i == 0.0 + assert assist.last is None + + +def test_integrator_builds_up_over_one_second(): + assist = LowSpeedBrakeAssist(DT) + out = -1.0 + for _ in range(int(round(1.0 / DT))): + out = assist.update(target=-1.0, sent=-1.0, a_ego=-0.6, v_ego=2.0, active=True, stopping=False) + assert assist.i == pytest.approx(-0.2, abs=0.02) + assert out == pytest.approx(-1.0 + assist.i, abs=1e-6) + + +def test_integral_clips_to_neg_limit_and_zero(): + assist = LowSpeedBrakeAssist(DT) + # a_ego is less negative than target (an undershoot): the integrator deepens until clipped. + for _ in range(2000): + assist.update(target=-1.0, sent=-1.0, a_ego=0.0, v_ego=2.0, active=True, stopping=False) + assert assist.i == pytest.approx(NEG_LIMIT) + assert assist.i <= 0.0 + + +def test_out_never_exceeds_sent_when_not_stopping(): + assist = LowSpeedBrakeAssist(DT) + sent = -1.0 + for _ in range(100): + out = assist.update(target=-1.0, sent=sent, a_ego=-0.6, v_ego=2.0, active=True, stopping=False) + assert out <= sent + 1e-9 + + +def test_release_above_v_max_or_positive_target(): + assist = LowSpeedBrakeAssist(DT) + for _ in range(50): + assist.update(target=-1.0, sent=-1.0, a_ego=-0.6, v_ego=2.0, active=True, stopping=False) + assert assist.i < 0.0 + # Above V_MAX, the integrator releases at RELEASE_RATE back to 0. + steps = int(round(abs(assist.i) / (RELEASE_RATE * DT))) + 2 + for _ in range(steps): + out = assist.update(target=-1.0, sent=-1.0, a_ego=-0.6, v_ego=V_MAX + 0.1, active=True, stopping=False) + assert assist.i == pytest.approx(0.0, abs=1e-9) + assert out == pytest.approx(-1.0, abs=1e-9) + + assist2 = LowSpeedBrakeAssist(DT) + for _ in range(50): + assist2.update(target=-1.0, sent=-1.0, a_ego=-0.6, v_ego=2.0, active=True, stopping=False) + assert assist2.i < 0.0 + steps2 = int(round(abs(assist2.i) / (RELEASE_RATE * DT))) + 2 + for _ in range(steps2): + out2 = assist2.update(target=1.0, sent=-1.0, a_ego=-0.6, v_ego=2.0, active=True, stopping=False) + assert assist2.i == pytest.approx(0.0, abs=1e-9) + assert out2 == pytest.approx(-1.0, abs=1e-9) + + +def test_below_v_reset_drops_integral_immediately(): + assist = LowSpeedBrakeAssist(DT) + for _ in range(50): + assist.update(target=-1.0, sent=-1.0, a_ego=-0.6, v_ego=2.0, active=True, stopping=False) + assert assist.i < 0.0 + out = assist.update(target=-1.0, sent=-1.0, a_ego=-0.6, v_ego=V_RESET - 0.1, active=True, stopping=False) + assert assist.i == 0.0 + assert out == -1.0 + + +def test_stopping_guard_limits_deepen_rate(): + assist = LowSpeedBrakeAssist(DT) + # Build up some extra brake first. + for _ in range(50): + assist.update(target=-1.0, sent=-1.0, a_ego=-0.6, v_ego=1.0, active=True, stopping=False) + prev = assist.last + out = assist.update(target=-1.0, sent=-1.0, a_ego=-5.0, v_ego=1.0, active=True, stopping=True) + # Even though the integrator would want to deepen a lot in one tick, the stopping guard caps it. + assert out >= prev - STOP_DEEPEN_RATE * DT - 1e-9 + + +def bosch(identity): + cp = CarInterface.get_params(identity, gen_empty_fingerprint(), [], True, False, False, make_test_car_config(bosch_a_radar=True)) + return BoschLongitudinal(cp, star7_params(cp, CarControllerParams(cp))) + + +def frame(state="pid", gas=False, brake=False, active=True, speed=2.0, measured=0.0): + cc = structs.CarControl(longActive=active) + cc.actuators.longControlState = state + return cc, SimpleNamespace(out=SimpleNamespace(vEgo=speed, aEgo=measured, gasPressed=gas, brakePressed=brake)) + + +def test_bosch_a_harness_civic_gets_low_speed_brake(): + civic = bosch(CAR.HONDA_CIVIC_BOSCH) + assert civic.low_speed_brake is not None + + +def test_non_harness_car_gets_none(): + # Civic 2022 is not a Bosch-A harness car (radarless); the Accord is one since the Bosch-A toggles (PR #45). + other = bosch(CAR.HONDA_CIVIC_2022) + assert other.low_speed_brake is None + + +def test_low_speed_shortfall_deepens_accel(): + civic = bosch(CAR.HONDA_CIVIC_BOSCH) + cc, cs = frame(speed=2.0, measured=-0.4) + civic.update(cc, cs, 0.0) + for _ in range(50): + result = civic.update(cc, cs, -1.0) + assert result[0] < -1.0 + + +def test_stopping_guard_never_holds_back_the_command(): + assist = LowSpeedBrakeAssist(DT) + assist.update(target=0.0, sent=0.0, a_ego=0.0, v_ego=1.0, active=True, stopping=False) + # A sudden deeper planner command in the stopping state passes through unchanged. + out = assist.update(target=-1.5, sent=-1.5, a_ego=-1.5, v_ego=1.0, active=True, stopping=True) + assert out == -1.5 + for _ in range(100): + out = assist.update(target=-1.0, sent=-1.0, a_ego=0.0, v_ego=1.0, active=True, stopping=True) + assert out <= -1.0 diff --git a/opendbc/car/honda/tests/test_bosch_stopping.py b/opendbc/car/honda/tests/test_bosch_stopping.py index 225a69d606e..98d0bffd657 100644 --- a/opendbc/car/honda/tests/test_bosch_stopping.py +++ b/opendbc/car/honda/tests/test_bosch_stopping.py @@ -15,6 +15,13 @@ def test_bosch_stop_follows_sent_acceleration_not_just_planner_state(car, has_re # Then check zero, negative, release without a state change, and pid with negative accel. cases = [("pid", .003, .003, 0), ("stopping", -.1, .001, 0), ("stopping", 0., 0., 1), ("stopping", -.1, -.002, 2), ("stopping", .25, .25, 0), ("pid", -.1, -.1, 0)] + if car == CAR.HONDA_CIVIC_BOSCH: + # Owner decision (Bosch-A port): on Bosch-A cars Testing's BoschLongitudinal replaces nrdr's brake_pid/stopping + # ramp, so the planner accel is sent as-is (and the last case runs through its brake onset/edge smoothing). + # The counter follows Testing's rule (SOURCE carcontroller l.257): every stopping frame counts, whatever the + # accel, so standstill holds through the positive-accel stopping frame. The last case's sent accel was observed. + cases = [("pid", .003, .003, 0), ("stopping", -.1, -.1, 1), ("stopping", 0., 0., 2), + ("stopping", -.1, -.1, 3), ("stopping", .25, .25, 4), ("pid", -.1, -0.07445255585395508, 0)] for frame, (state, accel, sent, expected_counter) in enumerate(cases): cc = control(accel) cc.actuators.longControlState = state diff --git a/opendbc/car/honda/tests/test_brake_onset.py b/opendbc/car/honda/tests/test_brake_onset.py new file mode 100644 index 00000000000..85be842af2c --- /dev/null +++ b/opendbc/car/honda/tests/test_brake_onset.py @@ -0,0 +1,126 @@ +from types import SimpleNamespace + +import pytest + +from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs +from opendbc.car.honda.bosch_longitudinal import BoschLongitudinal, star7_params +from opendbc.car.honda.brake_onset import (BrakeOnsetCompensation, FADE_END_MPS2, MAX_RELIEF_MPS2, + OVERSHOOT_GAIN, OVERSHOOT_TAU_S) +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.values import CAR, CarControllerParams +from opendbc.sunnypilot.car.tests.runtime_config import make_test_car_config + +DT = 2 * DT_CTRL + + +def plant(commands, gain=OVERSHOOT_GAIN, tau=OVERSHOOT_TAU_S): + """The fitted overshoot model: the car decelerates past a deepening command.""" + alpha = DT / (tau + DT) + state = min(commands[0], 0.0) + out = [] + for u in commands: + braking = min(u, 0.0) + state += alpha * (braking - state) + out.append(u + gain * min(braking - state, 0.0)) + return out + + +def ramp(start, end, rate, hold_s=2.0): + commands, value = [], start + while value > end: + commands.append(value) + value -= rate * DT + return commands + [end] * int(hold_s / DT) + + +def compensate(commands, active=True): + comp = BrakeOnsetCompensation(DT) + return [comp.update(u, active) for u in commands] + + +@pytest.mark.parametrize("end,rate,capped", [(-1.0, 1.5, False), (-2.3, 3.0, False), (-2.7, 4.0, True)]) +def test_modelled_car_tracks_the_planner_command(end, rate, capped): + commands = ramp(0.0, end, rate, hold_s=8.0) + raw = plant(commands) + compensated = plant(compensate(commands)) + raw_miss = max(abs(a - u) for a, u in zip(raw, commands, strict=True)) + comp_miss = max(abs(a - u) for a, u in zip(compensated, commands, strict=True)) + assert raw_miss > 0.1 + if capped: + # The relief cap leaves part of a very steep onset uncorrected. + assert comp_miss < raw_miss / 2 + else: + assert comp_miss < 1e-6 + # The settled command is delivered unchanged. + assert compensate(commands)[-1] == pytest.approx(end, abs=0.01) + + +def test_never_adds_braking_and_never_touches_a_release(): + commands = ramp(0.5, -2.0, 3.0, hold_s=0.5) + [-2.0 + 0.1 * i for i in range(30)] + comp = BrakeOnsetCompensation(DT) + for u in commands: + deepening = comp.state is not None and min(u, 0.0) < comp.state + out = comp.update(u, True) + assert out >= u + assert out - u <= MAX_RELIEF_MPS2 + 1e-9 + if not deepening: + assert out == u + + +def test_full_brake_request_is_sent_as_asked(): + commands = [0.0] * 5 + [-3.5] * 20 + assert compensate(commands)[5:] == commands[5:] + assert all(out >= u for out, u in zip(compensate([0.0, -3.2]), [0.0, -3.2], strict=True)) + assert compensate([0.0, FADE_END_MPS2 - 0.1])[-1] == FADE_END_MPS2 - 0.1 + + +def test_inactive_passes_through_and_resets(): + commands = ramp(0.0, -2.0, 3.0) + assert compensate(commands, active=False) == commands + comp = BrakeOnsetCompensation(DT) + comp.update(0.0, True) + comp.update(-1.0, True) + assert comp.relief > 0 + assert comp.update(-1.5, False) == -1.5 + assert comp.relief == 0.0 + assert comp.update(-1.5, True) == -1.5 + + +def bosch(identity): + cp = CarInterface.get_params(identity, gen_empty_fingerprint(), [], True, False, False, make_test_car_config(bosch_a_radar=True)) + return BoschLongitudinal(cp, star7_params(cp, CarControllerParams(cp))) + + +def frame(state="pid", gas=False, brake=False, active=True): + cc = structs.CarControl(longActive=active) + cc.actuators.longControlState = state + return cc, SimpleNamespace(out=SimpleNamespace(vEgo=10.0, aEgo=0.0, gasPressed=gas, brakePressed=brake)) + + +def test_civic_bosch_command_is_compensated_only_in_pid(): + civic = bosch(CAR.HONDA_CIVIC_BOSCH) + cc, cs = frame() + civic.update(cc, cs, 0.0) + assert civic.update(cc, cs, -1.5)[0] > -1.5 + for state, gas, brake in (("stopping", False, False), ("pid", True, False), ("pid", False, True)): + other = bosch(CAR.HONDA_CIVIC_BOSCH) + cc, cs = frame(state, gas, brake) + other.update(cc, cs, 0.0) + assert other.update(cc, cs, -1.5)[0] == -1.5 + + +def test_accord_bosch_a_harness_command_is_compensated(): + accord = bosch(CAR.HONDA_ACCORD) + assert accord.brake_onset is not None + cc, cs = frame() + accord.update(cc, cs, 0.0) + assert accord.update(cc, cs, -1.5)[0] > -1.5 + + +def test_other_bosch_cars_are_unchanged(): + # Civic 2022 is not a Bosch-A harness car. + other = bosch(CAR.HONDA_CIVIC_2022) + assert other.brake_onset is None + cc, cs = frame() + other.update(cc, cs, 0.0) + assert other.update(cc, cs, -1.5)[0] == -1.5