Tunes

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -276,6 +276,8 @@ GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.55
 GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC = 0.065
 
 GENESIS_G70_FRICTION_THRESHOLD_GAIN = 0.10
+GENESIS_G70_FRICTION_THRESHOLD_SPEED_BP = [10.0, 20.0]
+GENESIS_G70_FRICTION_THRESHOLD_SPEED_V = [1.0, 2.0]
 GENESIS_G70_FRICTION_SPEED_ONSET = 10.0
 GENESIS_G70_FRICTION_SPEED_ONSET_WIDTH = 3.0
 GENESIS_G70_FRICTION_SPEED_CUTOFF = 35.0
@@ -3289,6 +3291,7 @@ def get_genesis_gv70_stabilized_output(output_torque: float, prev_output_torque:
 def get_genesis_g70_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
                                        desired_lateral_jerk: float = 0.0) -> float:
   base_threshold = get_standard_friction_threshold(v_ego)
+  base_threshold *= np.interp(v_ego, GENESIS_G70_FRICTION_THRESHOLD_SPEED_BP, GENESIS_G70_FRICTION_THRESHOLD_SPEED_V)
   speed_onset = _sigmoid((v_ego - GENESIS_G70_FRICTION_SPEED_ONSET) / GENESIS_G70_FRICTION_SPEED_ONSET_WIDTH)
   speed_cutoff = _sigmoid((GENESIS_G70_FRICTION_SPEED_CUTOFF - v_ego) / GENESIS_G70_FRICTION_SPEED_CUTOFF_WIDTH)
   center_weight = _sigmoid((GENESIS_G70_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) /

selfdrive/controls/tests/test_g70_friction_threshold.py

@@ -0,0 +1,42 @@
+import math
+
+import numpy as np
+import pytest
+
+from opendbc.car import structs
+from opendbc.car.lateral import get_friction
+from openpilot.selfdrive.controls.lib import latcontrol_vehicle_tunes as tunes
+
+
+def legacy_threshold(speed, accel, jerk):
+  def sigmoid(x):
+    return 1.0 / (1.0 + math.exp(-x))
+  gain = (0.10 * sigmoid((speed - 10.0) / 3.0) * sigmoid((35.0 - speed) / 6.0) *
+          sigmoid((0.28 - abs(accel)) / 0.10) * sigmoid((0.35 - abs(jerk)) / 0.10))
+  return tunes.get_standard_friction_threshold(speed) * (1.0 + gain)
+
+
+@pytest.mark.parametrize('speed,scale', [(0, 1), (5, 1), (10, 1), (15, 1.5), (20, 2), (35, 2), (45, 2)])
+@pytest.mark.parametrize('accel,jerk', [(0, 0), (0.1, 0.2), (0.8, 0.3), (1.5, -0.5), (-0.8, -0.3)])
+def test_speed_blend(speed, scale, accel, jerk):
+  assert tunes.get_genesis_g70_friction_threshold(speed, accel, jerk) == pytest.approx(
+    legacy_threshold(speed, accel, jerk) * scale)
+
+
+def test_small_error_gain_and_full_compensation():
+  params = structs.CarParams.LateralTorqueTuning.new_message(friction=0.08, latAccelFactor=2.96)
+  old = legacy_threshold(30, 0.8, 0.2)
+  new = tunes.get_genesis_g70_friction_threshold(30, 0.8, 0.2)
+  for error in (-0.1, 0.1):
+    assert get_friction(error, 0, new, params) == pytest.approx(get_friction(error, 0, old, params) / 2)
+  for error in (-2, 2):
+    assert get_friction(error, 0, new, params) == pytest.approx(get_friction(error, 0, old, params))
+
+
+def test_symmetric_and_continuous():
+  for speed in np.linspace(0, 45, 100):
+    assert tunes.get_genesis_g70_friction_threshold(speed, 0.8, 0.2) == pytest.approx(
+      tunes.get_genesis_g70_friction_threshold(speed, -0.8, -0.2))
+  for speed in (10, 20):
+    assert abs(tunes.get_genesis_g70_friction_threshold(speed + 1e-6) -
+               tunes.get_genesis_g70_friction_threshold(speed - 1e-6)) < 1e-6

starpilot/car/ford/lateral.py

@@ -43,6 +43,8 @@ MACH_E_LOW_SPEED_TURN_IN_FADE_SPEED = 12.0
 MACH_E_TURN_IN_MIN_CURVATURE = 0.002
 MACH_E_TURN_IN_FULL_CURVATURE = 0.008
 MACH_E_TURN_IN_LAG_CURVATURE = 0.006
+MACH_E_UNWIND_LOOKAHEAD_EXTRA = 0.80
+MACH_E_UNWIND_FULL_LAG_CURVATURE = 0.0005
 MACH_E_DIRECTION_CHANGE_MIN_SPEED = 9.0
 MACH_E_DIRECTION_CHANGE_LOOKAHEAD_RAMP_SPEED = 10.0
 MACH_E_DIRECTION_CHANGE_LOOKAHEAD_FULL_SPEED = 12.0
@@ -225,6 +227,21 @@ class FordLateralController:
         precision = 0
     return requested, precision
 
+  def _unwind_preview(self, desired: float, predicted: float, current: float, v_ego: float) -> float:
+    if self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1:
+      return predicted
+    if desired * current <= 0.0 or desired * predicted <= 0.0 or desired * self.desired_curvature_last <= 0.0:
+      return predicted
+    if abs(desired) >= abs(self.desired_curvature_last) or abs(current) <= abs(desired):
+      return predicted
+    preview = self._predicted_curvature(v_ego, self._curvature_lookahead() + MACH_E_UNWIND_LOOKAHEAD_EXTRA)
+    if desired * preview <= 0.0 or abs(preview) >= min(abs(desired), abs(predicted)):
+      return predicted
+    speed_weight = float(np.interp(v_ego, [5.0, 7.0, 12.0, 15.0], [0.0, 1.0, 1.0, 0.0]))
+    curvature_weight = float(np.interp(abs(desired), [0.002, 0.008], [0.0, 1.0]))
+    lag_weight = float(np.interp(abs(current) - abs(desired), [0.0, MACH_E_UNWIND_FULL_LAG_CURVATURE], [0.0, 1.0]))
+    return predicted + speed_weight * curvature_weight * lag_weight * (preview - predicted)
+
   def _turn_in_preview_weight(self, desired: float, preview: float, current: float) -> float:
     if self.CP.carFingerprint not in FORD_CONSERVATIVE_PREVIEW_CARS:
       return 0.0
@@ -463,7 +480,10 @@ class FordLateralController:
         if turn_in_weight > 0.0:
           turn_in_target = float(np.copysign(max(abs(desired), abs(turn_in_predicted)), desired))
           predicted = float(np.interp(turn_in_weight, [0.0, 1.0], [predicted, turn_in_target]))
-    requested, precision = self._blend_and_scale(desired, predicted, v_ego, current, allow_opposite_preview)
+    command_predicted = predicted
+    if not allow_opposite_preview and not CS.out.steeringPressed and not self._lane_change()[0]:
+      command_predicted = self._unwind_preview(desired, predicted, current, v_ego)
+    requested, precision = self._blend_and_scale(desired, command_predicted, v_ego, current, allow_opposite_preview)
     self.desired_curvature_last = desired
 
     if v_ego > 9.0:

starpilot/car/ford/tests/test_lateral.py

@@ -46,6 +46,72 @@ def car_state(speed=15.0, accel=0.0, curvature=0.0, steering_pressed=False, stee
   ))
 
 
+@pytest.mark.parametrize("sign", (-1, 1))
+@pytest.mark.parametrize("speed,weight", ((4.0, 0.0), (5.0, 0.0), (6.0, 0.5), (7.0, 1.0),
+                                         (12.0, 1.0), (13.5, 0.5), (15.0, 0.0), (20.0, 0.0)))
+def test_mach_e_unwind_preview_speed_and_direction(controller, monkeypatch, sign, speed, weight):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.desired_curvature_last = sign * 0.011
+  monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.004)
+  result = controller._unwind_preview(sign * 0.010, sign * 0.009, sign * 0.011, speed)
+  assert result == pytest.approx(sign * (0.009 - 0.005 * weight))
+
+
+@pytest.mark.parametrize("desired,last,current,preview", (
+  (0.010, 0.009, 0.011, 0.004),
+  (0.010, 0.010, 0.011, 0.004),
+  (0.010, 0.011, 0.009, 0.004),
+  (0.010, 0.011, 0.010, 0.004),
+  (0.010, -0.011, 0.011, 0.004),
+  (0.010, 0.011, -0.011, 0.004),
+  (0.010, 0.011, 0.011, -0.004),
+  (0.010, 0.011, 0.011, 0.009),
+  (0.010, 0.011, 0.011, 0.012),
+  (0.001, 0.002, 0.003, 0.0005),
+))
+def test_mach_e_unwind_preserves_other_phases(controller, monkeypatch, desired, last, current, preview):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.desired_curvature_last = last
+  monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: preview)
+  assert controller._unwind_preview(desired, 0.009, current, 10.0) == 0.009
+
+
+@pytest.mark.parametrize("fingerprint", (CAR.FORD_EDGE_MK2, CAR.FORD_EXPLORER_MK6, CAR.FORD_F_150_MK14))
+def test_unwind_preview_does_not_change_other_fords(controller, fingerprint):
+  controller.CP.carFingerprint = fingerprint
+  controller.desired_curvature_last = 0.011
+  assert controller._unwind_preview(0.010, 0.009, 0.011, 10.0) == 0.009
+
+
+def test_mach_e_unwind_lag_ramps_continuously(controller, monkeypatch):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.desired_curvature_last = 0.011
+  monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: 0.004)
+  assert controller._unwind_preview(0.010, 0.009, 0.01025, 10.0) == pytest.approx(0.0065)
+  assert controller._unwind_preview(0.010, -0.009, 0.011, 10.0) == -0.009
+
+
+@pytest.mark.parametrize("driver,lane_change,active", ((False, False, True), (True, False, True),
+                                                    (False, True, True), (False, False, False)))
+def test_mach_e_unwind_update_scope_and_rate(controller, monkeypatch, driver, lane_change, active):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  controller.desired_curvature_last = 0.011
+  controller.curvature_last = 0.010
+  controller.curvature_samples.append(0.010)
+  monkeypatch.setattr(controller, "_lane_change", lambda: (lane_change, 0))
+  monkeypatch.setattr(controller, "_predicted_curvature", lambda v, t: 0.010 if t < 0.5 else 0.004)
+  result = controller.update(SimpleNamespace(latActive=active),
+                             car_state(speed=10.0, curvature=0.011, steering_pressed=driver),
+                             SimpleNamespace(curvature=0.010))
+  assert result.curvature_rate == 0.0
+  if not active:
+    assert not result.active
+    assert controller.desired_curvature_last == 0.0
+  else:
+    assert result.curvature == pytest.approx(0.010 if driver or lane_change else 0.009)
+
+
 def test_human_turn_requires_sustained_input():
   detector = HumanTurnDetector()
   assert not detector.update(True, True, 0.0)