diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 9c3997f3c1..529c8d65a2 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/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)) / diff --git a/selfdrive/controls/tests/test_g70_friction_threshold.py b/selfdrive/controls/tests/test_g70_friction_threshold.py new file mode 100644 index 0000000000..75c70390bc --- /dev/null +++ b/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 diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index 56f6fc4580..88da3a7b5b 100644 --- a/starpilot/car/ford/lateral.py +++ b/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: diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index c3020ad9f5..1e5b411a74 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/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)