Quintessence: A Quest

opendbc_repo/opendbc/car/volvo/carcontroller.py

@@ -248,12 +248,12 @@ class CarController(CarControllerBase):
     # LCA_5 (formerly SPEED_1) - 0x67 - 50 Hz
     # Contains wheel speeds + LCA signals (LCA_TURN_BITS, LCA_5_STEER)
     if self.frame % 2 == 0: # 50 Hz
-      # Initialize counter from CarState on first run
-      if self.lca_5_counter is None:
+      if not lat_active or self.lca_5_counter is None:
         self.lca_5_counter = CS.msg_lca_5['COUNTER']
 
       # Increment counter by +4, wrap at 15 (0xF never used)
-      self.lca_5_counter = (self.lca_5_counter + 4) % 15
+      if lat_active:
+        self.lca_5_counter = (self.lca_5_counter + 4) % 15
 
       can_sends.append(create_lca_5_message(self.packer, lat_active, apply_angle,
                                             CS.msg_lca_5, self.lca_5_counter))

opendbc_repo/opendbc/car/volvo/tests/test_controller.py

@@ -3,6 +3,7 @@ from types import SimpleNamespace
 
 import pytest
 
+from opendbc.can.parser import CANParser
 from opendbc.car.volvo.carcontroller import CarController
 from opendbc.car.volvo.helpers import checksum_lca_5_message
 from opendbc.car.volvo.interface import CarInterface
@@ -56,21 +57,36 @@ def test_controller_emits_valid_eight_byte_messages_and_lca5_checksum():
   assert data[2] == checksum_lca_5_message(data[0], data[1], data[3], data[4], data[5])
 
 
-def test_controller_relays_stock_lca5_angle_when_inactive():
-  cp = CarInterface.get_non_essential_params("VOLVO_XC40_RECHARGE")
+@pytest.mark.parametrize("fingerprint", [CAR.POLESTAR_2, CAR.VOLVO_XC40_RECHARGE])
+def test_controller_relays_complete_stock_lca5_when_inactive(fingerprint):
+  cp = CarInterface.get_non_essential_params(fingerprint)
   controller = CarController(DBC[cp.carFingerprint], cp)
+  parser = CANParser("volvo_mid_1", [("LCA_5", 50)], 0)
+  stock_data = bytes.fromhex("88c04cef1190ba00")
+  parser.update([0, [(0x67, stock_data, 0)]])
   cs = _state()
-  cs.msg_lca_5["LCA_5_STEER"] = 12.0
+  cs.msg_lca_5 = parser.vl["LCA_5"]
   cc = SimpleNamespace(latActive=False, actuators=_Actuators())
+  safety = libsafety_py.libsafety
+  config = cp.safetyConfigs[0]
+  assert safety.set_safety_hooks(config.safetyModel.raw, config.safetyParam) == 0
+  safety.init_tests()
+  safety.set_controls_allowed(False)
+  safety.safety_rx_hook(libsafety_py.make_CANPacket(0x67, 0, stock_data))
 
   _, can_sends = controller.update(cc, cs, 0, None)
   lca5 = next(msg for msg in can_sends if msg[0] == 0x67)
+  assert lca5[1] == stock_data
+  assert safety.safety_tx_hook(libsafety_py.make_CANPacket(lca5[0], lca5[2], lca5[1]))
 
-  # The inactive path must not manufacture a new angle command.
-  raw = ((lca5[1][6] & 0x7F) << 8) | lca5[1][7]
-  if raw & (1 << 14):
-    raw -= 1 << 15
-  assert abs(raw * 0.05596 - 12.0) < 0.1
+  cs.msg_lca_5["COUNTER"] = 7
+  controller.update(cc, cs, 0, None)
+  controller.update(cc, cs, 0, None)
+  controller.update(cc, cs, 0, None)
+  cc.latActive = True
+  _, can_sends = controller.update(cc, cs, 0, None)
+  active_lca5 = next(msg for msg in can_sends if msg[0] == 0x67)
+  assert active_lca5[1][3] >> 4 == 11
 
 
 @pytest.mark.parametrize("fingerprint", [CAR.POLESTAR_2, CAR.VOLVO_XC40_RECHARGE])

opendbc_repo/opendbc/car/volvo/volvocan.py

@@ -250,6 +250,9 @@ def create_lca_5_message(packer, lat_active: bool, target_angle_deg: float, msg_
     CAN message for LCA_5 on bus 2
   """
 
+  if not lat_active:
+    return packer.make_can_msg('LCA_5', 2, msg_lca_5)
+
   # DBC defines LCA_5_STEER as 15-bit signed with scale 0.05596 deg/count
   # Packer handles the encoding automatically - just pass the angle in degrees
 
@@ -261,7 +264,7 @@ def create_lca_5_message(packer, lat_active: bool, target_angle_deg: float, msg_
     'WHEEL_SPEED_2': msg_lca_5['WHEEL_SPEED_2'],
     'NEW_SIGNAL_5': msg_lca_5['NEW_SIGNAL_5'],
     'NEW_SIGNAL_2': msg_lca_5['NEW_SIGNAL_2'],
-    'LCA_5_STEER': target_angle_deg if lat_active else msg_lca_5['LCA_5_STEER'],
+    'LCA_5_STEER': target_angle_deg,
     'COUNTER': counter,
   }
 

opendbc_repo/opendbc/safety/modes/volvo.h

@@ -71,6 +71,27 @@ static uint16_t volvo_ecm_1_addr;
 static uint16_t volvo_bus1_cruise_control_addr;
 static bool volvo_c1;
 
+#define VOLVO_STOCK_LCA5_FRAMES 4U
+#define VOLVO_STOCK_LCA5_MAX_AGE_US 100000U
+static uint8_t volvo_stock_lca5_data[VOLVO_STOCK_LCA5_FRAMES][8];
+static uint32_t volvo_stock_lca5_ts[VOLVO_STOCK_LCA5_FRAMES];
+static bool volvo_stock_lca5_valid[VOLVO_STOCK_LCA5_FRAMES];
+static uint8_t volvo_stock_lca5_index;
+
+static bool volvo_lca5_stock_relay(const CANPacket_t *msg) {
+  const uint32_t now = microsecond_timer_get();
+  bool matches_stock = false;
+  for (uint8_t i = 0U; i < VOLVO_STOCK_LCA5_FRAMES; i++) {
+    bool matches = volvo_stock_lca5_valid[i] &&
+                   (safety_get_ts_elapsed(now, volvo_stock_lca5_ts[i]) <= VOLVO_STOCK_LCA5_MAX_AGE_US);
+    for (uint8_t byte = 0U; byte < 8U; byte++) {
+      matches &= msg->data[byte] == volvo_stock_lca5_data[i][byte];
+    }
+    matches_stock |= matches;
+  }
+  return matches_stock;
+}
+
 static int volvo_be_15(const CANPacket_t *msg, uint8_t byte) {
   return (int)(((uint16_t)(msg->data[byte] & 0x7FU) << 8U) | msg->data[byte + 1U]);
 }
@@ -159,6 +180,15 @@ static void volvo_rx_hook(const CANPacket_t *msg) {
 
   // Main bus (bus 0) messages
   if (msg->bus == VOLVO_MAIN_BUS) {
+    if (msg->addr == VOLVO_LCA_5) {
+      for (uint8_t byte = 0U; byte < 8U; byte++) {
+        volvo_stock_lca5_data[volvo_stock_lca5_index][byte] = msg->data[byte];
+      }
+      volvo_stock_lca5_ts[volvo_stock_lca5_index] = microsecond_timer_get();
+      volvo_stock_lca5_valid[volvo_stock_lca5_index] = true;
+      volvo_stock_lca5_index = (volvo_stock_lca5_index + 1U) % VOLVO_STOCK_LCA5_FRAMES;
+    }
+
     // Update brake pedal and cruise state from BCM2
     if (msg->addr == VOLVO_LCA_2) {
       // DBC: SG_ BRAKE_PEDAL_PRESSED_A : 47|1@0+ (-1,1) - inverted in DBC, so we invert raw bit
@@ -263,9 +293,13 @@ static bool volvo_tx_hook(const CANPacket_t *msg) {
   // LCA frame also contains an angle-shaped field, but the imported controller
   // deliberately leaves that field at the observed vehicle value.
   if (msg->addr == VOLVO_LCA_5) {
-    const int desired_angle = volvo_lca_5_angle(msg);
-    tx &= SAFETY_ABS(desired_angle) <= VOLVO_MAX_ANGLE_CAN;
-    tx &= !steer_angle_cmd_checks(desired_angle, controls_allowed, VOLVO_ANGLE_STEERING_LIMITS);
+    if (!controls_allowed && volvo_lca5_stock_relay(msg)) {
+      desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -VOLVO_MAX_ANGLE_CAN, VOLVO_MAX_ANGLE_CAN);
+    } else {
+      const int desired_angle = volvo_lca_5_angle(msg);
+      tx &= SAFETY_ABS(desired_angle) <= VOLVO_MAX_ANGLE_CAN;
+      tx &= !steer_angle_cmd_checks(desired_angle, controls_allowed, VOLVO_ANGLE_STEERING_LIMITS);
+    }
   }
 
   // Keep the two torque-authority arms and the companion LCA angle bounded even
@@ -357,6 +391,10 @@ static bool volvo_tx_hook(const CANPacket_t *msg) {
 static safety_config volvo_init(uint16_t param) {
   bool spa = GET_FLAG(param, VOLVO_FLAG_SPA);
   volvo_c1 = GET_FLAG(param, VOLVO_FLAG_C1);
+  volvo_stock_lca5_index = 0U;
+  for (uint8_t i = 0U; i < VOLVO_STOCK_LCA5_FRAMES; i++) {
+    volvo_stock_lca5_valid[i] = false;
+  }
 
   if (volvo_c1) {
     static const CanMsg VOLVO_C1_TX_MSGS[] = {

opendbc_repo/opendbc/safety/tests/test_volvo.py

@@ -177,6 +177,68 @@ class TestVolvoSafetyBase(common.CarSafetyTest):
     self.assertTrue(self._tx(self._angle_cmd_msg(10)))
     self.assertFalse(self._tx(self._angle_cmd_msg(20)))
 
+  STOCK_LCA5 = bytes.fromhex("88c04cef1190ba00")
+
+  def _stock_lca5(self, data=None, bus=VOLVO_PARTY_BUS):
+    return libsafety_py.make_CANPacket(VOLVO_LCA_5, bus, self.STOCK_LCA5 if data is None else data)
+
+  def test_inactive_lca5_stock_relay_requires_unchanged_received_frame(self):
+    self._reset_angle_measurement(10)
+    self.safety.set_timer(100000)
+    self.assertFalse(self._tx(self._stock_lca5()))
+    self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS))
+    self.assertTrue(self._tx(self._stock_lca5()))
+    self.assertEqual(self.safety.get_desired_angle_last(), round(10 / 0.05596))
+
+    for byte in range(8):
+      altered = bytearray(self.STOCK_LCA5)
+      altered[byte] ^= 1
+      self.assertFalse(self._tx(self._stock_lca5(bytes(altered))), f"altered {byte=}")
+    self.assertFalse(self._tx(self._stock_lca5(bus=VOLVO_MAIN_BUS)))
+    self.assertFalse(self._tx(self._stock_lca5(bus=VOLVO_PT_BUS)))
+
+  def test_inactive_lca5_stock_relay_wrong_rx_bus_and_length(self):
+    self._rx(self._stock_lca5(bus=VOLVO_PT_BUS))
+    self._rx(self._stock_lca5(self.STOCK_LCA5[:7], bus=VOLVO_MAIN_BUS))
+    self.assertFalse(self._tx(self._stock_lca5()))
+
+  def test_inactive_lca5_stock_relay_expires_across_timer_wrap(self):
+    start = 0xFFFF0000
+    self.safety.set_timer(start)
+    self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS))
+    self.safety.set_timer((start + 100000) & 0xFFFFFFFF)
+    self.assertTrue(self._tx(self._stock_lca5()))
+    self.safety.set_timer((start + 100001) & 0xFFFFFFFF)
+    self.assertFalse(self._tx(self._stock_lca5()))
+
+  def test_inactive_lca5_stock_relay_history_is_bounded(self):
+    frames = []
+    for i in range(5):
+      data = bytearray(self.STOCK_LCA5)
+      data[3] = (data[3] + i) & 0xFF
+      frames.append(bytes(data))
+      self.safety.set_timer(i * 20000)
+      self._rx(self._stock_lca5(frames[-1], bus=VOLVO_MAIN_BUS))
+    self.assertFalse(self._tx(self._stock_lca5(frames[0])))
+    for data in frames[1:]:
+      self.assertTrue(self._tx(self._stock_lca5(data)))
+
+  def test_inactive_lca5_stock_relay_cleared_on_safety_init(self):
+    self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS))
+    self.assertTrue(self._tx(self._stock_lca5()))
+    self.safety.set_safety_hooks(SAFETY_VOLVO, self.SAFETY_PARAM)
+    self.assertFalse(self._tx(self._stock_lca5()))
+
+  def test_stock_lca5_placeholder_does_not_bypass_active_angle_limits(self):
+    self._reset_angle_measurement(10)
+    self._reset_speed_measurement(50)
+    self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS))
+    self.assertTrue(self._tx(self._stock_lca5()))
+    self.safety.set_controls_allowed(True)
+    self.assertFalse(self._tx(self._stock_lca5()))
+    self.assertTrue(self._tx(self._angle_cmd_msg(10.4)))
+    self.assertFalse(self._tx(self._angle_cmd_msg(11)))
+
   def test_angle_tx_rate_matches_controller_cadence(self):
     """LCA_5 is 50 Hz, so each frame may contain two 100 Hz controller steps."""
     self._reset_speed_measurement(50)

selfdrive/controls/lib/latcontrol_pid.py

@@ -8,6 +8,7 @@ from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
   RAV4_TSS2_CARS,
   SUBARU_IMPREZA_CARS,
+  get_honda_crv_5g_pid_kp_scale,
   get_honda_crv_5g_pid_output,
   get_rav4_tss2_pid_output,
   get_subaru_impreza_pid_output_scale,
@@ -105,6 +106,7 @@ class LatControlPID(LatControl):
     self.honda_lateral_pid_ki_scale = 1.0
     self.is_civic_bosch_modified = CP.carFingerprint == HONDA.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED)
     self.is_honda_crv_5g = CP.carFingerprint == HONDA.HONDA_CRV_5G
+    self.is_honda_crv_5g_stock_eps = self.is_honda_crv_5g and not bool(CP.flags & HondaFlags.EPS_MODIFIED)
     self.is_subaru_impreza = CP.carFingerprint in SUBARU_IMPREZA_CARS
     self.is_rav4_tss2 = CP.carFingerprint in RAV4_TSS2_CARS
     self.prev_angle_steers_des_no_offset = 0.0
@@ -163,6 +165,10 @@ class LatControlPID(LatControl):
 
       freeze_integrator = steer_limited_by_safety or steering_pressed or CS.vEgo < 5
 
+      if self.is_honda_crv_5g_stock_eps:
+        kp_scale = self.honda_lateral_pid_kp_scale * get_honda_crv_5g_pid_kp_scale(angle_steers_des_no_offset, CS.vEgo)
+        self.pid._k_p = [self.base_kp_bp, scale_lateral_pid_gain_values(self.base_kp_v, kp_scale)]
+
       output_torque = self.pid.update(error,
                                 feedforward=ff,
                                 speed=CS.vEgo,

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -1261,6 +1261,9 @@ HONDA_CRV_5G_PID_CENTER_ANGLE = 14.0
 HONDA_CRV_5G_PID_CENTER_ANGLE_WIDTH = 3.0
 HONDA_CRV_5G_PID_OUTPUT_SCALE_MIN = 0.62
 HONDA_CRV_5G_PID_OUTPUT_ALPHA_MIN = 0.28
+HONDA_CRV_5G_PID_CENTER_KP_SCALE_MIN = 0.50
+HONDA_CRV_5G_PID_CENTER_KP_SPEED_BP = [11.0 * CV.MPH_TO_MS, 18.0 * CV.MPH_TO_MS]
+HONDA_CRV_5G_PID_CENTER_KP_ANGLE_BP = [6.0, 18.0]
 
 RAV4_TSS2_CENTER_FRICTION_THRESHOLD_GAIN = 0.14
 RAV4_TSS2_CENTER_FRICTION_LAT = 0.30
@@ -1905,6 +1908,12 @@ def get_rav4_tss2_pid_output(output_torque: float, prev_output_torque: float,
   return float(prev_output_torque + output_alpha * (limited_output - prev_output_torque))
 
 
+def get_honda_crv_5g_pid_kp_scale(desired_angle_deg: float, v_ego: float) -> float:
+  speed_weight = np.interp(max(v_ego, 0.0), HONDA_CRV_5G_PID_CENTER_KP_SPEED_BP, [1.0, 0.0])
+  center_weight = np.interp(abs(desired_angle_deg), HONDA_CRV_5G_PID_CENTER_KP_ANGLE_BP, [1.0, 0.0])
+  return float(1.0 - (1.0 - HONDA_CRV_5G_PID_CENTER_KP_SCALE_MIN) * speed_weight * center_weight)
+
+
 def get_honda_crv_5g_pid_output(output_torque: float, prev_output_torque: float,
                                 desired_angle_deg: float, v_ego: float) -> float:
   """Damp low-speed CR-V 5G center reversals without blunting real turns."""

selfdrive/controls/tests/test_honda_crv_pid_gain.py

@@ -0,0 +1,61 @@
+import pytest
+
+from openpilot.common.constants import CV
+from openpilot.common.pid import PIDController
+from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import get_honda_crv_5g_pid_kp_scale
+
+
+@pytest.mark.parametrize('mph', [0.0, 3.0, 6.0, 8.0, 11.0])
+@pytest.mark.parametrize('angle', [-6.0, -3.0, 0.0, 3.0, 6.0])
+def test_low_speed_center_gain(mph, angle):
+  assert get_honda_crv_5g_pid_kp_scale(angle, mph * CV.MPH_TO_MS) == 0.5
+
+
+@pytest.mark.parametrize('mph', [18.0, 25.0, 45.0, 70.0])
+@pytest.mark.parametrize('angle', [-30.0, -6.0, 0.0, 6.0, 30.0])
+def test_normal_speed_gain_unchanged(mph, angle):
+  assert get_honda_crv_5g_pid_kp_scale(angle, mph * CV.MPH_TO_MS) == 1.0
+
+
+@pytest.mark.parametrize('mph', [0.0, 8.0, 14.0])
+@pytest.mark.parametrize('angle', [-90.0, -30.0, -18.0, 18.0, 30.0, 90.0])
+def test_real_turn_gain_unchanged(mph, angle):
+  assert get_honda_crv_5g_pid_kp_scale(angle, mph * CV.MPH_TO_MS) == 1.0
+
+
+def test_gain_is_bounded_symmetric_and_monotonic():
+  for mph in [0, 8, 11, 12, 14, 16, 18, 45]:
+    scales = [get_honda_crv_5g_pid_kp_scale(angle, mph * CV.MPH_TO_MS) for angle in [0, 6, 9, 12, 15, 18, 30]]
+    assert scales == sorted(scales)
+    assert all(0.5 <= value <= 1.0 for value in scales)
+    for angle in [0, 6, 9, 12, 15, 18, 30]:
+      assert get_honda_crv_5g_pid_kp_scale(angle, mph * CV.MPH_TO_MS) == get_honda_crv_5g_pid_kp_scale(-angle, mph * CV.MPH_TO_MS)
+  scales = [get_honda_crv_5g_pid_kp_scale(0.0, mph * CV.MPH_TO_MS) for mph in [0, 8, 11, 12, 14, 16, 18, 45]]
+  assert scales == sorted(scales)
+
+
+@pytest.mark.parametrize('mph', [11.0, 18.0])
+def test_speed_boundary_continuity(mph):
+  left = get_honda_crv_5g_pid_kp_scale(0.0, (mph - 1e-7) * CV.MPH_TO_MS)
+  right = get_honda_crv_5g_pid_kp_scale(0.0, (mph + 1e-7) * CV.MPH_TO_MS)
+  assert abs(left - right) < 1e-7
+
+
+@pytest.mark.parametrize('angle', [-18.0, -6.0, 6.0, 18.0])
+def test_angle_boundary_continuity(angle):
+  left = get_honda_crv_5g_pid_kp_scale(angle - 1e-7, 8.0 * CV.MPH_TO_MS)
+  right = get_honda_crv_5g_pid_kp_scale(angle + 1e-7, 8.0 * CV.MPH_TO_MS)
+  assert abs(left - right) < 1e-7
+
+
+def test_gain_reduces_feedback_before_saturation_without_changing_feedforward_or_integral():
+  base = PIDController(0.64, 0.192, pos_limit=1, neg_limit=-1)
+  tuned = PIDController(0.64 * get_honda_crv_5g_pid_kp_scale(3.0, 8.0 * CV.MPH_TO_MS), 0.192, pos_limit=1, neg_limit=-1)
+  base.i = tuned.i = -0.019
+  base_output = base.update(2.0, feedforward=0.004, freeze_integrator=True)
+  tuned_output = tuned.update(2.0, feedforward=0.004, freeze_integrator=True)
+  assert base_output == 1.0
+  assert tuned_output == pytest.approx(0.625)
+  assert tuned.p == pytest.approx(base.p * 0.5)
+  assert tuned.i == base.i
+  assert tuned.f == base.f

selfdrive/controls/tests/test_latcontrol.py

@@ -2092,6 +2092,41 @@ class TestLatControl:
     assert lac_log.active
     assert abs(tuned_output) < abs(base_output)
 
+  def test_honda_crv_5g_pid_center_gain_update_path(self):
+    controller, VM, CS, params, toggles = self._build_pid_controller(HONDA.HONDA_CRV_5G)
+    CS.vEgo = 8.0 * 0.44704
+    CS.steeringAngleDeg = -2.0
+    controller.pid.i = 0.03
+    toggles.honda_lateral_pid_kp_scale = 1.2
+    toggles.honda_lateral_pid_ki_scale = 0.8
+    for _ in range(20):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
+      assert lac_log.p == pytest.approx(controller.base_kp_v[0] * 1.2 * 0.5 * lac_log.angleError)
+      assert lac_log.i == pytest.approx(0.03)
+      assert controller.pid._k_i[1] == pytest.approx([value * 0.8 for value in controller.base_ki_v])
+
+    CS.vEgo = 25.0 * 0.44704
+    _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
+    assert lac_log.p == pytest.approx(controller.base_kp_v[0] * 1.2 * lac_log.angleError)
+
+  @pytest.mark.parametrize('car_name,eps_modified', [(HONDA.HONDA_CRV_5G, True), (HONDA.HONDA_CIVIC_BOSCH, False),
+                                                  (TOYOTA.TOYOTA_RAV4_TSS2, False)])
+  def test_honda_crv_5g_pid_center_gain_does_not_change_other_paths(self, monkeypatch, car_name, eps_modified):
+    controller, VM, CS, params, toggles = self._build_pid_controller(car_name)
+    if eps_modified:
+      CP = interfaces[car_name].get_non_essential_params(car_name)
+      CP.flags = int(CP.flags | HondaFlags.EPS_MODIFIED)
+      controller = LatControlPID(CP.as_reader(), interfaces[car_name](CP, custom.StarPilotCarParams.new_message()), DT_CTRL)
+    CS.vEgo = 8.0 * 0.44704
+    CS.steeringAngleDeg = -2.0
+
+    def unexpected_gain(*_args):
+      raise AssertionError('CR-V center gain must not run for this controller')
+
+    monkeypatch.setattr(latcontrol_pid, 'get_honda_crv_5g_pid_kp_scale', unexpected_gain)
+    _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
+    assert lac_log.p == pytest.approx(controller.base_kp_v[0] * lac_log.angleError)
+
   def test_rav4_tss2_torque_center_tune_fades_before_real_turns(self):
     low_speed_center = get_rav4_tss2_center_output_scale(0.05, 8.0)
     low_speed_turn = get_rav4_tss2_center_output_scale(1.0, 8.0)

starpilot/car/ford/lateral.py

@@ -245,7 +245,7 @@ class FordLateralController:
     if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or
         steering_pressed or lane_change):
       return base
-    speed_weight = float(np.interp(v_ego, [9.0, 10.0, 14.0, 16.0], [0.0, 1.0, 1.0, 0.0]))
+    speed_weight = float(np.interp(v_ego, [8.0, 9.0, 14.0, 16.0], [0.0, 1.0, 1.0, 0.0]))
     deficit_weight = 0.0
     planned_curve = abs(desired) >= 0.003 or (abs(requested) >= 0.003 and requested * predicted > 0.0)
     if requested * desired > 0.0 and planned_curve:

starpilot/car/ford/tests/test_lateral.py

@@ -97,7 +97,7 @@ def test_mach_e_unwind_lag_ramps_continuously(controller, monkeypatch):
   (12.0, -0.012, -0.012, 0.004, False, False, 0.006),
   (12.0, 0.012, 0.012, 0.010, False, False, 0.002),
   (12.0, 0.012, 0.012, 0.014, False, False, 0.002),
-  (9.5, 0.012, 0.012, 0.004, False, False, 0.004),
+  (9.5, 0.012, 0.012, 0.004, False, False, 0.006),
   (15.0, 0.012, 0.012, 0.004, False, False, 0.004),
   (16.0, 0.012, 0.012, 0.004, False, False, 0.002),
   (12.0, 0.012, 0.012, 0.004, True, False, 0.002),
@@ -118,7 +118,7 @@ def test_understeer_error_preserves_other_fords(controller):
 
 
 @pytest.mark.parametrize("sign", (-1, 1))
-@pytest.mark.parametrize("speed,expected", ((9.0, 0.002), (9.5, 0.004), (10.0, 0.006),
+@pytest.mark.parametrize("speed,expected", ((8.0, 0.002), (8.5, 0.004), (9.0, 0.006), (9.5, 0.006), (10.0, 0.006),
                                           (12.0, 0.006), (15.0, 0.004), (16.0, 0.002)))
 def test_mach_e_planned_curve_error_uses_preview_request_before_action_builds(controller, sign, speed, expected):
   controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
@@ -167,10 +167,58 @@ def test_mach_e_medium_speed_turn_in_lead_is_not_clipped_by_small_action_curvatu
   assert result.path_angle == 0.0
 
 
+@pytest.mark.parametrize("sign", (-1, 1))
+def test_mach_e_curve_request_does_not_collapse_when_speed_crosses_curvature_clamp(controller, monkeypatch, sign):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.006)
+  controller.curvature_last = sign * 0.006
+  controller.desired_curvature_last = sign * 0.006
+  commands = []
+  for speed in (8.9, 9.0, 9.01, 9.2, 9.5, 10.0):
+    result = controller.update(SimpleNamespace(latActive=True), car_state(speed=speed),
+                               SimpleNamespace(curvature=sign * 0.006))
+    assert result.active
+    assert result.path_angle == 0.0
+    commands.append(result.curvature)
+  assert commands == pytest.approx([sign * 0.006] * 6)
+
+
+@pytest.mark.parametrize("sign", (-1, 1))
+def test_mach_e_reversal_request_does_not_collapse_at_curvature_clamp(controller, monkeypatch, sign):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: -sign * 0.004)
+  monkeypatch.setattr(controller, "_blend_and_scale", lambda *_: (-sign * 0.005, 1))
+  controller.curvature_last = -sign * 0.005
+  for speed in (8.9, 9.01, 9.5, 10.0):
+    result = controller.update(SimpleNamespace(latActive=True), car_state(speed=speed, curvature=sign * 0.001),
+                               SimpleNamespace(curvature=sign * 0.002))
+    assert result.curvature == pytest.approx(-sign * 0.005)
+    assert result.path_angle == 0.0
+
+
+@pytest.mark.parametrize("fingerprint,flags,driver,lane_change", (
+  (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, True, False),
+  (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, False, True),
+  (CAR.FORD_MUSTANG_MACH_E_MK1, 0, False, False),
+  (CAR.FORD_EDGE_MK2, FordFlags.CANFD, False, False),
+  (CAR.FORD_EXPLORER_MK6, FordFlags.CANFD, False, False),
+  (CAR.FORD_F_150_MK14, FordFlags.CANFD, False, False),
+))
+def test_curve_clamp_transition_preserves_driver_lane_change_and_other_fords(controller, fingerprint, flags, driver, lane_change):
+  controller.CP.carFingerprint = fingerprint
+  controller.CP.flags = flags
+  for speed in (9.01, 9.5):
+    assert controller._curvature_error_limit(0.006, 0.006, 0.0, speed, driver, lane_change, 0.006) == 0.002
+
+
 @pytest.mark.parametrize("sign", (-1, 1))
 @pytest.mark.parametrize("speed,preview,expected", (
-  (9.0, -0.004, 0.002),
-  (9.5, -0.004, 0.004),
+  (8.0, -0.004, 0.002),
+  (8.5, -0.004, 0.004),
+  (9.0, -0.004, 0.006),
+  (9.5, -0.004, 0.006),
   (10.0, -0.004, 0.006),
   (12.0, -0.004, 0.006),
   (14.0, -0.004, 0.006),