forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Prebuilt Release @ 67fd9c2
This commit is contained in:
@@ -0,0 +1,166 @@
|
||||
from iqdbc.car import DT_CTRL, gen_empty_fingerprint, structs
|
||||
from iqdbc.car.honda.interface import CarInterface
|
||||
from iqdbc.car.honda.values import CAR
|
||||
|
||||
CANFD_CAR = CAR.HONDA_CRV_6G
|
||||
|
||||
RADAR_DIAG_ADDR = 0x18DAB0F1
|
||||
ACC_CONTROL_ADDR = 0x1DF
|
||||
ACC_HUD_ADDR = 0x30C
|
||||
SCM_BUTTONS_ADDR = 0x296
|
||||
RADAR_HUD_ADDR = 0x310
|
||||
LANE_PATH_ADDR = 0x6CD5558
|
||||
HUD_OBJECTS_ADDR = 0x6CD5559
|
||||
RADAR_LEAD_ADDR = 0xF31AA5C
|
||||
RADAR_LEAD2_ADDR = 0xF31AA52
|
||||
SUPPLEMENTAL_ADDR = 0x1A45AA4E
|
||||
LOOKALIKE_ADDRS = (RADAR_HUD_ADDR, LANE_PATH_ADDR, HUD_OBJECTS_ADDR, RADAR_LEAD_ADDR, RADAR_LEAD2_ADDR, SUPPLEMENTAL_ADDR)
|
||||
|
||||
EXT_DIAG_SESSION = b'\x02\x10\x03\x00\x00\x00\x00\x00'
|
||||
COMM_CONTROL_DISABLE = b'\x03\x28\x83\x03\x00\x00\x00\x00'
|
||||
|
||||
|
||||
def build_long_interface():
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
CP = CarInterface.get_params(CANFD_CAR, fingerprint, [], False, False, False)
|
||||
CP.openpilotLongitudinalControl = True
|
||||
CP.pcmCruise = False
|
||||
CP_IQ = CarInterface.get_params_iq(CP, CANFD_CAR, fingerprint, [], False, False, False)
|
||||
return CarInterface(CP, CP_IQ)
|
||||
|
||||
|
||||
def make_cc(enabled=True):
|
||||
CC = structs.CarControl()
|
||||
CC.enabled = enabled
|
||||
CC.latActive = enabled
|
||||
CC.longActive = enabled
|
||||
return CC.as_reader()
|
||||
|
||||
|
||||
class CanfdControllerHarness:
|
||||
def __init__(self):
|
||||
self.ci = build_long_interface()
|
||||
self.cs = self.ci.CS
|
||||
self.ci.update([])
|
||||
self.now_nanos = 0
|
||||
self.set_radar(alive=True, relay_open=False)
|
||||
self.set_ticks()
|
||||
|
||||
def set_radar(self, alive, relay_open):
|
||||
self.cs.stock_acc_alive = alive
|
||||
self.cs.canfd_relay_open = relay_open
|
||||
|
||||
def set_ticks(self, hud=False, supp=False, five=False, fifty=False):
|
||||
self.cs.hud_tick = hud
|
||||
self.cs.supp_tick = supp
|
||||
self.cs.radar_5hz_tick = five
|
||||
self.cs.radar_50hz_tick = fifty
|
||||
|
||||
def step(self, CC=None, model=None):
|
||||
self.now_nanos += int(DT_CTRL * 1e9)
|
||||
_, can_sends = self.ci.apply(CC or make_cc(), structs.IQCarControl(), self.now_nanos, model)
|
||||
return can_sends
|
||||
|
||||
@staticmethod
|
||||
def by_addr(can_sends, addr):
|
||||
return [m for m in can_sends if m[0] == addr]
|
||||
|
||||
|
||||
class TestCanfdDeferredRadarDisable:
|
||||
def setup_method(self):
|
||||
self.h = CanfdControllerHarness()
|
||||
|
||||
def test_no_disable_requests_before_relay_open(self):
|
||||
for _ in range(20):
|
||||
sends = self.h.step()
|
||||
assert not self.h.by_addr(sends, RADAR_DIAG_ADDR)
|
||||
assert not self.h.by_addr(sends, ACC_CONTROL_ADDR)
|
||||
assert not any(self.h.by_addr(sends, a) for a in LOOKALIKE_ADDRS)
|
||||
|
||||
def test_disable_handshake_after_relay_open(self):
|
||||
self.h.set_radar(alive=True, relay_open=True)
|
||||
payloads = []
|
||||
for _ in range(101):
|
||||
for msg in self.h.by_addr(self.h.step(), RADAR_DIAG_ADDR):
|
||||
payloads.append(msg[1])
|
||||
assert payloads == [EXT_DIAG_SESSION, COMM_CONTROL_DISABLE, EXT_DIAG_SESSION, COMM_CONTROL_DISABLE, EXT_DIAG_SESSION]
|
||||
|
||||
def test_tester_present_keeps_radar_down_once_silent(self):
|
||||
self.h.set_radar(alive=False, relay_open=True)
|
||||
payloads = []
|
||||
for _ in range(60):
|
||||
payloads += [m[1] for m in self.h.by_addr(self.h.step(), RADAR_DIAG_ADDR)]
|
||||
assert payloads == [b'\x02\x3E\x80\x00\x00\x00\x00\x00'] * 6
|
||||
|
||||
|
||||
class TestCanfdReplacementStream:
|
||||
def setup_method(self):
|
||||
self.h = CanfdControllerHarness()
|
||||
self.h.set_radar(alive=False, relay_open=True)
|
||||
|
||||
def test_acc_control_every_second_frame(self):
|
||||
seen = [bool(self.h.by_addr(self.h.step(), ACC_CONTROL_ADDR)) for _ in range(10)]
|
||||
assert sum(seen) == 5
|
||||
|
||||
def test_no_acc_control_while_stock_alive(self):
|
||||
self.h.set_radar(alive=True, relay_open=True)
|
||||
for _ in range(10):
|
||||
assert not self.h.by_addr(self.h.step(), ACC_CONTROL_ADDR)
|
||||
|
||||
def test_lookalikes_mirrored_byte_identical_on_both_buses(self):
|
||||
self.h.set_ticks(hud=True, supp=True, five=True, fifty=True)
|
||||
sends = self.h.step()
|
||||
for addr in LOOKALIKE_ADDRS:
|
||||
msgs = self.h.by_addr(sends, addr)
|
||||
assert len(msgs) == 2, hex(addr)
|
||||
buses = sorted(m[2] for m in msgs)
|
||||
assert buses == [0, 2], hex(addr)
|
||||
assert msgs[0][1] == msgs[1][1], hex(addr)
|
||||
|
||||
def test_no_lookalikes_without_ticks(self):
|
||||
sends = self.h.step()
|
||||
for addr in (RADAR_HUD_ADDR, RADAR_LEAD_ADDR, RADAR_LEAD2_ADDR, SUPPLEMENTAL_ADDR, LANE_PATH_ADDR, HUD_OBJECTS_ADDR):
|
||||
assert not self.h.by_addr(sends, addr)
|
||||
|
||||
def test_mux_sweep_contiguous_across_banks(self):
|
||||
self.h.set_ticks(fifty=True)
|
||||
muxes = []
|
||||
for _ in range(45):
|
||||
msgs = self.h.by_addr(self.h.step(), LANE_PATH_ADDR)
|
||||
muxes.append(msgs[0][1][0] >> 2)
|
||||
sweep = list(range(1, 11)) + list(range(17, 27)) + list(range(33, 43)) + list(range(49, 59))
|
||||
assert muxes == (sweep + sweep)[:45]
|
||||
|
||||
def test_acc_hud_rides_hud_tick(self):
|
||||
assert not self.h.by_addr(self.h.step(), ACC_HUD_ADDR)
|
||||
self.h.set_ticks(hud=True)
|
||||
assert self.h.by_addr(self.h.step(), ACC_HUD_ADDR)
|
||||
self.h.set_ticks()
|
||||
assert not self.h.by_addr(self.h.step(), ACC_HUD_ADDR)
|
||||
|
||||
|
||||
class TestCanfdButtonTakeover:
|
||||
def setup_method(self):
|
||||
self.h = CanfdControllerHarness()
|
||||
self.h.set_radar(alive=False, relay_open=True)
|
||||
|
||||
def test_buttons_streamed_to_camera_while_engaged(self):
|
||||
seen = 0
|
||||
for _ in range(20):
|
||||
for msg in self.h.by_addr(self.h.step(), SCM_BUTTONS_ADDR):
|
||||
assert msg[2] == 2
|
||||
seen += 1
|
||||
assert seen == 5
|
||||
|
||||
def test_no_button_stream_when_disengaged(self):
|
||||
for _ in range(20):
|
||||
assert not self.h.by_addr(self.h.step(make_cc(enabled=False)), SCM_BUTTONS_ADDR)
|
||||
|
||||
def test_ambient_light_echoed(self):
|
||||
self.h.cs.scm_ambient_light = 0x77
|
||||
for _ in range(4):
|
||||
msgs = self.h.by_addr(self.h.step(), SCM_BUTTONS_ADDR)
|
||||
if msgs:
|
||||
assert msgs[0][1][2] == 0x77
|
||||
return
|
||||
raise AssertionError("no SCM_BUTTONS takeover frame seen")
|
||||
@@ -0,0 +1,202 @@
|
||||
import pytest
|
||||
|
||||
from iqdbc.can import CANPacker
|
||||
from iqdbc.car import Bus, DT_CTRL, gen_empty_fingerprint
|
||||
from iqdbc.car.honda.interface import CarInterface
|
||||
from iqdbc.car.honda.values import CAR, DBC
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
|
||||
CANFD_CAR = CAR.HONDA_CRV_6G
|
||||
RADARLESS_CAR = CAR.HONDA_CIVIC_2022
|
||||
CAMERA_MESSAGES_ADDR = 0x35E
|
||||
|
||||
|
||||
def build_car(candidate, extra_pt_addrs=()):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
for addr in extra_pt_addrs:
|
||||
fingerprint[0][addr] = 8
|
||||
CP = CarInterface.get_params(candidate, fingerprint, [], False, False, False)
|
||||
CP_IQ = CarInterface.get_params_iq(CP, candidate, fingerprint, [], False, False, False)
|
||||
return CarInterface(CP, CP_IQ)
|
||||
|
||||
|
||||
class CanFeed:
|
||||
def __init__(self, ci, dbc_name):
|
||||
self.ci = ci
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.nanos = 0
|
||||
# the first CarState.update lazily subscribes vl-read messages, so run one empty
|
||||
# cycle before feeding data or the first fed frame of those messages is dropped
|
||||
self.step()
|
||||
self.ci.CS.update(self.ci.can_parsers)
|
||||
|
||||
def step(self, msgs=()):
|
||||
self.nanos += int(DT_CTRL * 1e9)
|
||||
packed = [self.packer.make_can_msg(name, bus, values) for name, bus, values in msgs]
|
||||
for parser in self.ci.can_parsers.values():
|
||||
parser.update([self.nanos, packed])
|
||||
|
||||
|
||||
class TestHondaCanfdRadarState:
|
||||
def setup_method(self):
|
||||
self.ci = build_car(CANFD_CAR)
|
||||
self.cs = self.ci.CS
|
||||
self.feed = CanFeed(self.ci, DBC[CANFD_CAR][Bus.pt])
|
||||
|
||||
def update(self, msgs=()):
|
||||
self.feed.step(msgs)
|
||||
return self.cs.update(self.ci.can_parsers)
|
||||
|
||||
def test_parsers_include_radar_bus(self):
|
||||
assert Bus.radar in self.ci.can_parsers
|
||||
assert self.ci.can_parsers[Bus.radar].bus == 1
|
||||
|
||||
def test_50hz_tick_fires_one_frame_before_next_tick(self):
|
||||
ticks = []
|
||||
for frame in range(20):
|
||||
msgs = [("RADAR_50HZ_TICK_REFERENCE", 1, {})] if frame % 2 == 0 else []
|
||||
self.update(msgs)
|
||||
ticks.append(self.cs.radar_50hz_tick)
|
||||
assert ticks[2:] == [frame % 2 == 1 for frame in range(2, 20)]
|
||||
|
||||
def test_hud_tick_fires_one_frame_before_next_tick(self):
|
||||
fired = []
|
||||
for frame in range(40):
|
||||
msgs = [("RADAR_HUD_TICK_REFERENCE", 1, {})] if frame % 10 == 0 else []
|
||||
self.update(msgs)
|
||||
if self.cs.hud_tick:
|
||||
fired.append(frame)
|
||||
assert fired == [9, 19, 29, 39]
|
||||
|
||||
def test_5hz_tick_fires_at_stock_radar_lead_offset(self):
|
||||
fired = []
|
||||
for frame in range(60):
|
||||
msgs = [("RADAR_REFERENCE", 0, {})] if frame % 20 == 0 else []
|
||||
self.update(msgs)
|
||||
if self.cs.radar_5hz_tick:
|
||||
fired.append(frame)
|
||||
assert fired == [11, 31, 51]
|
||||
|
||||
def test_stock_acc_alive_until_four_silent_frames(self):
|
||||
for frame in range(11):
|
||||
msgs = [("ACC_CONTROL", 0, {})] if frame % 2 == 0 else []
|
||||
self.update(msgs)
|
||||
assert self.cs.stock_acc_alive
|
||||
|
||||
silent_state = []
|
||||
for _ in range(6):
|
||||
self.update()
|
||||
silent_state.append(self.cs.stock_acc_alive)
|
||||
assert silent_state == [True, True, True, False, False, False]
|
||||
|
||||
self.update([("ACC_CONTROL", 0, {})])
|
||||
assert self.cs.stock_acc_alive
|
||||
|
||||
def test_relay_open_when_camera_steering_disappears(self):
|
||||
for _ in range(10):
|
||||
self.update([("STEERING_CONTROL", 0, {})])
|
||||
assert not self.cs.canfd_relay_open
|
||||
assert self.cs.camera_steer_seen
|
||||
|
||||
open_state = []
|
||||
for _ in range(7):
|
||||
self.update()
|
||||
open_state.append(self.cs.canfd_relay_open)
|
||||
assert open_state == [False, False, False, False, True, True, True]
|
||||
|
||||
def test_relay_open_fallback_without_camera(self):
|
||||
primed_frames = self.cs.canfd_frames
|
||||
for frame in range(510):
|
||||
self.update()
|
||||
assert self.cs.canfd_relay_open == (primed_frames + frame + 1 >= 500)
|
||||
|
||||
def test_ambient_light_echoed_from_scm_buttons(self):
|
||||
self.update([("SCM_BUTTONS", 0, {"AMBIENT_LIGHT_MAYBE": 0x5A})])
|
||||
assert self.cs.scm_ambient_light == 0x5A
|
||||
|
||||
|
||||
class TestHondaNonCanfdRadarState:
|
||||
def test_no_radar_parser_and_ticks_stay_low(self):
|
||||
ci = build_car(RADARLESS_CAR)
|
||||
feed = CanFeed(ci, DBC[RADARLESS_CAR][Bus.pt])
|
||||
assert Bus.radar not in ci.can_parsers
|
||||
for _ in range(5):
|
||||
feed.step()
|
||||
ci.CS.update(ci.can_parsers)
|
||||
assert not ci.CS.radar_50hz_tick
|
||||
assert not ci.CS.hud_tick
|
||||
assert not ci.CS.supp_tick
|
||||
assert not ci.CS.radar_5hz_tick
|
||||
|
||||
|
||||
class TestCanfdLongInterface:
|
||||
def test_alpha_long_available_on_canfd(self):
|
||||
CP = CarInterface.get_params(CANFD_CAR, gen_empty_fingerprint(), [], False, False, False)
|
||||
assert CP.alphaLongitudinalAvailable
|
||||
assert not CP.openpilotLongitudinalControl
|
||||
assert CP.pcmCruise
|
||||
|
||||
def test_alpha_long_enabled_on_canfd(self):
|
||||
CP = CarInterface.get_params(CANFD_CAR, gen_empty_fingerprint(), [], True, False, False)
|
||||
assert CP.openpilotLongitudinalControl
|
||||
assert not CP.pcmCruise
|
||||
assert CP.longitudinalActuatorDelay == pytest.approx(0.05)
|
||||
|
||||
def test_canfd_long_init_clears_dtcs_without_disabling_radar(self, mocker):
|
||||
clear_all = mocker.patch("iqdbc.car.honda.interface.clear_all_dtcs")
|
||||
clear_ecu = mocker.patch("iqdbc.car.honda.interface.clear_ecu_dtcs")
|
||||
disable = mocker.patch("iqdbc.car.honda.interface.disable_ecu")
|
||||
|
||||
CP = CarInterface.get_params(CANFD_CAR, gen_empty_fingerprint(), [], True, False, False)
|
||||
CarInterface.init(CP, None, None, None)
|
||||
assert clear_all.call_count == 1
|
||||
assert clear_all.call_args.args[1] == [0, 2]
|
||||
assert clear_ecu.call_count == 1
|
||||
assert disable.call_count == 0
|
||||
|
||||
def test_canfd_deinit_reenables_radar(self, mocker):
|
||||
clear_all = mocker.patch("iqdbc.car.honda.interface.clear_all_dtcs")
|
||||
disable = mocker.patch("iqdbc.car.honda.interface.disable_ecu")
|
||||
|
||||
CP = CarInterface.get_params(CANFD_CAR, gen_empty_fingerprint(), [], True, False, False)
|
||||
CarInterface.deinit(CP, None, None)
|
||||
assert clear_all.call_count == 0
|
||||
assert disable.call_count == 1
|
||||
|
||||
def test_bosch_a_long_init_still_disables_radar(self, mocker):
|
||||
clear_all = mocker.patch("iqdbc.car.honda.interface.clear_all_dtcs")
|
||||
disable = mocker.patch("iqdbc.car.honda.interface.disable_ecu")
|
||||
|
||||
CP = CarInterface.get_params(CAR.HONDA_ACCORD, gen_empty_fingerprint(), [], True, False, False)
|
||||
CarInterface.init(CP, None, None, None)
|
||||
assert clear_all.call_count == 0
|
||||
assert disable.call_count == 1
|
||||
|
||||
|
||||
class TestHondaDashboardSpeedLimit:
|
||||
def build(self, candidate, with_camera_messages):
|
||||
extra = (CAMERA_MESSAGES_ADDR,) if with_camera_messages else ()
|
||||
return build_car(candidate, extra_pt_addrs=extra)
|
||||
|
||||
@pytest.mark.parametrize("sign_value,expected_mph", [(101, 25), (97, 5), (113, 85)])
|
||||
def test_speed_limit_sign_reported(self, sign_value, expected_mph):
|
||||
ci = self.build(RADARLESS_CAR, True)
|
||||
feed = CanFeed(ci, DBC[RADARLESS_CAR][Bus.pt])
|
||||
feed.step([("CAMERA_MESSAGES", 2, {"SPEED_LIMIT_SIGN": sign_value})])
|
||||
_, ret_iq = ci.CS.update(ci.can_parsers)
|
||||
assert ret_iq.speedLimit == pytest.approx(expected_mph * CV.MPH_TO_MS)
|
||||
|
||||
@pytest.mark.parametrize("sign_value", [125, 0, 32])
|
||||
def test_invalid_sign_reports_no_limit(self, sign_value):
|
||||
ci = self.build(RADARLESS_CAR, True)
|
||||
feed = CanFeed(ci, DBC[RADARLESS_CAR][Bus.pt])
|
||||
feed.step([("CAMERA_MESSAGES", 2, {"SPEED_LIMIT_SIGN": sign_value})])
|
||||
_, ret_iq = ci.CS.update(ci.can_parsers)
|
||||
assert ret_iq.speedLimit == 0.0
|
||||
|
||||
def test_without_camera_messages_flag_no_limit(self):
|
||||
ci = self.build(RADARLESS_CAR, False)
|
||||
feed = CanFeed(ci, DBC[RADARLESS_CAR][Bus.pt])
|
||||
feed.step([("CAMERA_MESSAGES", 2, {"SPEED_LIMIT_SIGN": 101})])
|
||||
_, ret_iq = ci.CS.update(ci.can_parsers)
|
||||
assert ret_iq.speedLimit == 0.0
|
||||
@@ -0,0 +1,235 @@
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
|
||||
from iqdbc.can import CANPacker
|
||||
from iqdbc.car.honda import dash_lane, dash_objects
|
||||
|
||||
V_EGO = 30.0
|
||||
|
||||
|
||||
def model_at(center_y):
|
||||
x = list(np.linspace(0.0, 110.0, 23))
|
||||
|
||||
def line(y):
|
||||
return SimpleNamespace(x=x, y=[y] * len(x))
|
||||
return SimpleNamespace(laneLines=[line(center_y + 3.3), line(center_y + 1.65), line(center_y - 1.65), line(center_y - 3.3)],
|
||||
laneLineProbs=[0.0, 1.0, 1.0, 0.0],
|
||||
leadsV3=[])
|
||||
|
||||
|
||||
def lane_xy(center_y):
|
||||
m = model_at(center_y)
|
||||
return m.laneLines[1].x, [(a + b) / 2.0 for a, b in zip(m.laneLines[1].y, m.laneLines[2].y, strict=True)]
|
||||
|
||||
|
||||
class TestLanePathSlew:
|
||||
def test_first_fit_shown_unslewed(self):
|
||||
renderer = dash_lane.LanePathRenderer()
|
||||
lane = renderer.update(model_at(-2.0), V_EGO, 0.0)
|
||||
assert lane.offsets == dash_lane.encode_lane_path(*lane_xy(-2.0))
|
||||
|
||||
def test_step_is_rate_limited(self):
|
||||
renderer = dash_lane.LanePathRenderer()
|
||||
prev = renderer.update(model_at(0.0), V_EGO, 0.0).offsets
|
||||
assert all(o == 0 for o in prev)
|
||||
|
||||
target = dash_lane.encode_lane_path(*lane_xy(-2.0))
|
||||
max_step = math.ceil(dash_lane.SLEW_MAX_STEP)
|
||||
for _ in range(10):
|
||||
cur = renderer.update(model_at(-2.0), V_EGO, 0.0).offsets
|
||||
for p, c, t in zip(prev, cur, target, strict=True):
|
||||
assert abs(c - p) <= max_step
|
||||
assert abs(t - c) <= abs(t - p)
|
||||
prev = cur
|
||||
assert prev == target
|
||||
|
||||
def test_full_scale_takes_two_seconds(self):
|
||||
renderer = dash_lane.LanePathRenderer()
|
||||
renderer.update(model_at(0.0), V_EGO, 0.0)
|
||||
target = dash_lane.encode_lane_path(*lane_xy(-100.0))
|
||||
assert all(t == dash_lane.OFFSET_VALID_MAX for t in target)
|
||||
|
||||
n_updates = round(dash_lane.SLEW_FULL_SCALE_S * dash_lane.SLEW_RATE_HZ)
|
||||
for i in range(n_updates):
|
||||
lane = renderer.update(model_at(-100.0), V_EGO, 0.0)
|
||||
if i < n_updates - 1:
|
||||
assert lane.offsets != target
|
||||
assert lane.offsets == target
|
||||
|
||||
def test_blank_resets_slew(self):
|
||||
renderer = dash_lane.LanePathRenderer()
|
||||
renderer.update(model_at(0.0), V_EGO, 0.0)
|
||||
lane = renderer.update(None, V_EGO, 0.0)
|
||||
assert lane.offsets == [dash_lane.OFFSET_UNAVAILABLE] * dash_lane.POINT_COUNT
|
||||
lane = renderer.update(model_at(-2.0), V_EGO, 0.0)
|
||||
assert lane.offsets == dash_lane.encode_lane_path(*lane_xy(-2.0))
|
||||
|
||||
def test_short_path_passthrough_and_reset(self):
|
||||
renderer = dash_lane.LanePathRenderer()
|
||||
renderer.update(model_at(0.0), V_EGO, 0.0)
|
||||
|
||||
short = model_at(-2.0)
|
||||
for ll in short.laneLines:
|
||||
ll.x = ll.x[:10]
|
||||
ll.y = ll.y[:10]
|
||||
lane = renderer.update(short, V_EGO, 0.0)
|
||||
assert lane.offsets == [dash_lane.OFFSET_UNAVAILABLE] * dash_lane.POINT_COUNT
|
||||
|
||||
lane = renderer.update(model_at(-2.0), V_EGO, 0.0)
|
||||
assert lane.offsets == dash_lane.encode_lane_path(*lane_xy(-2.0))
|
||||
|
||||
|
||||
class TestLaneLineHysteresis:
|
||||
def test_single_line_offset_and_hysteresis(self):
|
||||
renderer = dash_lane.LanePathRenderer()
|
||||
m = model_at(0.0)
|
||||
m.laneLineProbs = [0.0, 0.0, 1.0, 0.0]
|
||||
lane = renderer.update(m, V_EGO, 0.0)
|
||||
assert not lane.left_line and lane.right_line
|
||||
assert lane.offsets == dash_lane.encode_lane_path(m.laneLines[2].x, [y - dash_lane.HALF_LANE_M for y in m.laneLines[2].y])
|
||||
|
||||
# a left prob between OFF and ON must not switch the left line on
|
||||
m.laneLineProbs = [0.0, (dash_lane.LINE_PROB_OFF + dash_lane.LINE_PROB_ON) / 2, 1.0, 0.0]
|
||||
lane = renderer.update(m, V_EGO, 0.0)
|
||||
assert not lane.left_line
|
||||
|
||||
# once on, the same mid prob keeps it on
|
||||
m.laneLineProbs = [0.0, dash_lane.LINE_PROB_ON, 1.0, 0.0]
|
||||
assert renderer.update(m, V_EGO, 0.0).left_line
|
||||
m.laneLineProbs = [0.0, (dash_lane.LINE_PROB_OFF + dash_lane.LINE_PROB_ON) / 2, 1.0, 0.0]
|
||||
assert renderer.update(m, V_EGO, 0.0).left_line
|
||||
|
||||
|
||||
class TestCanfdReshape:
|
||||
def test_idle_pattern_when_blank(self):
|
||||
assert dash_lane.canfd_lane_offsets(dash_lane.RenderedLane()) == dash_lane.CANFD_IDLE_OFFSETS
|
||||
assert dash_lane.canfd_lane_length(dash_lane.RenderedLane()) == dash_lane.CANFD_MIN_VALID_PTS
|
||||
|
||||
def test_terminated_prefix_matches_length_law(self):
|
||||
for v_ego, expected in ((0.0, 7), (10.0, 15), (19.0, 23), (38.0, 23)):
|
||||
lane = dash_lane.RenderedLane(offsets=[5] * dash_lane.POINT_COUNT, reach=1.0, v_ego=v_ego)
|
||||
n = dash_lane.canfd_lane_length(lane)
|
||||
assert n == expected
|
||||
offs = dash_lane.canfd_lane_offsets(lane)
|
||||
assert offs[:n] == [5] * n
|
||||
assert offs[n:] == [dash_lane.OFFSET_UNAVAILABLE] * (dash_lane.POINT_COUNT - n)
|
||||
|
||||
|
||||
class TestMuxMapping:
|
||||
def test_mux_cycle_covers_all_banks(self):
|
||||
assert len(dash_lane.MUX_CYCLE) == 40
|
||||
assert set(dash_lane.MUX_CYCLE) == set(range(1, 11)) | set(range(17, 27)) | set(range(33, 43)) | set(range(49, 59))
|
||||
|
||||
def test_lane_path_frame_selects_offsets_by_mux(self):
|
||||
packer = CANPacker("honda_bosch_radarless_generated")
|
||||
offsets = list(range(40))
|
||||
for mux in dash_lane.MUX_CYCLE:
|
||||
addr, dat, bus = dash_lane.create_lane_path(packer, 0, offsets, mux)
|
||||
base = ((mux - 1) % 16) * 4
|
||||
raw_mux = dat[0] >> 2
|
||||
assert raw_mux == mux
|
||||
assert base < 40
|
||||
|
||||
|
||||
class TestDashObjectAuthor:
|
||||
def make_lead(self, prob=0.9, d=30.0, y=0.0, v=0.0):
|
||||
status = prob >= dash_objects.LEAD_PROB_ON
|
||||
return dash_objects.ModelLead(status, d, y, v, prob=prob)
|
||||
|
||||
def payload(self, msg):
|
||||
return msg[1]
|
||||
|
||||
def test_inactive_slot_bytes_match_stock_sentinel(self):
|
||||
packer = CANPacker("honda_common_canfd_generated")
|
||||
author = dash_objects.DashObjectAuthor()
|
||||
msg = author.create(packer, 0, self.make_lead(prob=0.0), None, 2, 0.0)
|
||||
parsed_long = ((self.payload(msg)[4] << 2) | (self.payload(msg)[5] >> 6)) & 0x3FF
|
||||
assert parsed_long == 1023
|
||||
|
||||
def test_lead_rendered_in_slot0_only(self):
|
||||
packer = CANPacker("honda_common_canfd_generated")
|
||||
author = dash_objects.DashObjectAuthor()
|
||||
lead = self.make_lead()
|
||||
slot0 = author.create(packer, 0, lead, None, 1, 0.0)
|
||||
slot3 = author.create(packer, 0, lead, None, 4, 0.02)
|
||||
assert self.payload(slot0)[1] != 0
|
||||
assert self.payload(slot3)[1] & 0xF8 == 0
|
||||
|
||||
def test_lead_prob_hysteresis_and_hold(self):
|
||||
packer = CANPacker("honda_common_canfd_generated")
|
||||
author = dash_objects.DashObjectAuthor()
|
||||
now = 0.0
|
||||
|
||||
def object_id(prob):
|
||||
nonlocal now
|
||||
now += 0.02
|
||||
msg = author.create(packer, 0, self.make_lead(prob=prob), None, 1, now)
|
||||
return self.payload(msg)[1] >> 3
|
||||
|
||||
assert object_id(0.6) != 0
|
||||
# dips below ON but above OFF keep rendering
|
||||
assert object_id(0.4) != 0
|
||||
# a full drop is bridged for LEAD_HOLD_S
|
||||
assert object_id(0.0) != 0
|
||||
now += dash_objects.LEAD_HOLD_S
|
||||
assert object_id(0.0) == 0
|
||||
|
||||
def test_reid_on_range_discontinuity(self):
|
||||
ident = dash_objects.LeadIdentity()
|
||||
now = 0.0
|
||||
first = ident.update(True, 30.0, 0.0, now)
|
||||
# stay steady past the re-id refractory window
|
||||
for _ in range(int(dash_objects.REID_REFRACTORY / 0.02) + 10):
|
||||
now += 0.02
|
||||
same = ident.update(True, 30.0, 0.0, now)
|
||||
assert same == first
|
||||
now += 0.02
|
||||
assert ident.update(True, 60.0, 0.0, now) != first
|
||||
|
||||
def test_camera_lead_never_forwarded(self):
|
||||
packer = CANPacker("honda_bosch_radarless_generated")
|
||||
author = dash_objects.DashObjectAuthor()
|
||||
tracks = [dash_objects.CameraObject(slot=i, object_id=0, d_rel=0.0, y_rel=0.0, is_lead_car=False, valid=False)
|
||||
for i in range(dash_objects.NUM_SLOTS)]
|
||||
tracks[0] = dash_objects.CameraObject(slot=0, object_id=9, d_rel=40.0, y_rel=0.0, is_lead_car=True, valid=True,
|
||||
car_type=7, rotation=0)
|
||||
msg = author.create(packer, 0, self.make_lead(prob=0.0), tracks, 1, 0.0)
|
||||
assert self.payload(msg)[1] >> 3 == 0
|
||||
|
||||
def test_adjacent_car_forwarded_with_own_mux(self):
|
||||
packer = CANPacker("honda_bosch_radarless_generated")
|
||||
tracks = [dash_objects.CameraObject(slot=i, object_id=0, d_rel=0.0, y_rel=0.0, is_lead_car=False, valid=False)
|
||||
for i in range(dash_objects.NUM_SLOTS)]
|
||||
tracks[3] = dash_objects.CameraObject(slot=3, object_id=12, d_rel=25.0, y_rel=3.0, is_lead_car=False, valid=True,
|
||||
car_type=7, rotation=1)
|
||||
msg = dash_objects.forward_hud_object(packer, 0, 20, tracks)
|
||||
assert msg[1][0] >> 2 == 20
|
||||
assert msg[1][1] >> 3 == 12
|
||||
|
||||
|
||||
class TestCameraObjectTracker:
|
||||
def test_tracks_persist_across_banks(self):
|
||||
tracker = dash_objects.CameraObjectTracker()
|
||||
|
||||
class FakeParser:
|
||||
vl_all = {"HUD_OBJECTS": {
|
||||
"MUX": [2, 18], "OBJECT_ID": [5, 5], "LONG_DIST": [30.0, 31.0], "LAT_DIST": [1.0, 1.1],
|
||||
"IS_LEAD_CAR": [0, 0], "CAR_TYPE": [7, 7], "ROTATION": [0, 0],
|
||||
}}
|
||||
tracker.update(FakeParser())
|
||||
snap = tracker.snapshot()
|
||||
assert snap[1].valid and snap[1].object_id == 5
|
||||
assert snap[1].d_rel == 31.0
|
||||
|
||||
def test_empty_sentinel_invalid(self):
|
||||
tracker = dash_objects.CameraObjectTracker()
|
||||
|
||||
class FakeParser:
|
||||
vl_all = {"HUD_OBJECTS": {
|
||||
"MUX": [1], "OBJECT_ID": [0], "LONG_DIST": [196.9], "LAT_DIST": [204.7],
|
||||
"IS_LEAD_CAR": [0], "CAR_TYPE": [-1], "ROTATION": [-128],
|
||||
}}
|
||||
tracker.update(FakeParser())
|
||||
assert not tracker.snapshot()[0].valid
|
||||
@@ -0,0 +1,18 @@
|
||||
import re
|
||||
|
||||
from iqdbc.car.honda.fingerprints import FW_VERSIONS
|
||||
from iqdbc.car.honda.values import HONDA_BOSCH, HONDA_BOSCH_TJA_CONTROL
|
||||
|
||||
HONDA_FW_VERSION_RE = br"[A-Z0-9]{5}(-|,)[A-Z0-9]{3}(-|,)[A-Z0-9]{4}(\x00){2}$"
|
||||
|
||||
|
||||
class TestHondaFingerprint:
|
||||
def test_fw_version_format(self):
|
||||
# Asserts all FW versions follow an expected format
|
||||
for fw_by_ecu in FW_VERSIONS.values():
|
||||
for fws in fw_by_ecu.values():
|
||||
for fw in fws:
|
||||
assert re.match(HONDA_FW_VERSION_RE, fw) is not None, fw
|
||||
|
||||
def test_tja_bosch_only(self):
|
||||
assert set(HONDA_BOSCH_TJA_CONTROL).issubset(set(HONDA_BOSCH)), "Nidec car found in TJA control list"
|
||||
@@ -0,0 +1,490 @@
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from iqdbc.can import CANParser
|
||||
from iqdbc.car.honda.radar_scan import (AGE_RAW_INVALID, ALL_SCAN_ADDRS, BEARING_RAW_INVALID, BEARING_ZERO,
|
||||
CLOSING_SPEED_RAW_INVALID, CLOSING_SPEED_RAW_ZERO,
|
||||
CLOSING_SPEED_SIGMA_TRUST_MAX, DIST_BIAS_M, DIST_LSB_M,
|
||||
DIST_RATIO_RAW_INVALID, DIST_RAW_INVALID, HondaRadarScanner,
|
||||
QUIET_TIMEOUT_S, SCAN_DBC_NAME, SCAN_SLOTS, STATE_INVALID,
|
||||
SWEEP_TRIGGER_ADDR, decode_closing_speed, decode_dist_ratio)
|
||||
from iqdbc.dbc.generator.honda.honda_radar_scan import FRAME_SIGNALS, frame_address
|
||||
|
||||
BUS = 2
|
||||
SWEEP_DT_NS = 66_000_000
|
||||
|
||||
|
||||
def set_bits(data, start_bit, size, value):
|
||||
value = int(value) & ((1 << size) - 1)
|
||||
pos = start_bit
|
||||
for i in range(size):
|
||||
bit = (value >> (size - 1 - i)) & 1
|
||||
byte_i, bit_i = pos // 8, pos % 8
|
||||
if bit:
|
||||
data[byte_i] |= (1 << bit_i)
|
||||
pos = pos - 1 if bit_i > 0 else pos + 15
|
||||
|
||||
|
||||
GEOMETRY = {kind: {name: (start, size) for name, start, size in sigs} for kind, sigs in FRAME_SIGNALS.items()}
|
||||
|
||||
|
||||
def build_frame(slot, kind, **fields):
|
||||
data = bytearray(8)
|
||||
for name, value in fields.items():
|
||||
set_bits(data, *GEOMETRY[kind][name], value)
|
||||
return (frame_address(slot, kind), bytes(data), BUS)
|
||||
|
||||
|
||||
def quartet(slot, cycle, dist_raw=1000, bearing_raw=BEARING_ZERO, state=1, dist_sigma=0, presence=40,
|
||||
age=100, handle=5):
|
||||
return [
|
||||
build_frame(slot, "POS", SCAN_STATE=state, CYCLE=cycle, DIST_RAW=dist_raw, BEARING_RAW=bearing_raw,
|
||||
DIST_SIGMA_RAW=dist_sigma),
|
||||
build_frame(slot, "SHAPE", CYCLE=cycle, PRESENCE_RAW=presence),
|
||||
build_frame(slot, "LIFE", CYCLE=cycle, AGE_RAW=age),
|
||||
build_frame(slot, "IDENT", CYCLE=cycle, OBJECT_HANDLE=handle),
|
||||
]
|
||||
|
||||
|
||||
def motion_frame(slot, cycle, speed_raw=CLOSING_SPEED_RAW_ZERO, sigma_raw=0, ratio_raw=500):
|
||||
return build_frame(slot, "MOTION", CYCLE=cycle, CLOSING_SPEED_RAW=speed_raw,
|
||||
CLOSING_SPEED_SIGMA_RAW=sigma_raw, DIST_RATIO_RAW=ratio_raw)
|
||||
|
||||
|
||||
def closing_sweep(slot_msgs, cycle):
|
||||
# slot 15's quartet closes every sweep so the trigger fires
|
||||
msgs = list(slot_msgs)
|
||||
if not any(m[0] == SWEEP_TRIGGER_ADDR for m in msgs):
|
||||
msgs += quartet(15, cycle, state=STATE_INVALID, dist_raw=DIST_RAW_INVALID,
|
||||
bearing_raw=BEARING_RAW_INVALID, age=AGE_RAW_INVALID, handle=0)
|
||||
return msgs
|
||||
|
||||
|
||||
class ScanHarness:
|
||||
def __init__(self):
|
||||
self.scanner = object.__new__(HondaRadarScanner)
|
||||
self.scanner.rcp = CANParser(SCAN_DBC_NAME, [(a, 15) for a in ALL_SCAN_ADDRS], BUS)
|
||||
self.scanner.trigger_msg = SWEEP_TRIGGER_ADDR
|
||||
self.scanner.pts = {}
|
||||
self.scanner._ledgers = {}
|
||||
self.scanner._slot_handles = [None] * SCAN_SLOTS
|
||||
self.scanner._last_sweep_nanos = -1
|
||||
self.updated = set()
|
||||
self.nanos = 0
|
||||
self.cycle = 0
|
||||
|
||||
def feed(self, msgs, dt_ns=SWEEP_DT_NS):
|
||||
self.nanos += dt_ns
|
||||
vls = self.scanner.rcp.update([self.nanos, list(msgs)])
|
||||
self.updated.update(vls)
|
||||
if self.scanner.trigger_msg not in self.updated:
|
||||
if self.scanner.sweep_overdue():
|
||||
return self.scanner.quiet_bus_radardata()
|
||||
return None
|
||||
result = self.scanner.process_sweep(self.updated)
|
||||
self.updated.clear()
|
||||
return result
|
||||
|
||||
def sweep(self, slot_msgs=(), cycle_step=1, dt_ns=SWEEP_DT_NS):
|
||||
self.cycle = (self.cycle + cycle_step) & 0xF
|
||||
return self.feed(closing_sweep(slot_msgs, self.cycle), dt_ns=dt_ns)
|
||||
|
||||
def object_sweep(self, slot=0, handle=5, dist_raw=1000, with_motion=True, cycle_step=1, age_step=None,
|
||||
dt_ns=SWEEP_DT_NS, **kwargs):
|
||||
if age_step is None:
|
||||
age_step = 2 * cycle_step
|
||||
self._age = (getattr(self, "_age", 100) + age_step) & 0xFFF
|
||||
cycle = (self.cycle + cycle_step) & 0xF
|
||||
msgs = quartet(slot, cycle, dist_raw=dist_raw, age=self._age, handle=handle, **kwargs)
|
||||
if with_motion:
|
||||
msgs.append(motion_frame(slot, cycle))
|
||||
return self.sweep(msgs, cycle_step=cycle_step, dt_ns=dt_ns)
|
||||
|
||||
|
||||
class TestFieldDecoding:
|
||||
def test_dist_conversion(self):
|
||||
assert DIST_LSB_M * 1000 + DIST_BIAS_M == pytest.approx(54.12)
|
||||
|
||||
def test_closing_speed_decode_and_domain(self):
|
||||
assert decode_closing_speed(CLOSING_SPEED_RAW_ZERO) == 0.0
|
||||
assert decode_closing_speed(CLOSING_SPEED_RAW_ZERO + 64) == 1.0
|
||||
assert decode_closing_speed(CLOSING_SPEED_RAW_INVALID) is None
|
||||
assert decode_closing_speed(1729) is None
|
||||
assert decode_closing_speed(None) is None
|
||||
|
||||
def test_closing_speed_sigma_veto(self):
|
||||
assert decode_closing_speed(CLOSING_SPEED_RAW_ZERO, CLOSING_SPEED_SIGMA_TRUST_MAX) == 0.0
|
||||
assert decode_closing_speed(CLOSING_SPEED_RAW_ZERO, CLOSING_SPEED_SIGMA_TRUST_MAX + 1) is None
|
||||
|
||||
def test_dist_ratio_decode(self):
|
||||
assert decode_dist_ratio(500) == pytest.approx(1.0)
|
||||
assert decode_dist_ratio(DIST_RATIO_RAW_INVALID) is None
|
||||
assert decode_dist_ratio(None) is None
|
||||
|
||||
def test_bearing_sign_convention(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep(bearing_raw=BEARING_ZERO + 100)
|
||||
result = h.object_sweep(bearing_raw=BEARING_ZERO + 100)
|
||||
assert result.points[0].yRel > 0 # left of center is positive
|
||||
dist = result.points[0].dRel
|
||||
assert result.points[0].yRel == pytest.approx(dist * math.tan(100 / 2048))
|
||||
|
||||
def test_bearing_right_of_center_is_negative(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep(bearing_raw=BEARING_ZERO - 100)
|
||||
result = h.object_sweep(bearing_raw=BEARING_ZERO - 100)
|
||||
assert result.points[0].yRel < 0
|
||||
|
||||
def test_boresight_is_zero(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep(bearing_raw=BEARING_ZERO)
|
||||
result = h.object_sweep(bearing_raw=BEARING_ZERO)
|
||||
assert result.points[0].yRel == 0.0
|
||||
|
||||
|
||||
class TestPublicationRules:
|
||||
def test_birth_is_withheld_until_second_observation(self):
|
||||
h = ScanHarness()
|
||||
result = h.object_sweep()
|
||||
assert len(result.points) == 0
|
||||
result = h.object_sweep()
|
||||
assert len(result.points) == 1
|
||||
point = result.points[0]
|
||||
assert point.trackId == 5
|
||||
assert point.measured
|
||||
assert math.isnan(point.aRel) and math.isnan(point.yvRel)
|
||||
|
||||
def test_handle_is_wire_identity_not_synthetic(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep(handle=0x22)
|
||||
result = h.object_sweep(handle=0x22)
|
||||
assert result.points[0].trackId == 0x22
|
||||
|
||||
@pytest.mark.parametrize("field,value", [("state", STATE_INVALID), ("dist_raw", DIST_RAW_INVALID),
|
||||
("bearing_raw", BEARING_RAW_INVALID)])
|
||||
def test_sentinels_invalidate_observation(self, field, value):
|
||||
h = ScanHarness()
|
||||
h.object_sweep()
|
||||
h.object_sweep()
|
||||
kwargs = {field: value}
|
||||
result = h.object_sweep(**kwargs)
|
||||
assert len(result.points) == 0
|
||||
|
||||
def test_age_sentinel_invalidates_observation(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep()
|
||||
h.object_sweep()
|
||||
cycle = (h.cycle + 1) & 0xF
|
||||
msgs = quartet(0, cycle, age=AGE_RAW_INVALID, handle=5) + [motion_frame(0, cycle)]
|
||||
result = h.sweep(msgs)
|
||||
assert len(result.points) == 0
|
||||
|
||||
@pytest.mark.parametrize("handle", [0, 0x40, 0xFF])
|
||||
def test_out_of_range_handle_invalidates(self, handle):
|
||||
h = ScanHarness()
|
||||
h.object_sweep()
|
||||
h.object_sweep()
|
||||
result = h.object_sweep(handle=handle)
|
||||
assert len(result.points) == 0
|
||||
|
||||
def test_incomplete_quartet_is_not_an_observation(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep()
|
||||
h.object_sweep()
|
||||
cycle = (h.cycle + 1) & 0xF
|
||||
h._age = (h._age + 2) & 0xFFF
|
||||
msgs = quartet(0, cycle, age=h._age, handle=5)[:3] # drop IDENT
|
||||
result = h.sweep(msgs)
|
||||
# a dropped CAN frame is not a lifecycle event: the published point persists untouched
|
||||
assert len(result.points) == 1
|
||||
result = h.object_sweep()
|
||||
assert len(result.points) == 1
|
||||
assert result.points[0].measured
|
||||
|
||||
def test_cycle_mismatch_across_quartet_is_incoherent(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep()
|
||||
h.object_sweep()
|
||||
cycle = (h.cycle + 1) & 0xF
|
||||
msgs = quartet(0, cycle, age=200, handle=5)
|
||||
bad_life = build_frame(0, "LIFE", CYCLE=(cycle + 1) & 0xF, AGE_RAW=200)
|
||||
msgs[2] = bad_life
|
||||
result = h.sweep(msgs)
|
||||
# an incoherent quartet is not an observation: the published point persists untouched
|
||||
assert len(result.points) == 1
|
||||
assert result.points[0].measured
|
||||
|
||||
|
||||
class TestLifecycle:
|
||||
def test_age_advances_two_per_cycle_keeps_identity(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep()
|
||||
h.object_sweep()
|
||||
result = h.object_sweep()
|
||||
assert len(result.points) == 1
|
||||
|
||||
def test_continuity_across_skipped_cycles(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep()
|
||||
h.object_sweep()
|
||||
result = h.object_sweep(cycle_step=3, age_step=6)
|
||||
assert len(result.points) == 1
|
||||
|
||||
def test_cycle_and_age_wraparound_stay_same_incarnation(self):
|
||||
h = ScanHarness()
|
||||
h.cycle = 14
|
||||
h._age = 4094
|
||||
h.object_sweep() # cycle 15, age 4094+2 wraps
|
||||
h.object_sweep() # cycle 0
|
||||
result = h.object_sweep()
|
||||
assert len(result.points) == 1
|
||||
|
||||
def test_lifecycle_break_starts_new_incarnation(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep()
|
||||
h.object_sweep()
|
||||
# same handle, age jumps arbitrarily: history must not carry over, so no publication this sweep
|
||||
result = h.object_sweep(age_step=500)
|
||||
assert len(result.points) == 0
|
||||
result = h.object_sweep()
|
||||
assert len(result.points) == 1
|
||||
|
||||
def test_death_then_rebirth_reuses_handle_with_clean_history(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep()
|
||||
h.object_sweep()
|
||||
for _ in range(4):
|
||||
h.sweep() # object absent long enough to expire its ledger
|
||||
result = h.object_sweep()
|
||||
assert len(result.points) == 0
|
||||
result = h.object_sweep()
|
||||
assert len(result.points) == 1
|
||||
|
||||
|
||||
class TestMotionPolicy:
|
||||
def test_native_speed_is_published(self):
|
||||
h = ScanHarness()
|
||||
speed_raw = CLOSING_SPEED_RAW_ZERO + 128
|
||||
cycle = (h.cycle + 1) & 0xF
|
||||
h.sweep(quartet(0, cycle, age=100, handle=5) + [motion_frame(0, cycle, speed_raw=speed_raw)])
|
||||
cycle = (h.cycle + 1) & 0xF
|
||||
result = h.sweep(quartet(0, cycle, age=102, handle=5) + [motion_frame(0, cycle, speed_raw=speed_raw)])
|
||||
assert result.points[0].vRel == pytest.approx(2.0)
|
||||
assert result.points[0].measured
|
||||
|
||||
def test_missing_motion_frame_never_invalidates_geometry(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep(with_motion=False)
|
||||
result = h.object_sweep(with_motion=False)
|
||||
# without any motion source and no held speed, the point is withheld rather than synthesized
|
||||
assert len(result.points) == 0
|
||||
|
||||
def test_stale_motion_cycle_is_ignored(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep()
|
||||
h.object_sweep()
|
||||
cycle = (h.cycle + 1) & 0xF
|
||||
h._age = (h._age + 2) & 0xFFF
|
||||
msgs = quartet(0, cycle, age=h._age, handle=5) + [motion_frame(0, (cycle - 1) & 0xF)]
|
||||
result = h.sweep(msgs)
|
||||
# motion from another cycle contributes nothing: coasts on held speed, unmeasured
|
||||
assert len(result.points) == 1
|
||||
assert not result.points[0].measured
|
||||
|
||||
def test_high_sigma_speed_coasts_instead_of_synthesizing(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep()
|
||||
h.object_sweep()
|
||||
cycle = (h.cycle + 1) & 0xF
|
||||
h._age = (h._age + 2) & 0xFFF
|
||||
msgs = quartet(0, cycle, age=h._age, handle=5) + \
|
||||
[motion_frame(0, cycle, sigma_raw=CLOSING_SPEED_SIGMA_TRUST_MAX + 1)]
|
||||
result = h.sweep(msgs)
|
||||
assert len(result.points) == 1
|
||||
assert not result.points[0].measured
|
||||
assert result.points[0].vRel == pytest.approx(0.0) # the held speed, not a derivative
|
||||
|
||||
def test_ratio_field_supplies_speed_when_native_missing(self):
|
||||
h = ScanHarness()
|
||||
dist_raw = 1000
|
||||
cycle = (h.cycle + 1) & 0xF
|
||||
h.sweep(quartet(0, cycle, dist_raw=dist_raw, age=100, handle=5) +
|
||||
[motion_frame(0, cycle, speed_raw=CLOSING_SPEED_RAW_INVALID, ratio_raw=490)])
|
||||
cycle = (h.cycle + 1) & 0xF
|
||||
result = h.sweep(quartet(0, cycle, dist_raw=dist_raw, age=102, handle=5) +
|
||||
[motion_frame(0, cycle, speed_raw=CLOSING_SPEED_RAW_INVALID, ratio_raw=490)])
|
||||
assert len(result.points) == 1
|
||||
dist = DIST_LSB_M * dist_raw + DIST_BIAS_M
|
||||
dt = SWEEP_DT_NS * 1e-9
|
||||
assert result.points[0].vRel == pytest.approx(dist * (1.0 - 0.99) / dt)
|
||||
assert result.points[0].measured
|
||||
|
||||
def test_fast_clean_range_rate_without_sources_is_withheld(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep(with_motion=False, dist_raw=1000)
|
||||
# large clean jump with no motion evidence: raw-rate limit rejects the range outright
|
||||
result = h.object_sweep(with_motion=False, dist_raw=3000)
|
||||
assert len(result.points) == 0
|
||||
|
||||
|
||||
class TestRangeAcceptance:
|
||||
def test_discontinuity_is_rejected_and_never_becomes_baseline(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep(dist_raw=1000)
|
||||
h.object_sweep(dist_raw=1002)
|
||||
# jump far beyond the hard innovation gate while claiming zero closing speed
|
||||
result = h.object_sweep(dist_raw=3000)
|
||||
assert len(result.points) == 1
|
||||
assert not result.points[0].measured
|
||||
# the rejected range did not become the derivative baseline: returning to the
|
||||
# consistent range publishes measured again
|
||||
result = h.object_sweep(dist_raw=1004)
|
||||
assert result.points[0].measured
|
||||
|
||||
def test_small_innovation_accepted(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep(dist_raw=1000)
|
||||
result = h.object_sweep(dist_raw=1005)
|
||||
assert result.points[0].measured
|
||||
|
||||
|
||||
class TestSlotsAndIdentity:
|
||||
def test_slot_migration_preserves_identity(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep(slot=2)
|
||||
h.object_sweep(slot=2)
|
||||
result = h.object_sweep(slot=9)
|
||||
assert len(result.points) == 1
|
||||
assert result.points[0].trackId == 5
|
||||
|
||||
def test_duplicate_identity_prefers_bound_slot(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep(slot=2, dist_raw=1000)
|
||||
h.object_sweep(slot=2, dist_raw=1002)
|
||||
cycle = (h.cycle + 1) & 0xF
|
||||
h._age = (h._age + 2) & 0xFFF
|
||||
msgs = quartet(2, cycle, dist_raw=1004, age=h._age, handle=5) + [motion_frame(2, cycle)] + \
|
||||
quartet(9, cycle, dist_raw=2000, age=h._age, handle=5) + [motion_frame(9, cycle)]
|
||||
result = h.sweep(msgs)
|
||||
assert len(result.points) == 1
|
||||
assert result.points[0].dRel == pytest.approx(DIST_LSB_M * 1004 + DIST_BIAS_M)
|
||||
|
||||
def test_slot_replacement_hides_old_occupant(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep(slot=3, handle=7)
|
||||
h.object_sweep(slot=3, handle=7)
|
||||
# a different identity takes the slot; the old one is hidden but not destroyed
|
||||
result = h.object_sweep(slot=3, handle=9, age_step=333)
|
||||
assert all(p.trackId != 7 for p in result.points)
|
||||
|
||||
def test_one_identity_never_two_points(self):
|
||||
h = ScanHarness()
|
||||
cycle = (h.cycle + 1) & 0xF
|
||||
msgs = quartet(1, cycle, age=100, handle=5) + [motion_frame(1, cycle)] + \
|
||||
quartet(6, cycle, age=100, handle=5) + [motion_frame(6, cycle)]
|
||||
h.sweep(msgs)
|
||||
cycle = (h.cycle + 1) & 0xF
|
||||
msgs = quartet(1, cycle, age=102, handle=5) + [motion_frame(1, cycle)] + \
|
||||
quartet(6, cycle, age=102, handle=5) + [motion_frame(6, cycle)]
|
||||
result = h.sweep(msgs)
|
||||
assert len(result.points) == 1
|
||||
|
||||
|
||||
class TestBusSilence:
|
||||
def test_quiet_bus_publishes_empty_not_none(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep()
|
||||
h.object_sweep()
|
||||
result = None
|
||||
for _ in range(30):
|
||||
result = h.feed([], dt_ns=10_000_000)
|
||||
if result is not None:
|
||||
break
|
||||
assert result is not None
|
||||
assert result.errors.radarUnavailableTemporary
|
||||
assert len(result.points) == 0
|
||||
|
||||
def test_recovery_after_silence_starts_fresh(self):
|
||||
h = ScanHarness()
|
||||
h.object_sweep()
|
||||
h.object_sweep()
|
||||
for _ in range(30):
|
||||
if h.feed([], dt_ns=10_000_000) is not None:
|
||||
break
|
||||
result = h.object_sweep()
|
||||
assert len(result.points) == 0
|
||||
result = h.object_sweep()
|
||||
assert len(result.points) == 1
|
||||
|
||||
def test_no_stale_publication_before_first_sweep(self):
|
||||
h = ScanHarness()
|
||||
for _ in range(50):
|
||||
assert h.feed([], dt_ns=10_000_000) is None
|
||||
|
||||
|
||||
class TestQuietTimeoutValue:
|
||||
def test_timeout_is_about_three_sweeps(self):
|
||||
assert QUIET_TIMEOUT_S == pytest.approx(3 / 15, abs=0.01)
|
||||
|
||||
|
||||
class TestScanInterfaceGating:
|
||||
def build(self, candidate, alpha_long=False, docs=False):
|
||||
from iqdbc.car import gen_empty_fingerprint
|
||||
from iqdbc.car.honda.interface import CarInterface
|
||||
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], alpha_long, False, docs)
|
||||
return CP
|
||||
|
||||
def test_verified_platform_has_radar(self):
|
||||
from iqdbc.car.honda.values import CAR
|
||||
for car in (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_ACCORD, CAR.HONDA_CRV_5G):
|
||||
assert not self.build(car).radarUnavailable
|
||||
|
||||
def test_radar_survives_openpilot_longitudinal(self):
|
||||
from iqdbc.car.honda.values import CAR
|
||||
CP = self.build(CAR.HONDA_CIVIC_BOSCH, alpha_long=True)
|
||||
assert CP.openpilotLongitudinalControl
|
||||
assert not CP.radarUnavailable
|
||||
|
||||
def test_unverified_family_platform_stays_off(self):
|
||||
from iqdbc.car.honda.values import CAR
|
||||
for car in (CAR.HONDA_E, CAR.HONDA_INSIGHT, CAR.HONDA_NBOX_2G, CAR.ACURA_RDX_3G, CAR.HONDA_CRV_HYBRID):
|
||||
assert self.build(car).radarUnavailable
|
||||
|
||||
def test_radarless_and_canfd_stay_off(self):
|
||||
from iqdbc.car.honda.values import CAR
|
||||
assert self.build(CAR.HONDA_CIVIC_2022).radarUnavailable
|
||||
assert self.build(CAR.HONDA_CRV_6G).radarUnavailable
|
||||
|
||||
def test_docs_never_claim_radar(self):
|
||||
from iqdbc.car.honda.values import CAR
|
||||
assert self.build(CAR.HONDA_CIVIC_BOSCH, docs=True).radarUnavailable
|
||||
|
||||
def test_radar_interface_routes_scanner(self):
|
||||
from iqdbc.car import gen_empty_fingerprint
|
||||
from iqdbc.car.honda.interface import CarInterface
|
||||
from iqdbc.car.honda.values import CAR
|
||||
CP = self.build(CAR.HONDA_CIVIC_BOSCH)
|
||||
CP_IQ = CarInterface.get_params_iq(CP, CAR.HONDA_CIVIC_BOSCH, gen_empty_fingerprint(), [], False, False, False)
|
||||
ri = CarInterface.RadarInterface(CP, CP_IQ)
|
||||
assert ri.scanner is not None
|
||||
assert ri.trigger_msg == SWEEP_TRIGGER_ADDR
|
||||
|
||||
def test_radar_interface_keeps_nidec_path(self):
|
||||
from iqdbc.car import gen_empty_fingerprint
|
||||
from iqdbc.car.honda.interface import CarInterface
|
||||
from iqdbc.car.honda.values import CAR
|
||||
CP = self.build(CAR.HONDA_CIVIC)
|
||||
CP_IQ = CarInterface.get_params_iq(CP, CAR.HONDA_CIVIC, gen_empty_fingerprint(), [], False, False, False)
|
||||
ri = CarInterface.RadarInterface(CP, CP_IQ)
|
||||
assert ri.scanner is None
|
||||
assert ri.trigger_msg == 0x445
|
||||
|
||||
def test_radar_interface_sleeps_when_unavailable(self):
|
||||
from iqdbc.car import gen_empty_fingerprint
|
||||
from iqdbc.car.honda.interface import CarInterface
|
||||
from iqdbc.car.honda.values import CAR
|
||||
CP = self.build(CAR.HONDA_E)
|
||||
CP_IQ = CarInterface.get_params_iq(CP, CAR.HONDA_E, gen_empty_fingerprint(), [], False, False, False)
|
||||
ri = CarInterface.RadarInterface(CP, CP_IQ)
|
||||
assert ri.scanner is None and ri.rcp is None
|
||||
@@ -0,0 +1,84 @@
|
||||
from iqdbc.can.dbc import DBC as DbcFile
|
||||
from iqdbc.car import Bus
|
||||
from iqdbc.car.honda.values import CAR, DBC, HONDA_RADAR_SCAN_CAPABLE, HONDA_RADAR_SCAN_VERIFIED
|
||||
from iqdbc.dbc.generator.honda.honda_radar_scan import (FRAME_SIGNALS, QUARTET_KINDS, SCAN_SLOTS,
|
||||
frame_address, motion_address, quartet_base_address)
|
||||
|
||||
SCAN_DBC_NAME = 'honda_radar_scan_generated'
|
||||
|
||||
|
||||
class TestScanAddressing:
|
||||
def test_quartet_bases(self):
|
||||
assert [quartet_base_address(s) for s in range(SCAN_SLOTS)] == \
|
||||
[0x280, 0x284, 0x288, 0x28C, 0x2D0, 0x2D4, 0x2D8, 0x2DC, 0x2E0, 0x2E4, 0x2E8, 0x2EC, 0x2F0, 0x2F4, 0x2F8, 0x2FC]
|
||||
|
||||
def test_motion_addresses(self):
|
||||
assert [motion_address(s) for s in range(SCAN_SLOTS)] == \
|
||||
[0x2C8, 0x2C9, 0x2CA, 0x2CB, 0x2CC, 0x2CD, 0x2CE, 0x2CF, 0x290, 0x291, 0x292, 0x293, 0x294, 0x295, 0x296, 0x297]
|
||||
|
||||
def test_eighty_unique_addresses(self):
|
||||
addrs = [frame_address(s, k) for s in range(SCAN_SLOTS) for k in (*QUARTET_KINDS, "MOTION")]
|
||||
assert len(addrs) == 80
|
||||
assert len(set(addrs)) == 80
|
||||
|
||||
def test_quartet_kind_order(self):
|
||||
for slot in range(SCAN_SLOTS):
|
||||
base = quartet_base_address(slot)
|
||||
assert [frame_address(slot, k) for k in QUARTET_KINDS] == [base, base + 1, base + 2, base + 3]
|
||||
|
||||
|
||||
class TestScanDbcGeometry:
|
||||
def setup_method(self):
|
||||
self.dbc = DbcFile(SCAN_DBC_NAME)
|
||||
|
||||
def geometry(self, addr):
|
||||
msg = self.dbc.addr_to_msg[addr]
|
||||
return {sig.name: (sig.start_bit, sig.size) for sig in msg.sigs.values()}
|
||||
|
||||
def test_every_frame_present_with_size_8(self):
|
||||
for slot in range(SCAN_SLOTS):
|
||||
for kind in (*QUARTET_KINDS, "MOTION"):
|
||||
msg = self.dbc.addr_to_msg[frame_address(slot, kind)]
|
||||
assert msg.name == f"RADAR_SCAN_{slot:02d}_{kind}"
|
||||
assert msg.size == 8
|
||||
|
||||
def test_bit_geometry_matches_spec(self):
|
||||
expected = {kind: {name: (start, size) for name, start, size in sigs} for kind, sigs in FRAME_SIGNALS.items()}
|
||||
for slot in range(SCAN_SLOTS):
|
||||
for kind in (*QUARTET_KINDS, "MOTION"):
|
||||
assert self.geometry(frame_address(slot, kind)) == expected[kind], (slot, kind)
|
||||
|
||||
def test_pos_frame_field_widths(self):
|
||||
geo = self.geometry(frame_address(0, "POS"))
|
||||
assert geo["DIST_RAW"] == (23, 12)
|
||||
assert geo["BEARING_RAW"] == (39, 11)
|
||||
assert geo["SCAN_STATE"] == (15, 4)
|
||||
assert geo["DIST_SIGMA_RAW"] == (7, 7)
|
||||
|
||||
def test_ident_handle_is_byte_six(self):
|
||||
geo = self.geometry(frame_address(0, "IDENT"))
|
||||
assert geo["OBJECT_HANDLE"] == (55, 8)
|
||||
|
||||
def test_motion_field_widths(self):
|
||||
geo = self.geometry(frame_address(0, "MOTION"))
|
||||
assert geo["CLOSING_SPEED_RAW"] == (7, 11)
|
||||
assert geo["CLOSING_SPEED_SIGMA_RAW"] == (23, 10)
|
||||
assert geo["DIST_RATIO_RAW"] == (55, 10)
|
||||
|
||||
def test_cycle_positions_per_kind(self):
|
||||
positions = {"POS": (27, 4), "SHAPE": (28, 4), "LIFE": (11, 4), "IDENT": (12, 4), "MOTION": (12, 4)}
|
||||
for kind, expected in positions.items():
|
||||
assert self.geometry(frame_address(3, kind))["CYCLE"] == expected
|
||||
|
||||
|
||||
class TestScanPlatformWiring:
|
||||
def test_scan_dbc_on_exactly_the_capable_family(self):
|
||||
for car in CAR:
|
||||
has_scan_dbc = DBC[car].get(Bus.radar) == SCAN_DBC_NAME
|
||||
assert has_scan_dbc == (car in HONDA_RADAR_SCAN_CAPABLE), car
|
||||
|
||||
def test_verified_platforms_are_capable(self):
|
||||
assert HONDA_RADAR_SCAN_VERIFIED <= HONDA_RADAR_SCAN_CAPABLE
|
||||
|
||||
def test_verified_set(self):
|
||||
assert HONDA_RADAR_SCAN_VERIFIED == {CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G}
|
||||
Reference in New Issue
Block a user