the bell

opendbc_repo/opendbc/car/hyundai/interface.py

@@ -7,7 +7,6 @@ from opendbc.car.hyundai.values import HyundaiFlags, CAR, CarControllerParams, \
                                                    CANFD_UNSUPPORTED_LONGITUDINAL_CAR, \
                                                    CANFD_SECURITYACCESS_CAR, \
                                                    CANFD_ANGLE_LONGITUDINAL_CAR, \
-                                                   CANFD_RADAR_LIVE_LONGITUDINAL_CAR, \
                                                    CANFD_RADAR_ECU_KEEPALIVE_CAR, \
                                                    RADAR_LIVE_LONGITUDINAL_CAR, \
                                                    UNSUPPORTED_LONGITUDINAL_CAR, HyundaiSafetyFlags, \
@@ -30,7 +29,7 @@ Ecu = structs.CarParams.Ecu
 
 
 def get_communication_control_request(car_fingerprint):
-  if car_fingerprint in CANFD_RADAR_ECU_KEEPALIVE_CAR:
+  if car_fingerprint in CANFD_RADAR_ECU_KEEPALIVE_CAR and car_fingerprint != CAR.HYUNDAI_IONIQ_5:
     return bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX,
                   uds.MESSAGE_TYPE.NORMAL])
 

opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py

@@ -130,13 +130,18 @@ def get_test_toggles() -> SimpleNamespace:
 
 
 class TestHyundaiFingerprint:
-  def test_ev6_uses_stock_hda2_communication_control_path(self):
+  def test_egmp_communication_control_paths(self):
     stock_request = bytes([0x28, 0x83, 0x01])
     radar_keepalive_request = bytes([0x28, 0x01, 0x01])
 
     assert CAR.KIA_EV6 in CANFD_RADAR_LIVE_LONGITUDINAL_CAR
     assert CAR.KIA_EV6 not in CANFD_RADAR_ECU_KEEPALIVE_CAR
     assert get_communication_control_request(CAR.KIA_EV6) == stock_request
+
+    assert CAR.HYUNDAI_IONIQ_5 in CANFD_RADAR_LIVE_LONGITUDINAL_CAR
+    assert CAR.HYUNDAI_IONIQ_5 in CANFD_RADAR_ECU_KEEPALIVE_CAR
+    assert get_communication_control_request(CAR.HYUNDAI_IONIQ_5) == stock_request
+
     assert get_communication_control_request(CAR.HYUNDAI_IONIQ_6) == radar_keepalive_request
 
   def test_carnival_hev_low_speed_torque_rate_limits(self):

selfdrive/controls/lib/latcontrol_torque.py

@@ -652,6 +652,10 @@ class LatControlTorque(LatControl):
         output_torque *= get_genesis_g70_angle_output_scale(CS.steeringAngleDeg, output_torque)
         low_speed_output_limit = get_genesis_g70_low_speed_output_limit(setpoint, CS.vEgo)
         output_torque = float(np.clip(output_torque, -low_speed_output_limit, low_speed_output_limit))
+        if not CS.steeringPressed:
+          output_torque = get_genesis_g70_stabilized_output(
+            output_torque, self.prev_output_torque, setpoint, desired_lateral_jerk, CS.vEgo, self.dt,
+          )
       elif self.is_genesis_gv70:
         output_torque *= get_genesis_gv70_center_output_scale(setpoint, CS.vEgo)
         output_torque *= get_genesis_gv70_low_speed_center_overshoot_scale(

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -335,6 +335,17 @@ GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR = 0.18
 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH = 0.15
 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK = 0.15
 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH = 0.10
+GENESIS_G70_OUTPUT_SMOOTHING_SPEED = 40.0 * CV.MPH_TO_MS
+GENESIS_G70_OUTPUT_SMOOTHING_SPEED_WIDTH = 6.0 * CV.MPH_TO_MS
+GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT = 0.42
+GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH = 0.14
+GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC = 0.30
+GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC = 0.10
+GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_RC = 0.10
+GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE = 0.04
+GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08
+GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.45
+GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC = 0.055
 GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN = 0.45
 GENESIS_G70_ANGLE_OUTPUT_TAPER_START = 70.0
 GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH = 6.0
@@ -3380,6 +3391,32 @@ def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_acc
   return 1.0 - reduction
 
 
+def get_genesis_g70_stabilized_output(output_torque: float, prev_output_torque: float,
+                                      desired_lateral_accel: float, desired_lateral_jerk: float,
+                                      v_ego: float, dt: float) -> float:
+  speed_weight = _sigmoid((max(v_ego, 0.0) - GENESIS_G70_OUTPUT_SMOOTHING_SPEED) /
+                          GENESIS_G70_OUTPUT_SMOOTHING_SPEED_WIDTH)
+  center_weight = _sigmoid((GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT - abs(desired_lateral_accel)) /
+                           GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH)
+  curve_weight = 1.0 - center_weight
+  response_time = (GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC * curve_weight +
+                   GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC * center_weight)
+
+  unwind_phase = -desired_lateral_accel * desired_lateral_jerk
+  unwind_weight = _sigmoid((unwind_phase - GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE) /
+                           GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH)
+  response_time += GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_RC * curve_weight * unwind_weight
+
+  changing_direction = (abs(desired_lateral_accel) >= GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT and
+                        prev_output_torque * desired_lateral_accel <= 0.0)
+  if changing_direction:
+    response_time = min(response_time, GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC)
+
+  output_alpha = dt / (max(response_time, 0.0) + dt)
+  smoothed_output = prev_output_torque + output_alpha * (output_torque - prev_output_torque)
+  return float(output_torque + speed_weight * (smoothed_output - output_torque))
+
+
 def _ioniq_5_sigmoid(x: float) -> float:
   return _sigmoid(x)
 

selfdrive/controls/tests/test_latcontrol.py

@@ -55,6 +55,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
   get_subaru_impreza_pid_output_scale,
   get_genesis_gv70_low_speed_center_overshoot_scale,
   get_genesis_g70_high_speed_transition_scale,
+  get_genesis_g70_stabilized_output,
   normalize_flm_overrides,
   set_flm_runtime_overrides,
 )
@@ -1736,6 +1737,38 @@ class TestLatControl:
     assert lac_log.active
     assert 0.0 < abs(output) <= get_genesis_g70_low_speed_output_limit(0.0, CS.vEgo)
 
+  def test_genesis_g70_output_stabilizer_is_speed_and_phase_aware(self):
+    low_speed = get_genesis_g70_stabilized_output(-0.2, 0.2, 0.1, -0.4, 5.0, DT_CTRL)
+    high_speed_center = get_genesis_g70_stabilized_output(-0.2, 0.2, 0.1, -0.4, 30.0, DT_CTRL)
+    high_speed_wind = get_genesis_g70_stabilized_output(0.1, 0.3, 0.8, 0.5, 30.0, DT_CTRL)
+    high_speed_unwind = get_genesis_g70_stabilized_output(0.1, 0.3, 0.8, -0.5, 30.0, DT_CTRL)
+    high_speed_direction_change = get_genesis_g70_stabilized_output(-0.3, 0.3, -0.8, -0.5, 30.0, DT_CTRL)
+
+    assert low_speed == pytest.approx(-0.2, abs=0.005)
+    assert abs(high_speed_center - 0.2) < abs(low_speed - 0.2)
+    assert high_speed_unwind > high_speed_wind > 0.1
+    assert abs(high_speed_direction_change - 0.3) > abs(high_speed_center - 0.2)
+
+  def test_genesis_g70_output_stabilizer_update_path(self, monkeypatch):
+    calls = []
+
+    def stabilized_output(output_torque, prev_output_torque, desired_lateral_accel,
+                          desired_lateral_jerk, v_ego, dt):
+      calls.append((output_torque, prev_output_torque, desired_lateral_accel,
+                    desired_lateral_jerk, v_ego, dt))
+      return 0.123
+
+    monkeypatch.setattr(latcontrol_torque, "get_genesis_g70_stabilized_output", stabilized_output)
+    controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.GENESIS_G70_2020)
+    CS.vEgo = 25.0
+    output, _, lac_log = controller.update(
+      True, CS, VM, params, False, 0.0002, False, 0.2, None, None, starpilot_toggles,
+    )
+
+    assert calls
+    assert lac_log.active
+    assert output == pytest.approx(-0.123)
+
   def test_ioniq_5_default_update_path(self):
     controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_5)
     CarInterface = interfaces[HYUNDAI.HYUNDAI_IONIQ_5]