diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index f884c3a2e6..3dfbb33bf8 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -11,7 +11,7 @@ from opendbc.car.interfaces import CarControllerBase from opendbc.car.toyota import toyotacan from opendbc.car.toyota.values import CAR, MIN_ACC_SPEED, NO_STOP_TIMER_CAR, PEDAL_TRANSITION, TSS2_CAR, \ CarControllerParams, ToyotaFlags, ToyotaSafetyFlags, \ - UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, TOYOTA_AUTO_HOLD_AEB_CARS + UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, uses_toyota_auto_hold_aeb from opendbc.can import CANPacker Ecu = structs.CarParams.Ecu @@ -373,7 +373,7 @@ class CarController(CarControllerBase): return [] def reset_auto_hold_state(self): - if self.brake_hold_active and self.CP.carFingerprint not in TOYOTA_AUTO_HOLD_AEB_CARS: + if self.brake_hold_active and not uses_toyota_auto_hold_aeb(self.CP): self.standstill_req = False self._brake_hold_counter = 0 self.brake_hold_active = False @@ -475,7 +475,7 @@ class CarController(CarControllerBase): self._update_standstill_request(CC, CS, actuators, starpilot_toggles) if supports_toyota_auto_hold(self.CP, getattr(starpilot_toggles, "toyota_auto_hold", False)): - if self.CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS: + if uses_toyota_auto_hold_aeb(self.CP): can_sends.extend(self.create_auto_brake_hold_messages(CS)) else: self.update_auto_hold_state(CS, pcm_cancel_cmd, long_active=CC.longActive, stopping=stopping) @@ -590,7 +590,7 @@ class CarController(CarControllerBase): pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)) - if self.brake_hold_active and self.CP.carFingerprint not in TOYOTA_AUTO_HOLD_AEB_CARS: + if self.brake_hold_active and not uses_toyota_auto_hold_aeb(self.CP): pcm_accel_cmd = TOYOTA_AUTO_HOLD_ACCEL self.permit_braking = True self.standstill_req = True diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 18134b7fa9..f04d14e3bb 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -5,7 +5,7 @@ from opendbc.car.toyota.radar_interface import RadarInterface from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \ ToyotaSafetyFlags, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, \ - TOYOTA_AUTO_HOLD_AEB_CARS + uses_toyota_auto_hold_aeb from opendbc.car.disable_ecu import disable_ecu from opendbc.car.interfaces import CarInterfaceBase from opendbc.safety import ALTERNATIVE_EXPERIENCE @@ -167,7 +167,7 @@ class CarInterface(CarInterfaceBase): toyota_auto_hold = Params(return_defaults=True).get_bool("ToyotaAutoHold") if toyota_auto_hold and ret.openpilotLongitudinalControl and candidate in TOYOTA_AUTO_HOLD_CARS: ret.alternativeExperience |= (ALTERNATIVE_EXPERIENCE.ALLOW_AEB - if candidate in TOYOTA_AUTO_HOLD_AEB_CARS + if uses_toyota_auto_hold_aeb(ret) else ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD) ret.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index c48332a339..5189772b39 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -25,7 +25,7 @@ from opendbc.car.toyota.radar_interface import RadarInterface, TSSP_RADAR_EGO_SP from opendbc.car.toyota.values import CAR, DBC, MIN_ACC_SPEED, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \ FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \ ToyotaFlags, ToyotaSafetyFlags, ToyotaStarPilotFlags, TOYOTA_AUTO_HOLD_CARS, \ - TOYOTA_AUTO_HOLD_AEB_CARS, \ + uses_toyota_auto_hold_aeb, \ get_platform_codes from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.common.params import Params @@ -193,8 +193,15 @@ class TestToyotaInterfaces: if car_model in TSS2_CAR and car_model not in SECOC_CAR: assert dbc[Bus.pt] == "toyota_nodsu_pt_generated" - @pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H]) - def test_auto_hold_sets_flag_on_supported_toyota(self, candidate): + @pytest.mark.parametrize("candidate,hybrid", [ + (CAR.TOYOTA_CAMRY_TSS2, False), + (CAR.TOYOTA_RAV4, False), + (CAR.TOYOTA_RAV4H, True), + (CAR.TOYOTA_RAV4_TSS2, False), + (CAR.TOYOTA_RAV4_TSS2, True), + (CAR.TOYOTA_COROLLA_TSS2, True), + ]) + def test_auto_hold_sets_flag_on_supported_toyota(self, candidate, hybrid): params = Params() try: params.put_bool("ToyotaAutoHold", True) @@ -202,7 +209,7 @@ class TestToyotaInterfaces: candidate, {bus: ({0x2FF: 8} if candidate in (CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H) and bus == 0 else {}) for bus in range(8)}, - [], + [CarParams.CarFw(ecu=Ecu.hybrid, address=0x7D2, fwVersion=b"test")] if hybrid else [], alpha_long=False, is_release=False, docs=False, @@ -212,7 +219,9 @@ class TestToyotaInterfaces: params.remove("ToyotaAutoHold") assert car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value - if candidate in TOYOTA_AUTO_HOLD_AEB_CARS: + legacy_hold = candidate == CAR.TOYOTA_CAMRY_TSS2 or (candidate == CAR.TOYOTA_RAV4_TSS2 and hybrid) + assert uses_toyota_auto_hold_aeb(car_params) == legacy_hold + if legacy_hold: assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD else: @@ -222,16 +231,24 @@ class TestToyotaInterfaces: can_parsers = CarState.get_can_parsers(car_params) car_state = CarState(car_params, SimpleNamespace(flags=0)) car_state.update(can_parsers, SimpleNamespace(cluster_offset=1.0)) - assert (0x344 in can_parsers[Bus.cam].vl) == (candidate in TOYOTA_AUTO_HOLD_AEB_CARS) + assert (0x344 in can_parsers[Bus.cam].vl) == legacy_hold + assert car_state.auto_brake_hold == legacy_hold - @pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H]) - def test_auto_hold_is_disabled_by_default(self, candidate): + @pytest.mark.parametrize("candidate,hybrid", [ + (CAR.TOYOTA_CAMRY_TSS2, False), + (CAR.TOYOTA_RAV4, False), + (CAR.TOYOTA_RAV4H, True), + (CAR.TOYOTA_RAV4_TSS2, False), + (CAR.TOYOTA_RAV4_TSS2, True), + (CAR.TOYOTA_RAV4_TSS2_2022, True), + ]) + def test_auto_hold_is_disabled_by_default(self, candidate, hybrid): params = Params() params.remove("ToyotaAutoHold") car_params = CarInterface.get_params( candidate, {bus: {} for bus in range(8)}, - [], + [CarParams.CarFw(ecu=Ecu.hybrid, address=0x7D2, fwVersion=b"test")] if hybrid else [], alpha_long=False, is_release=False, docs=False, @@ -240,6 +257,8 @@ class TestToyotaInterfaces: assert not car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD + assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB + assert 0x344 not in CarState.get_can_parsers(car_params)[Bus.cam].vl def test_prius_openpilot_long_uses_hybrid_long_defaults(self): car_params = CarInterface.get_params( @@ -1239,11 +1258,13 @@ class TestToyotaCarController: class TestToyotaAutoHoldCruise: @staticmethod - def _make_car(*, candidate=CAR.TOYOTA_RAV4_TSS2, enabled=True, capability=True): + def _make_car(*, candidate=CAR.TOYOTA_RAV4_TSS2, hybrid=False, enabled=True, capability=True): cp = CarInterface.get_non_essential_params(candidate) - cp.flags &= ~ToyotaFlags.AUTO_BRAKE_HOLD.value + cp.flags &= ~(ToyotaFlags.AUTO_BRAKE_HOLD.value | ToyotaFlags.HYBRID.value) if capability: cp.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value + if hybrid: + cp.flags |= ToyotaFlags.HYBRID.value controller = CarController(DBC[candidate], cp) cc = structs.CarControl(enabled=True, longActive=True) cc.actuators.accel = -0.7 @@ -1263,16 +1284,18 @@ class TestToyotaAutoHoldCruise: pre_collision_2={}, ) toggles = SimpleNamespace(toyota_auto_hold=enabled, sng_hack=False, lock_doors=False, unlock_doors=False) - parser = CANParser(DBC[candidate][Bus.pt], [("ACC_CONTROL", 0)], 0) + parser = CANParser(DBC[candidate][Bus.pt], [("ACC_CONTROL", 0), ("PRE_COLLISION_2", 0)], 0) return SimpleNamespace(controller=controller, cc=cc, cs=cs, toggles=toggles, parser=parser) @staticmethod def _tick(car): # Run through a complete ACC_CONTROL send interval and decode the actual # controller output, including the ordinary standstill and PID paths. + car.can_sends = [] for _ in range(3): now_nanos = car.controller.frame * 10_000_000 _, messages = car.controller.update(car.cc.as_reader(), car.cs, now_nanos, car.toggles) + car.can_sends.extend(messages) car.parser.update([(now_nanos, messages)]) return car.parser.vl["ACC_CONTROL"] @@ -1412,6 +1435,92 @@ class TestToyotaAutoHoldCruise: assert not car.controller.brake_hold_active +class TestToyotaAutoHoldAeb: + _make_car = staticmethod(TestToyotaAutoHoldCruise._make_car) + _tick = staticmethod(TestToyotaAutoHoldCruise._tick) + + @pytest.mark.parametrize("candidate", list(CAR)) + @pytest.mark.parametrize("hybrid", [False, True]) + def test_legacy_path_is_scoped_to_camry_and_early_rav4_hybrid(self, candidate, hybrid): + cp = SimpleNamespace(carFingerprint=candidate, flags=ToyotaFlags.HYBRID.value if hybrid else 0) + expected = candidate == CAR.TOYOTA_CAMRY_TSS2 or (candidate == CAR.TOYOTA_RAV4_TSS2 and hybrid) + assert uses_toyota_auto_hold_aeb(cp) == expected + + @pytest.mark.parametrize("candidate,hybrid", [(CAR.TOYOTA_CAMRY_TSS2, False), (CAR.TOYOTA_RAV4_TSS2, True)]) + def test_manual_stop_uses_legacy_hold_and_releases_on_gas(self, candidate, hybrid): + car = self._make_car(candidate=candidate, hybrid=hybrid) + car.cc.enabled = False + car.cc.longActive = False + car.cc.latActive = True + car.cc.actuators.accel = 0.0 + car.cs.out.cruiseState.enabled = False + car.cs.out.cruiseState.standstill = False + car.cs.out.brakePressed = True + car.cs.pre_collision_2 = {"DSS1GDRV": -0.1, "PBRTRGR": 0} + + # Pass through the camera message until the pedal has been held at a stop. + command = self._tick(car) + assert not car.controller.brake_hold_active + assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -0.1 + for _ in range(34): + command = self._tick(car) + assert car.controller.brake_hold_active + assert any(msg[0] == 0x344 for msg in car.can_sends) + assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0 + assert car.parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 1 + # Do not simultaneously inject an ACC_CONTROL hold on legacy-path cars. + assert command["ACCEL_CMD"] == 0.0 + assert command["RELEASE_STANDSTILL"] == 1 + + car.cs.out.brakePressed = False + for _ in range(50): + self._tick(car) + assert car.controller.brake_hold_active + assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0 + + car.cs.out.gasPressed = True + self._tick(car) + assert not car.controller.brake_hold_active + assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -0.1 + assert car.parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 0 + + @pytest.mark.parametrize("enabled,capability", [(False, True), (True, False)]) + def test_rav4_hybrid_does_not_inject_legacy_messages_without_opt_in(self, enabled, capability): + car = self._make_car(hybrid=True, enabled=enabled, capability=capability) + car.cc.longActive = False + car.cc.actuators.accel = 0.0 + car.cs.out.cruiseState.enabled = False + car.cs.out.brakePressed = True + for _ in range(50): + self._tick(car) + assert not car.controller.brake_hold_active + assert all(msg[0] != 0x344 for msg in car.can_sends) + + @pytest.mark.parametrize("release", ["toggle", "main", "park", "reverse", "moving", "cruise"]) + def test_rav4_hybrid_legacy_hold_releases_when_conditions_change(self, release): + car = self._make_car(hybrid=True) + car.cc.longActive = False + car.cc.actuators.accel = 0.0 + car.cs.out.cruiseState.enabled = False + car.cs.out.brakePressed = True + for _ in range(35): + self._tick(car) + assert car.controller.brake_hold_active + + if release == "toggle": + car.toggles.toyota_auto_hold = False + elif release == "main": + car.cs.out.cruiseState.available = False + elif release in ("park", "reverse"): + car.cs.out.gearShifter = getattr(structs.CarState.GearShifter, release) + elif release == "moving": + car.cs.out.standstill = False + else: + car.cs.out.cruiseState.enabled = True + self._tick(car) + assert not car.controller.brake_hold_active + + class TestToyotaCarState: @pytest.mark.parametrize("candidate", [CAR.TOYOTA_PRIUS, CAR.TOYOTA_PRIUS_RETROFIT]) def test_legacy_prius_distance_button_generates_events(self, candidate): diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index 9f9b4e54c2..db8b41ad6e 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -629,10 +629,16 @@ TOYOTA_AUTO_HOLD_CARS = (TSS2_CAR - RADAR_ACC_CAR - SECOC_CAR) | { CAR.TOYOTA_RAV4H, } -# The Camry uses the legacy camera AEB replacement for Auto Hold. Other -# supported Toyota models use the ACC_CONTROL hold request. +# The Camry uses the legacy camera AEB replacement for Auto Hold. The +# 2019-2021 RAV4 uses it only with the detected hybrid powertrain. TOYOTA_AUTO_HOLD_AEB_CARS = {CAR.TOYOTA_CAMRY_TSS2} + +def uses_toyota_auto_hold_aeb(CP: CarParams) -> bool: + return (CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS or + (CP.carFingerprint == CAR.TOYOTA_RAV4_TSS2 and bool(CP.flags & ToyotaFlags.HYBRID.value))) + + # no resume button press required NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index e488df0700..d86e9bc242 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -13,6 +13,7 @@ from openpilot.common.pid import PIDController from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import * # noqa: F403 +from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import get_genesis_g70_center_measurement_damping_gain # At higher speeds (25+mph) we can assume: # Lateral acceleration achieved by a specific car correlates to @@ -561,6 +562,11 @@ class LatControlTorque(LatControl): ff, self.gv70_previous_feedforward, setpoint, desired_lateral_jerk, CS.vEgo, self.dt, ) self.gv70_previous_feedforward = ff + if self.is_genesis_g70: + damping_gain = 0.0 if CS.steeringPressed else get_genesis_g70_center_measurement_damping_gain( + CS.vEgo, setpoint, measurement, desired_lateral_jerk, + ) + self.pid._k_d = [[0.0], [damping_gain]] freeze_integrator = (steer_limited_by_safety or CS.steeringPressed or CS.vEgo < self.low_speed_reset_threshold or unwind_detected) error_rate = 0.0 if self.is_genesis_gv70 and CS.steeringPressed else -measurement_rate diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index b6f1c98e77..ac3711fc5b 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -366,6 +366,10 @@ GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC = 0.18 GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC = 0.10 GENESIS_G70_OUTPUT_SMOOTHING_RELEASE_RC = 0.03 GENESIS_G70_OUTPUT_SMOOTHING_OVERSHOOT = 0.08 +GENESIS_G70_CENTER_MEASUREMENT_DAMPING_MAX = 0.06 +GENESIS_G70_CENTER_MEASUREMENT_DAMPING_SPEED_BP = [50.0 * CV.MPH_TO_MS, 60.0 * CV.MPH_TO_MS] +GENESIS_G70_CENTER_MEASUREMENT_DAMPING_LAT_BP = [0.15, 0.35] +GENESIS_G70_CENTER_MEASUREMENT_DAMPING_JERK_BP = [0.20, 0.50] GENESIS_G70_HIGHWAY_STABILIZER_CENTER_LAT_BP = [1.4, 1.8] GENESIS_G70_HIGHWAY_STABILIZER_CURVE_EXIT_LAT = 0.15 GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN = 0.45 @@ -3574,6 +3578,15 @@ def get_genesis_g70_highway_turn_in_output_scale(output_torque: float, setpoint: return 1.0 - GENESIS_G70_HIGHWAY_TURN_IN_OUTPUT_REDUCTION * speed_weight * curve_weight * jerk_weight * tracking_weight +def get_genesis_g70_center_measurement_damping_gain(v_ego: float, desired_lateral_accel: float, + measured_lateral_accel: float, desired_lateral_jerk: float) -> float: + speed_weight = np.interp(v_ego, GENESIS_G70_CENTER_MEASUREMENT_DAMPING_SPEED_BP, [0.0, 1.0]) + center_weight = np.interp(max(abs(desired_lateral_accel), abs(measured_lateral_accel)), + GENESIS_G70_CENTER_MEASUREMENT_DAMPING_LAT_BP, [1.0, 0.0]) + jerk_weight = np.interp(abs(desired_lateral_jerk), GENESIS_G70_CENTER_MEASUREMENT_DAMPING_JERK_BP, [1.0, 0.0]) + return float(GENESIS_G70_CENTER_MEASUREMENT_DAMPING_MAX * speed_weight * center_weight * jerk_weight) + + def get_genesis_g70_stabilized_output(output_torque: float, prev_output_torque: float, desired_lateral_accel: float, measured_lateral_accel: float, desired_lateral_jerk: float, v_ego: float, dt: float) -> float: diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 8464ab6305..e01ced1c47 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -61,6 +61,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( get_genesis_gv70_low_speed_center_overshoot_scale, get_genesis_gv70_stabilized_output, get_genesis_g70_stabilized_output, + get_genesis_g70_center_measurement_damping_gain, normalize_flm_overrides, set_flm_runtime_overrides, ) @@ -1041,6 +1042,73 @@ class TestLatControl: assert lac_log.active assert output == pytest.approx(-0.123) + @pytest.mark.parametrize("mph,desired,measured,jerk,expected", [ + (65.0, 0.0, 0.10, 0.0, 0.06), + (50.0, 0.0, 0.10, 0.0, 0.0), + (55.0, 0.0, 0.10, 0.0, 0.03), + (65.0, 0.25, 0.10, 0.0, 0.03), + (65.0, 0.0, 0.25, 0.0, 0.03), + (65.0, 0.35, 0.10, 0.0, 0.0), + (65.0, 0.0, 0.35, 0.0, 0.0), + (65.0, 0.0, 0.10, 0.35, 0.03), + (65.0, 0.0, 0.10, 0.50, 0.0), + ]) + def test_genesis_g70_center_measurement_damping_gates(self, mph, desired, measured, jerk, expected): + for direction in [-1.0, 1.0]: + gain = get_genesis_g70_center_measurement_damping_gain( + mph * 0.44704, desired * direction, measured * direction, jerk * direction, + ) + assert gain == pytest.approx(expected) + + @pytest.mark.parametrize("direction", [-1.0, 1.0]) + def test_genesis_g70_center_measurement_damping_update_path(self, direction): + controller, VM, CS, params, toggles = self._build_torque_controller(HYUNDAI.GENESIS_G70_2020) + CS.vEgo = 65.0 * 0.44704 + CS.steeringAngleDeg = 0.0 + controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles) + CS.steeringAngleDeg = -direction * 0.5 + output, _, moving_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles) + assert moving_log.d * direction < 0.0 + assert abs(moving_log.d) <= 0.15 + assert abs(output) <= controller.steer_max + + for _ in range(150): + _, _, steady_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles) + assert steady_log.d == pytest.approx(0.0, abs=1e-6) + + CS.steeringPressed = True + CS.steeringAngleDeg = 0.0 + _, _, driver_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles) + assert driver_log.d == 0.0 + + CS.steeringPressed = False + controller.update(False, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles) + _, _, resumed_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles) + assert resumed_log.d == 0.0 + + CS.vEgo = 40.0 * 0.44704 + CS.steeringAngleDeg = -direction * 0.5 + _, _, low_speed_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles) + assert low_speed_log.d == 0.0 + + CS.vEgo = 65.0 * 0.44704 + curvature = direction * 0.8 / CS.vEgo ** 2 + controller.curvature_request_buffer = deque([curvature] * controller.request_buffer_len, + maxlen=controller.request_buffer_len) + _, _, curve_log = controller.update(True, CS, VM, params, False, curvature, False, 0.2, None, None, toggles) + assert curve_log.d == 0.0 + + @pytest.mark.parametrize("car_name", [HYUNDAI.GENESIS_GV70_1ST_GEN, HYUNDAI.KIA_EV6, HYUNDAI.HYUNDAI_IONIQ_6]) + def test_genesis_g70_center_measurement_damping_does_not_change_other_cars(self, monkeypatch, car_name): + def unexpected_damping(*_args): + pytest.fail("G70 center damping reached another vehicle") + + monkeypatch.setattr(latcontrol_torque, "get_genesis_g70_center_measurement_damping_gain", unexpected_damping) + controller, VM, CS, params, toggles = self._build_torque_controller(car_name) + CS.vEgo = 65.0 * 0.44704 + controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles) + assert controller.pid.d == 0.0 + def test_sonata_hybrid_center_output_taper_is_mid_speed_and_center_gated(self): low_speed = get_sonata_hybrid_center_output_scale(0.0, 8.0) center = get_sonata_hybrid_center_output_scale(0.0, 13.4) diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index 1edad330c1..70611ed9b0 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -264,7 +264,17 @@ class FordLateralController: [MACH_E_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE, MACH_E_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE], [0.0, 1.0], )) - return base + (MACH_E_CURVATURE_ERROR_MAX - base) * speed_weight * max(deficit_weight, reversal_weight) + unwind_weight = 0.0 + if (self.model is not None and len(self.model.orientationRate.z) >= 17 and + requested * current >= 0.0 and desired * current >= 0.0 and abs(desired) < abs(current)): + unwind_deficit = np.sign(current) * (current - requested) + preview_deficit = np.sign(current) * (current - predicted) + unwind_weight = float(np.interp( + min(unwind_deficit, preview_deficit), + [MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT, MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT], + [0.0, 1.0], + )) + return base + (MACH_E_CURVATURE_ERROR_MAX - base) * speed_weight * max(deficit_weight, reversal_weight, unwind_weight) def _path_angle_assist(self, requested: float, desired: float, applied: float, current: float, v_ego: float, steering_pressed: bool, lane_change: bool) -> float: diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index 0ca07105fa..b70215195d 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -130,6 +130,75 @@ def test_understeer_error_preserves_other_fords(controller): assert controller._curvature_error_limit(0.012, 0.012, 0.004, 12.0, False, False) == 0.002 +@pytest.mark.parametrize("sign", (-1, 1)) +@pytest.mark.parametrize("speed,expected", ((8.0, 0.002), (8.5, 0.004), (9.0, 0.006), (12.0, 0.006), + (14.0, 0.006), (15.0, 0.004), (16.0, 0.002))) +def test_mach_e_unwind_error_tracks_opening_path_before_direction_changes(controller, sign, speed, expected): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33)) + assert controller._curvature_error_limit( + sign * 0.001, sign * 0.002, sign * 0.005, speed, False, False, sign * 0.001) == pytest.approx(expected) + + +@pytest.mark.parametrize("sign", (-1, 1)) +@pytest.mark.parametrize("requested,preview,expected", ((0.003, 0.001, 0.002), (0.002, 0.001, 0.004), + (0.001, 0.001, 0.006), (0.001, 0.004, 0.002), + (0.001, 0.003, 0.002), (0.001, 0.002, 0.004), + (0.001, -0.0002, 0.006), (0.001, 0.0, 0.006), + (0.0, 0.0, 0.006))) +def test_mach_e_unwind_error_requires_measured_lag_and_opening_preview(controller, sign, requested, preview, expected): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33)) + assert controller._curvature_error_limit( + sign * requested, sign * 0.002, sign * 0.005, 12.0, False, False, sign * preview) == pytest.approx(expected) + + +@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_unwind_error_preserves_takeover_lane_changes_and_other_fords(controller, fingerprint, flags, driver, lane_change): + controller.CP.carFingerprint = fingerprint + controller.CP.flags = flags + controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33)) + assert controller._curvature_error_limit(0.001, 0.002, 0.005, 12.0, driver, lane_change, 0.001) == 0.002 + + +@pytest.mark.parametrize("model", (None, SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 16)))) +def test_mach_e_unwind_error_requires_model_preview(controller, model): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + controller.model = model + assert controller._curvature_error_limit(0.001, 0.002, 0.005, 12.0, False, False, 0.001) == 0.002 + + +@pytest.mark.parametrize("sign", (-1, 1)) +def test_mach_e_curve_exit_releases_without_waiting_for_left_right_reversal(controller, monkeypatch, sign): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33), + meta=SimpleNamespace(laneChangeState=0, laneChangeDirection=0)) + controller.curvature_last = sign * 0.004 + controller.desired_curvature_last = sign * 0.003 + monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.001) + commands = [] + for _ in range(4): + result = controller.update(SimpleNamespace(latActive=True), car_state(speed=12.0, curvature=sign * 0.005), + SimpleNamespace(curvature=sign * 0.002)) + assert result.active + assert result.path_angle == 0.0 + commands.append(sign * result.curvature) + assert commands[0] == pytest.approx(0.004 - 0.0018) + assert commands[-1] == pytest.approx(0.0016) + assert commands[-1] < 0.005 - 0.002 + + @pytest.mark.parametrize("sign", (-1, 1)) @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)))