IQ.Pilot Release Commit @ 0babf78

selfdrive/controls/lib/latcontrol_torque.py

@@ -10,8 +10,8 @@ import numpy as np
 from cereal import log, custom  # noqa: F401  (custom kept available for downstream imports)
 from iqdbc.car import structs
 from iqdbc.car.lateral import FRICTION_THRESHOLD, get_friction
-from iqdbc.iqpilot.car.interfaces import LatControlInputs
-from iqdbc.iqpilot.car.lateral_ext import get_friction as get_friction_in_torque_space
+from iqdbc.lvbs.car.interfaces import LatControlInputs
+from iqdbc.lvbs.car.iq_lateral import get_friction as get_friction_in_torque_space
 from openpilot.common.basedir import BASEDIR
 from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
 from openpilot.common.filter_simple import FirstOrderFilter
@@ -365,8 +365,12 @@ class NeuralNetworkFeedForward(PilotLateralBrain):
     super().__init__(lac_torque, CP, CP_IQ, CI)
     self.params = Params()
     self.enabled = self.params.get_bool("NeuralNetworkFeedForward")
-    self.has_nn_model = CP_IQ.iqLateralNet.model.path != MOCK_MODEL_PATH
-    self.model = NNTorqueModel(CP_IQ.iqLateralNet.model.path)
+    # NNFF applies only when a real trained model for this car is present on disk.
+    # No models shipped (or no match / MOCK) -> skip NNFF entirely and fall back to
+    # the stock torque feed-forward. Models are re-added as they are retrained.
+    self.has_nn_model = (CP_IQ.iqLateralNet.model.path != MOCK_MODEL_PATH
+                         and os.path.isfile(CP_IQ.iqLateralNet.model.path))
+    self.model = NNTorqueModel(CP_IQ.iqLateralNet.model.path) if self.has_nn_model else None
     self.pitch = FirstOrderFilter(0.0, 0.5, 0.01)
     self.pitch_last = 0.0
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -6,6 +6,7 @@ import cereal.messaging as messaging
 from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.common.constants import CV
 from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.params import Params, UnknownKeyName
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
@@ -31,6 +32,12 @@ LAUNCH_COMMIT_T = 3.5
 LAUNCH_MOVING_SPEED = 1.2
 LAUNCH_MAX_ACCEL = 1.5
 
+E2E_CRUISE_CONVERGENCE_TAU = 15.0
+E2E_CRUISE_ACCEL_MAX = 0.5
+E2E_MODEL_SPEED_HORIZON = 5.0
+E2E_ACCEL_INTENT_BP = [-0.05, 0.05]
+E2E_MODEL_SPEED_INTENT_BP = [-0.5, 0.0]
+
 # Lookup table for turns
 _A_TOTAL_MAX_V = [1.7, 3.2]
 _A_TOTAL_MAX_BP = [20., 40.]
@@ -69,6 +76,35 @@ def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt,
   return target_accel, cruise_should_stop
 
 
+def get_e2e_accel(v_ego, v_cruise, model_v, a_target, should_stop):
+  if should_stop or v_cruise <= v_ego or len(model_v) != len(T_IDXS_MPC):
+    return a_target
+
+  convergence_accel = min((v_cruise - v_ego) / E2E_CRUISE_CONVERGENCE_TAU, E2E_CRUISE_ACCEL_MAX)
+  if convergence_accel <= a_target:
+    return a_target
+
+  # Only help the model converge to cruise when both its immediate action and
+  # velocity trajectory show no active deceleration intent. The lead MPC and
+  # cruise candidates remain hard upper bounds on the final acceleration.
+  accel_intent = np.interp(a_target, E2E_ACCEL_INTENT_BP, [0.0, 1.0])
+  model_speed = np.interp(E2E_MODEL_SPEED_HORIZON, T_IDXS_MPC, model_v)
+  speed_intent = np.interp(model_speed - v_ego, E2E_MODEL_SPEED_INTENT_BP, [0.0, 1.0])
+  return float(np.interp(min(accel_intent, speed_intent), [0.0, 1.0], [a_target, convergence_accel]))
+
+
+def get_accel_candidates(e2e, has_lead, mpc_candidate, cruise_candidate, e2e_candidate):
+  candidates = []
+  # With no lead, the MPC follows a synthetic fast lead. It remains the ACC
+  # policy, but must not limit the model policy in full E2E.
+  if not e2e or has_lead:
+    candidates.append(mpc_candidate)
+  candidates.append(cruise_candidate)
+  if e2e:
+    candidates.append(e2e_candidate)
+  return candidates
+
+
 class LongitudinalPlanner(LongitudinalPlannerIQ):
   def __init__(self, CP, CP_IQ, init_v=0.0, init_a=0.0, dt=DT_MDL):
     self.CP = CP
@@ -85,6 +121,10 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
     self.output_a_target = 0.0
     self.output_should_stop = False
     self.launch_armed = False
+    try:
+      self.exp_speed_conv = Params().get_bool("expSpeedConv")
+    except UnknownKeyName:
+      self.exp_speed_conv = False
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
@@ -173,6 +213,8 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
     output_a_target_e2e, output_should_stop_e2e = self.apply_e2e_stop_distance(sm, v_ego, output_a_target_e2e, output_should_stop_e2e)
+    if self.is_e2e(sm) and self.exp_speed_conv and not self.mpc.status:
+      output_a_target_e2e = get_e2e_accel(v_ego, v_cruise, model_v, output_a_target_e2e, output_should_stop_e2e)
 
     if sm['carState'].standstill:
       self.launch_armed = True
@@ -193,10 +235,13 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
                                                           steer_angle_without_offset, self.CP, self.dt,
                                                           accel_coast, self.allow_throttle)
 
-    candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
-                  (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)]
-    if e2e:
-      candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e))
+    candidates = get_accel_candidates(
+      e2e,
+      self.mpc.status,
+      (output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
+      (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop),
+      (output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e),
+    )
 
     output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
     self.output_should_stop = any(should_stop for _, _, should_stop in candidates)

selfdrive/controls/radard.py

@@ -13,8 +13,7 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.common.simple_kalman import KF1D
 
 from iqdbc.car import structs
-from iqdbc.car.hyundai.values import HyundaiFlags
-from iqdbc.iqpilot.car.hyundai.values import HyundaiFlagsIQ
+from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiFlagsIQ
 from openpilot.iqpilot.selfdrive.controls.lib.custom_stop_distance import CustomStopDistance
 
 

selfdrive/controls/tests/test_latcontrol.py

@@ -26,7 +26,7 @@ class TestLatControl:
     CP = CarInterface.get_non_essential_params(car_name)
     CP_IQ = CarInterface.get_non_essential_params_iq(CP, car_name)
     CI = CarInterface(CP, CP_IQ)
-    iqpilot_interfaces.setup_interfaces(CI)
+    iqpilot_interfaces.apply_iq_car_config(CI)
     CP_IQ = convert_to_capnp(CP_IQ)
     VM = VehicleModel(CP)
 

selfdrive/controls/tests/test_latcontrol_torque_buffer.py

@@ -17,7 +17,7 @@ def get_controller(car_name):
   CP = CarInterface.get_non_essential_params(car_name)
   CP_IQ = CarInterface.get_non_essential_params_iq(CP, car_name)
   CI = CarInterface(CP, CP_IQ)
-  iqpilot_interfaces.setup_interfaces(CI)
+  iqpilot_interfaces.apply_iq_car_config(CI)
   CP_IQ = convert_to_capnp(CP_IQ)
   VM = VehicleModel(CP)
   controller = LatControlTorque(CP.as_reader(), CP_IQ.as_reader(), CI, DT_CTRL)

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -0,0 +1,54 @@
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource
+from openpilot.selfdrive.controls.lib.longitudinal_planner import get_accel_candidates, get_e2e_accel
+
+
+def model_velocity(v_ego, v_future):
+  return np.interp(T_IDXS, [T_IDXS[0], T_IDXS[-1]], [v_ego, v_future])
+
+
+class TestE2eCruiseConvergence:
+  def test_converges_when_model_wants_to_accelerate(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 25.0), 0.1, False) == pytest.approx(0.5)
+
+  def test_scales_down_near_cruise_speed(self):
+    assert get_e2e_accel(28.5, 30.0, model_velocity(28.5, 30.0), 0.0, False) == pytest.approx(0.05)
+
+  def test_preserves_active_model_deceleration(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 25.0), -0.05, False) == pytest.approx(-0.05)
+
+  def test_preserves_future_model_slowdown(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 18.0), 0.1, False) == pytest.approx(0.1)
+
+  @pytest.mark.parametrize("v_ego, v_cruise, should_stop", [
+    (30.0, 30.0, False),
+    (31.0, 30.0, False),
+    (20.0, 30.0, True),
+  ])
+  def test_never_overrides_cruise_or_stop(self, v_ego, v_cruise, should_stop):
+    assert get_e2e_accel(v_ego, v_cruise, model_velocity(v_ego, v_ego + 5.0), -0.2, should_stop) == pytest.approx(-0.2)
+
+
+class TestAccelCandidates:
+  MPC = (-0.2, LongitudinalPlanSource.lead0, True)
+  CRUISE = (0.5, LongitudinalPlanSource.cruise, False)
+  E2E = (0.1, LongitudinalPlanSource.e2e, False)
+
+  def test_e2e_without_lead_frees_model_from_mpc(self):
+    candidates = get_accel_candidates(True, False, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.CRUISE, self.E2E]
+    assert min(candidates, key=lambda c: c[0])[1] == LongitudinalPlanSource.e2e
+    assert not any(should_stop for _, _, should_stop in candidates)
+
+  def test_e2e_with_lead_keeps_mpc_safety_constraint(self):
+    candidates = get_accel_candidates(True, True, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.MPC, self.CRUISE, self.E2E]
+    assert min(candidates, key=lambda c: c[0])[1] == LongitudinalPlanSource.lead0
+    assert any(should_stop for _, _, should_stop in candidates)
+
+  def test_acc_without_lead_keeps_mpc_policy(self):
+    candidates = get_accel_candidates(False, False, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.MPC, self.CRUISE]