IQ.Pilot Release Commit @ bec7652

iqpilot/selfdrive/controls/lib/helpers/tests/__init__.py

@@ -0,0 +1,3 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""

iqpilot/selfdrive/controls/lib/helpers/tests/test_e2e_alerts.py

@@ -0,0 +1,133 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from types import SimpleNamespace
+
+import pytest
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import custom
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.helpers.e2e_alerts import (
+  EndToEndAlertEngine, CONFIRM_S, SETTLE_S, HORIZON_TAIL, PATH_SPEED_MPS, LEAD_SPEED_MPS, LEAD_GAP_M)
+
+E2E_CHIME = custom.IQOnroadEvent.EventName.e2eChime
+
+
+class _Events(list):
+  def add(self, name):
+    self.append(name)
+
+
+def _model(horizon):
+  msg = messaging.new_message('modelV2')
+  msg.modelV2.velocity.x = [0.0] * (33 - HORIZON_TAIL) + [horizon] * HORIZON_TAIL
+  return msg.as_reader().modelV2
+
+
+def _sm(*, v_ego=0.0, standstill=True, gas=False, enabled=False, cruise=False,
+        horizon=0.0, lead=None):
+  lead = lead or SimpleNamespace(status=False, dRel=0.0, vLead=0.0)
+  return {
+    'carState': SimpleNamespace(vEgo=v_ego, standstill=standstill, gasPressed=gas,
+                                cruiseState=SimpleNamespace(enabled=cruise)),
+    'selfdriveState': SimpleNamespace(enabled=enabled),
+    'radarState': SimpleNamespace(leadOne=lead),
+    'modelV2': _model(horizon),
+  }
+
+
+def _engine(path=True, lead=True):
+  engine = EndToEndAlertEngine()
+  engine._refresh_params = lambda: None
+  engine._on = {"path": path, "lead": lead}
+  return engine
+
+
+def _run(engine, sm, seconds):
+  events = _Events()
+  chimes = 0
+  for _ in range(int(seconds / DT_MDL)):
+    engine.update(sm, events)
+    chimes += events.count(E2E_CHIME)
+    events.clear()
+  return chimes
+
+
+def _lead(d_rel, v_lead=0.0):
+  return SimpleNamespace(status=True, dRel=d_rel, vLead=v_lead)
+
+
+def test_path_opens_chimes_once():
+  engine = _engine()
+  assert _run(engine, _sm(horizon=0.0), SETTLE_S + 1.0) == 0
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), CONFIRM_S + 1.0) == 1
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), 5.0) == 0
+
+
+def test_path_needs_the_settle_dwell():
+  engine = _engine()
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), SETTLE_S - 0.2) == 0
+
+
+def test_path_silent_while_openpilot_long_is_engaged():
+  engine = _engine()
+  _run(engine, _sm(horizon=0.0, enabled=True), SETTLE_S + 1.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0, enabled=True), 5.0) == 0
+
+
+def test_path_silent_while_stock_acc_holds_the_car():
+  engine = _engine()
+  _run(engine, _sm(horizon=0.0, cruise=True), SETTLE_S + 1.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0, cruise=True), 5.0) == 0
+
+
+def test_path_chimes_under_aol():
+  engine = _engine()
+  _run(engine, _sm(horizon=0.0), SETTLE_S + 1.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), CONFIRM_S + 1.0) == 1
+
+
+def test_path_ignores_a_visible_lead():
+  engine = _engine(lead=False)
+  _run(engine, _sm(horizon=0.0, lead=_lead(6.0)), SETTLE_S + 1.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0, lead=_lead(6.0)), 5.0) == 0
+
+
+def test_lead_pullaway_chimes_once():
+  engine = _engine()
+  assert _run(engine, _sm(lead=_lead(6.0)), SETTLE_S + 1.0) == 0
+  moving = _sm(lead=_lead(6.0 + LEAD_GAP_M + 0.5, LEAD_SPEED_MPS + 1.0))
+  assert _run(engine, moving, CONFIRM_S + 1.0) == 1
+  assert _run(engine, moving, 5.0) == 0
+
+
+def test_lead_creep_inside_the_gap_stays_silent():
+  engine = _engine()
+  _run(engine, _sm(lead=_lead(6.0)), SETTLE_S + 1.0)
+  assert _run(engine, _sm(lead=_lead(6.0 + LEAD_GAP_M / 2, LEAD_SPEED_MPS + 1.0)), 5.0) == 0
+
+
+def test_lead_far_ahead_is_not_a_queue():
+  engine = _engine()
+  _run(engine, _sm(lead=_lead(40.0)), SETTLE_S + 1.0)
+  assert _run(engine, _sm(lead=_lead(44.0, LEAD_SPEED_MPS + 1.0)), 5.0) == 0
+
+
+def test_gas_and_motion_rearm_the_dwell():
+  engine = _engine()
+  _run(engine, _sm(horizon=0.0), SETTLE_S + 1.0)
+  _run(engine, _sm(v_ego=5.0, standstill=False, horizon=8.0), 2.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), SETTLE_S - 0.2) == 0
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), CONFIRM_S + 1.0) == 1
+
+
+@pytest.mark.parametrize("param_off", ["path", "lead"])
+def test_each_param_gates_only_its_own_trigger(param_off):
+  engine = _engine(path=param_off != "path", lead=param_off != "lead")
+  if param_off == "path":
+    _run(engine, _sm(horizon=0.0), SETTLE_S + 1.0)
+    assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), 5.0) == 0
+  else:
+    _run(engine, _sm(lead=_lead(6.0)), SETTLE_S + 1.0)
+    assert _run(engine, _sm(lead=_lead(8.0, LEAD_SPEED_MPS + 1.0)), 5.0) == 0

iqpilot/selfdrive/controls/lib/helpers/tests/test_nav_torque_pulse.py

@@ -0,0 +1,105 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from iqpilot.cereal import custom
+import iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse as nav_pulse
+from iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse import (
+  NavTorquePulseBrain, TURN_PULSE_FRAMES, EXIT_PULSE_FRAMES)
+
+
+@pytest.fixture
+def influence_on():
+  nav_pulse.IQP_NAV_TORQUE_INFLUENCE_ENABLED = True
+  try:
+    yield
+  finally:
+    nav_pulse.IQP_NAV_TORQUE_INFLUENCE_ENABLED = False
+
+
+def _fixed_nav_sm(**fields):
+  nav = SimpleNamespace(
+    maneuverPhase=custom.IQNavState.ManeuverPhase.none,
+    maneuverDirection=custom.NavDirection.none,
+    shouldSendTurnDesire=False,
+    turnDesireDirection=0,
+    shouldSendLanePositioning=False,
+    lanePositioningDirection=0,
+  )
+  for k, v in fields.items():
+    setattr(nav, k, v)
+
+  class SM:
+    def update(self, _):
+      return None
+
+    def __getitem__(self, _):
+      return nav
+  return SM()
+
+
+def _brain(nav_sm=None, steer_max=1.0):
+  brain = NavTorquePulseBrain(SimpleNamespace(steer_max=steer_max))
+  if nav_sm is not None:
+    brain._nav_sm = nav_sm
+  return brain
+
+
+def test_passthrough_when_disabled():
+  brain = _brain()
+  cs = SimpleNamespace(steeringPressed=False)
+  assert brain.nudge_output_torque(True, cs, 0.42) == 0.42
+
+
+class TestPulseSign:
+  @pytest.mark.parametrize("direction,expect_negative", [(1, True), (2, False)])
+  def test_turn_desire_direction(self, influence_on, direction, expect_negative):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=direction))
+    cs = SimpleNamespace(steeringPressed=False)
+    first = brain.nudge_output_torque(True, cs, 0.0)
+    assert (first < 0.0) == expect_negative
+
+  def test_turn_active_phase_uses_turn_frames(self, influence_on):
+    brain = _brain(_fixed_nav_sm(maneuverPhase=custom.IQNavState.ManeuverPhase.turnActive,
+                                 turnDesireDirection=1))
+    cs = SimpleNamespace(steeringPressed=False)
+    outs = [brain.nudge_output_torque(True, cs, 0.0) for _ in range(TURN_PULSE_FRAMES + 2)]
+    assert outs[TURN_PULSE_FRAMES - 1] < 0.0
+    assert outs[TURN_PULSE_FRAMES] == 0.0
+
+
+class TestPulseLifecycle:
+  def test_pulse_expires_after_its_frame_count(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=1))
+    cs = SimpleNamespace(steeringPressed=False)
+    outs = [brain.nudge_output_torque(True, cs, 0.0) for _ in range(TURN_PULSE_FRAMES + 3)]
+    assert all(o < 0.0 for o in outs[:TURN_PULSE_FRAMES])
+    assert all(o == 0.0 for o in outs[TURN_PULSE_FRAMES:])
+    assert all(np.isfinite(o) for o in outs)
+
+  def test_steering_press_suppresses_pulse(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=1))
+    cs = SimpleNamespace(steeringPressed=True)
+    assert brain.nudge_output_torque(True, cs, 0.4) == 0.4
+
+  def test_inactive_suppresses_pulse(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=1))
+    cs = SimpleNamespace(steeringPressed=False)
+    assert brain.nudge_output_torque(False, cs, 0.4) == 0.4
+
+  def test_output_clamped_to_steer_max(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=2), steer_max=0.5)
+    cs = SimpleNamespace(steeringPressed=False)
+    out = brain.nudge_output_torque(True, cs, 0.4)  # 0.4 + 0.8 nudge, clamped to 0.5
+    assert out == pytest.approx(0.5)
+
+  def test_lane_positioning_uses_exit_frames(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendLanePositioning=True, lanePositioningDirection=1))
+    cs = SimpleNamespace(steeringPressed=False)
+    outs = [brain.nudge_output_torque(True, cs, 0.0) for _ in range(EXIT_PULSE_FRAMES + 2)]
+    assert outs[EXIT_PULSE_FRAMES - 1] < 0.0
+    assert outs[EXIT_PULSE_FRAMES] == 0.0