From 0c6ee693626be7d407768880c3e20f6739463e1a Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 24 Sep 2026 17:12:32 -0500 Subject: [PATCH] Long day --- .../opendbc/car/hyundai/carcontroller.py | 20 +++++----- opendbc_repo/opendbc/car/hyundai/interface.py | 4 +- .../opendbc/car/hyundai/tests/test_hyundai.py | 4 +- .../car/hyundai/tests/test_ray_pedal.py | 16 +++++--- opendbc_repo/opendbc/safety/modes/hyundai.h | 2 +- .../safety/tests/test_hyundai_ray_pedal.py | 4 +- .../controls/lib/latcontrol_vehicle_tunes.py | 11 ++++- .../lib/longitudinal_vehicle_tunes.py | 5 ++- .../tests/test_g70_friction_threshold.py | 26 ++++++++++++ .../controls/tests/test_starpilot_planner.py | 1 + starpilot/car/ford/lateral.py | 23 +++++++++-- starpilot/car/ford/tests/test_lateral.py | 40 +++++++++++++++++-- 12 files changed, 125 insertions(+), 31 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 0879b8b0a0..5aefb8606c 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -42,8 +42,8 @@ IONIQ_6_RESPONSE_MULTIPLIER = 1.2 IONIQ_6_CANFD_SCC_ACCEL_STEP = (6.0 / 50.0) * IONIQ_6_RESPONSE_MULTIPLIER IONIQ_6_CANFD_SCC_DECEL_STEP = (15.0 / 50.0) * IONIQ_6_RESPONSE_MULTIPLIER EV9_CANFD_SCC_DECEL_STEP = 10.0 / 50.0 -RAY_PEDAL_COMMAND_CAP = 0.35 # Ray firmware voltage scaling is route-derived; validate before raising. -RAY_PEDAL_RATE_UP = 0.012 # per 25 Hz command (0.30 normalized pedal per second) +RAY_PEDAL_COMMAND_CAP = 0.45 +RAY_PEDAL_RATE_UP = 0.02 RAY_PEDAL_RATE_DOWN = 0.06 GENESIS_G90_STOP_HOLD_SPEED_BP = [0.0, 0.03, 0.08, 0.16, 0.3, 0.5, 0.8, 1.2, 2.0, 3.0] GENESIS_G90_STOP_HOLD_ACCEL_V = [-0.10, -0.10, -0.12, -0.18, -0.30, -0.50, -0.75, -1.00, -1.40, -1.80] @@ -809,12 +809,12 @@ class CarController(CarControllerBase): # Button messages if not self.long_active_ecu: - if self.cancel_counter > CANCEL_BUTTON_DELAY_FRAMES: - can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP)) - elif self._ray_pedal and CC.enabled and CS.out.cruiseState.enabled: - if (self.frame - self.last_button_frame) * DT_CTRL > 0.1: + if self._ray_pedal and CC.enabled and CS.out.cruiseState.enabled: + if (self.frame - self.last_button_frame) * DT_CTRL > 0.04: can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP)) self.last_button_frame = self.frame + elif self.cancel_counter > CANCEL_BUTTON_DELAY_FRAMES: + can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP)) elif CC.cruiseControl.resume and not self._ray_pedal: # send resume at a max freq of 10Hz if (self.frame - self.last_button_frame) * DT_CTRL > 0.1: @@ -829,11 +829,11 @@ class CarController(CarControllerBase): if self._ray_pedal and self.frame % 4 == 0: pedal_ready = CS.ray_pedal_valid and CS.ray_pedal_state == 0 pedal_active = (CC.longActive and pedal_ready and not CC.cruiseControl.override and - not CS.out.gasPressed and not CS.out.brakePressed and - not CS.out.cruiseState.enabled) + not CS.out.gasPressed and not CS.out.brakePressed) if pedal_active: - target = float(np.clip(accel / CarControllerParams.ACCEL_MAX * RAY_PEDAL_COMMAND_CAP, - 0.0, RAY_PEDAL_COMMAND_CAP)) + pedal_offset = float(np.interp(CS.out.vEgo, [0., 2., 4., 8., 12., 20.], + [0.08, 0.13, 0.18, 0.25, 0.30, 0.32])) + target = float(np.clip(pedal_offset + accel * 0.32, 0.0, RAY_PEDAL_COMMAND_CAP)) self._ray_pedal_gas_last = rate_limit( target, self._ray_pedal_gas_last, -RAY_PEDAL_RATE_DOWN, RAY_PEDAL_RATE_UP, ) diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index ddeb94b579..a1dc76ba09 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -333,8 +333,8 @@ class CarInterface(CarInterfaceBase): if candidate == CAR.HYUNDAI_ELANTRA_2021: ret.longitudinalActuatorDelay = 0.22 - ret.stopAccel = -0.85 - ret.stoppingDecelRate = 0.35 + ret.stopAccel = -1.1 + ret.stoppingDecelRate = 0.55 if candidate == CAR.HYUNDAI_ELANTRA_HEV_2024: ret.longitudinalActuatorDelay = 0.22 diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index ad80ed83a7..ec49bb1a8b 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -1630,8 +1630,8 @@ class TestHyundaiFingerprint: CP = CarInterface.get_params(CAR.HYUNDAI_ELANTRA_2021, gen_empty_fingerprint(), [], True, False, False, toggles) assert CP.longitudinalActuatorDelay == pytest.approx(0.22) - assert CP.stopAccel == pytest.approx(-0.85) - assert CP.stoppingDecelRate == pytest.approx(0.35) + assert CP.stopAccel == pytest.approx(-1.1) + assert CP.stoppingDecelRate == pytest.approx(0.55) def test_elantra_hev_2024_longitudinal_delay_matches_observed_response(self): toggles = get_test_toggles() diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py b/opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py index 4fc6c7fea0..25e35f3eb9 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py @@ -167,23 +167,23 @@ def test_ray_controller_heartbeats_and_only_actuates_when_ready(speed): controller.frame = 16 messages = controller.create_can_msgs(True, 0, False, 0.0, 2.0, False, hud, actuators, CS, CC, 2, 0) - assert next(dat for addr, dat, bus in messages if addr == 0x200 and bus == 0)[:4] == bytes(4) - assert any(addr == 0x4F1 and bus == 0 for addr, _, bus in messages) # cancel stock CC + assert next(dat for addr, dat, bus in messages if addr == 0x200 and bus == 0)[4] & 0x80 + assert any(addr == 0x4F1 and bus == 0 and dat[0] & 7 == 4 for addr, dat, bus in messages) CS.out.cruiseState.enabled = False CS.out.brakePressed = True assert pedal_msg(2.0, 20)[:4] == bytes(4) CS.out.brakePressed = False assert pedal_msg(2.0, 24)[4] & 0x80 - assert controller._ray_pedal_gas_last == pytest.approx(0.012) + assert controller._ray_pedal_gas_last == pytest.approx(0.02) assert pedal_msg(2.0, 28)[4] & 0x80 - assert controller._ray_pedal_gas_last == pytest.approx(0.024) + assert controller._ray_pedal_gas_last == pytest.approx(0.04) CS.out.brakePressed = True assert pedal_msg(2.0, 32)[:4] == bytes(4) assert controller._ray_pedal_gas_last == 0.0 CS.out.brakePressed = False assert pedal_msg(2.0, 36)[4] & 0x80 - assert controller._ray_pedal_gas_last == pytest.approx(0.012) + assert controller._ray_pedal_gas_last == pytest.approx(0.02) CC.longActive = False assert pedal_msg(2.0, 40)[:4] == bytes(4) @@ -198,6 +198,11 @@ def test_ray_controller_heartbeats_and_only_actuates_when_ready(speed): CS.ray_pedal_state = fault assert pedal_msg(2.0, 48 + 4 * fault)[:4] == bytes(4) + CS.ray_pedal_state = 0 + CS.out.vEgo = 10.0 + assert pedal_msg(0.0, 72)[4] & 0x80 + assert pedal_msg(-1.0, 76)[:4] == bytes(4) + @pytest.mark.parametrize("candidate", [CAR.KIA_RAY_EV, CAR.HYUNDAI_KONA_EV_NON_SCC]) def test_ray_stock_cruise_cancellation_survives_accelerator_override(candidate): @@ -236,6 +241,7 @@ def test_ray_stock_cruise_cancellation_survives_accelerator_override(candidate): assert pedal[:4] == bytes(4) assert not (pedal[4] & 0x80) assert not cancel_frames(messages(24)) # retain the existing cancellation rate limit + assert cancel_frames(messages(25)) assert cancel_frames(messages(32)) CS.out.cruiseState.enabled = False diff --git a/opendbc_repo/opendbc/safety/modes/hyundai.h b/opendbc_repo/opendbc/safety/modes/hyundai.h index d60ffac42c..aa32ac12ed 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai.h @@ -330,7 +330,7 @@ static bool hyundai_tx_hook(const CANPacket_t *msg) { const int expected_track2 = 497 + (2 * ((int)track1 - 264)); if ((msg->data[4] & 0x70U) != 0U || (msg->data[5] != hyundai_ray_pedal_checksum(msg)) || - (enabled && (track1 < 264U || track1 > 397U || track2 < 497U || track2 > 766U || + (enabled && (track1 < 264U || track1 > 435U || track2 < 497U || track2 > 843U || SAFETY_ABS((int)track2 - expected_track2) > 40)) || (!enabled && ((track1 != 0U) || (track2 != 0U))) || longitudinal_interceptor_checks(msg) || diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_ray_pedal.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_ray_pedal.py index 111045686c..882ee87cc0 100644 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_ray_pedal.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_ray_pedal.py @@ -21,8 +21,8 @@ def test_ray_pedal_tx_isolation_and_limits(param): has_ray_signature = param in (0x9405, 0x9C05) assert tx(0) is has_ray_signature - assert tx(0.35) is has_ray_signature - assert not tx(0.36) # above the Ray-only initial command cap + assert tx(0.45) is has_ray_signature + assert not tx(0.46) assert not tx(1.0) if has_ray_signature: diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 529c8d65a2..64c42db75b 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -239,7 +239,7 @@ GENESIS_GV70_UNWIND_FF_JERK = 0.10 GENESIS_GV70_UNWIND_FF_JERK_WIDTH = 0.10 GENESIS_GV70_UNWIND_FF_SPEED = 10.0 * CV.MPH_TO_MS GENESIS_GV70_UNWIND_FF_SPEED_WIDTH = 4.0 * CV.MPH_TO_MS -GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_MAX = 0.18 +GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_MAX = 0.20 GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_SPEED = 50.0 * CV.MPH_TO_MS GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH = 8.0 * CV.MPH_TO_MS GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_ERROR = 0.18 @@ -276,6 +276,9 @@ 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_CURVE_TURN_IN_JERK_REDUCTION = 0.50 +GENESIS_G70_CURVE_TURN_IN_SPEED_BP = [20.0, 25.0] +GENESIS_G70_CURVE_TURN_IN_LAT_BP = [0.35, 0.70] 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 @@ -3312,6 +3315,12 @@ def get_genesis_g70_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH) deadzone = GENESIS_G70_FRICTION_JERK_DEADZONE_MAX * speed_weight * center_weight + if desired_lateral_accel * desired_lateral_jerk > 0.0: + turn_in_weight = (np.interp(v_ego, GENESIS_G70_CURVE_TURN_IN_SPEED_BP, [0.0, 1.0]) * + np.interp(abs(desired_lateral_accel), GENESIS_G70_CURVE_TURN_IN_LAT_BP, [0.0, 1.0])) + deadzone += (GENESIS_G70_CURVE_TURN_IN_JERK_REDUCTION * turn_in_weight * + max(abs(desired_lateral_jerk) - deadzone, 0.0)) + overshoot = max(abs(measured_lateral_accel) - abs(desired_lateral_accel), 0.0) if (desired_lateral_accel * desired_lateral_jerk < 0.0 and desired_lateral_accel * measured_lateral_accel > 0.0 and overshoot > 0.0): diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index 8b9ac5fa6a..7e36ff0b3b 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -23,6 +23,7 @@ HONDA_ACCORD_LOW_SPEED_STOP_MAX_LEAD_SPEED = 1.0 HONDA_ACCORD_STANDSTILL_GUARD_MAX_EGO_SPEED = 0.25 HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE = 1.25 GENESIS_GV70_ELECTRIFIED_LEAD_FOLLOW_JERK_SCALE = 1.75 +KIA_NIRO_EV_LEAD_FOLLOW_JERK_SCALE = 1.5 GENESIS_GV70_ELECTRIFIED_SCC_JERK_UPPER = 1.5 GENESIS_GV70_ELECTRIFIED_SCC_JERK_LOWER = 2.0 GENESIS_GV70_ELECTRIFIED_SCC_URGENT_JERK_LOWER = 5.0 @@ -517,9 +518,11 @@ def get_untracked_slow_lead_decel_scale(CP): def get_lead_follow_jerk_scale(CP): - """Spread the lead-source transition for cars with a sharp vision-lead handoff.""" + """Apply vehicle-specific acceleration smoothing while following a lead.""" if getattr(CP, "brand", "") == "hyundai" and str(getattr(CP, "carFingerprint", "")) == "HYUNDAI_ELANTRA_2021": return HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE + if getattr(CP, "brand", "") == "hyundai" and str(getattr(CP, "carFingerprint", "")) == "KIA_NIRO_EV": + return KIA_NIRO_EV_LEAD_FOLLOW_JERK_SCALE if ( getattr(CP, "brand", "") == "hyundai" and str(getattr(CP, "carFingerprint", "")) == "GENESIS_GV70_ELECTRIFIED_1ST_GEN" diff --git a/selfdrive/controls/tests/test_g70_friction_threshold.py b/selfdrive/controls/tests/test_g70_friction_threshold.py index 75c70390bc..ce540c8747 100644 --- a/selfdrive/controls/tests/test_g70_friction_threshold.py +++ b/selfdrive/controls/tests/test_g70_friction_threshold.py @@ -40,3 +40,29 @@ def test_symmetric_and_continuous(): 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 + + +@pytest.mark.parametrize('speed,accel,jerk,actual', [ + (30, 0.2, 0.8, 0.1), (30, 0.35, 0.8, 0.3), (20, 1.0, 0.8, 0.8), + (10, 1.0, 0.8, 0.8), (30, 1.0, -0.8, 1.2), (30, 1.0, 0.0, 1.2), +]) +def test_turn_in_preserves_center_low_speed_and_unwind(monkeypatch, speed, accel, jerk, actual): + revised = tunes.get_genesis_g70_friction_jerk_deadzone(speed, accel, jerk, actual) + monkeypatch.setattr(tunes, 'GENESIS_G70_CURVE_TURN_IN_JERK_REDUCTION', 0.0) + assert revised == tunes.get_genesis_g70_friction_jerk_deadzone(speed, accel, jerk, actual) + + +@pytest.mark.parametrize('direction', [-1, 1]) +@pytest.mark.parametrize('jerk', [0.1, 0.5, 1.0]) +def test_curve_turn_in_halves_remaining_jerk(monkeypatch, direction, jerk): + revised = tunes.get_genesis_g70_friction_jerk_deadzone(30, direction, direction * jerk, direction * 0.8) + monkeypatch.setattr(tunes, 'GENESIS_G70_CURVE_TURN_IN_JERK_REDUCTION', 0.0) + original = tunes.get_genesis_g70_friction_jerk_deadzone(30, direction, direction * jerk, direction * 0.8) + assert max(jerk - revised, 0) == pytest.approx(0.5 * max(jerk - original, 0)) + + +@pytest.mark.parametrize('speed,accel', [(20, 0.8), (25, 0.8), (30, 0.35), (30, 0.7)]) +def test_curve_turn_in_blend_continuity(speed, accel): + low = tunes.get_genesis_g70_friction_jerk_deadzone(speed - 1e-6, accel - 1e-6, 0.8) + high = tunes.get_genesis_g70_friction_jerk_deadzone(speed + 1e-6, accel + 1e-6, 0.8) + assert abs(high - low) < 1e-5 diff --git a/selfdrive/controls/tests/test_starpilot_planner.py b/selfdrive/controls/tests/test_starpilot_planner.py index 712f96141f..fddc1a95ff 100644 --- a/selfdrive/controls/tests/test_starpilot_planner.py +++ b/selfdrive/controls/tests/test_starpilot_planner.py @@ -48,6 +48,7 @@ def test_force_stop_jerk_scale_is_platform_specific(): def test_lead_follow_jerk_scale_is_platform_specific(): assert get_lead_follow_jerk_scale(SimpleNamespace(brand="hyundai", carFingerprint="HYUNDAI_ELANTRA_2021")) == 1.25 + assert get_lead_follow_jerk_scale(SimpleNamespace(brand="hyundai", carFingerprint="KIA_NIRO_EV")) == 1.5 assert get_lead_follow_jerk_scale(SimpleNamespace(brand="hyundai", carFingerprint="GENESIS_GV70_ELECTRIFIED_1ST_GEN")) == 1.75 assert get_lead_follow_jerk_scale(SimpleNamespace(brand="ford", carFingerprint="FORD_F_150_LIGHTNING_MK1")) == 1.35 assert get_lead_follow_jerk_scale(SimpleNamespace(brand="honda", carFingerprint="HONDA_CRV_5G")) == 1.35 diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index 4a8d09ed35..3f4d700320 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -159,6 +159,7 @@ class FordLateralController: self.human_turn = HumanTurnDetector() self.manual_turn_latched = False self.manual_turn_recovery_timer = 0.0 + self.manual_turn_direction = 0.0 self.curvature_samples = deque(maxlen=max(2, round(0.3 / STEER_DT))) self.curvature_last = 0.0 self.desired_curvature_last = 0.0 @@ -373,11 +374,12 @@ class FordLateralController: )) return speed_weight * curvature_weight * preview_weight * acceleration_weight - def _manual_turn(self, CC, CS) -> bool: + def _manual_turn(self, CC, CS, desired: float) -> bool: if not CC.latActive: self.human_turn.reset() self.manual_turn_latched = False self.manual_turn_recovery_timer = 0.0 + self.manual_turn_direction = 0.0 return False detected = self.human_turn.update( self.human_turn_enabled, CS.out.steeringPressed, CS.out.steeringAngleDeg) @@ -387,6 +389,7 @@ class FordLateralController: if not self.human_turn_enabled: self.manual_turn_latched = False self.manual_turn_recovery_timer = 0.0 + self.manual_turn_direction = 0.0 return False blinker_direction = float(CS.out.rightBlinker) - float(CS.out.leftBlinker) @@ -397,9 +400,14 @@ class FordLateralController: ) if detected or driver_turning_with_signal: self.manual_turn_latched = True + if blinker_direction != 0.0: + self.manual_turn_direction = blinker_direction + elif self.manual_turn_direction == 0.0: + self.manual_turn_direction = -float(np.sign(CS.out.steeringAngleDeg)) if not self.manual_turn_latched: self.manual_turn_recovery_timer = 0.0 + self.manual_turn_direction = 0.0 return False if (CS.out.steeringPressed or blinker_direction != 0.0 or @@ -408,8 +416,14 @@ class FordLateralController: else: self.manual_turn_recovery_timer += STEER_DT if self.manual_turn_recovery_timer + 1e-9 >= MANUAL_TURN_RECOVERY_SECONDS: - self.manual_turn_latched = False - self.manual_turn_recovery_timer = 0.0 + current = self._current_curvature(CS) + if (self.manual_turn_direction * desired > 0.0 and + self.manual_turn_direction * (desired - current) > CarControllerParams.CURVATURE_ERROR): + self.manual_turn_recovery_timer = MANUAL_TURN_RECOVERY_SECONDS + else: + self.manual_turn_latched = False + self.manual_turn_recovery_timer = 0.0 + self.manual_turn_direction = 0.0 return self.manual_turn_latched @@ -419,12 +433,13 @@ class FordLateralController: self.human_turn.reset() self.manual_turn_latched = False self.manual_turn_recovery_timer = 0.0 + self.manual_turn_direction = 0.0 self.curvature_samples.clear() self.curvature_last = 0.0 self.desired_curvature_last = 0.0 return FordLateralResult() - manual_turn = self._manual_turn(CC, CS) + manual_turn = self._manual_turn(CC, CS, float(actuators.curvature)) if manual_turn or CS.out.vEgoRaw < 0.1: self.curvature_samples.clear() self.curvature_last = 0.0 diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index d49f5a4599..0aea2aaa2d 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -880,7 +880,7 @@ def test_mach_e_signaled_manual_turn_yields_until_inputs_settle(controller): result = controller.update(CC, car_state(), actuators) assert not result.active - result = controller.update(CC, car_state(), actuators) + result = controller.update(CC, car_state(curvature=0.006), actuators) assert result.active assert result.curvature > 0.0 @@ -901,7 +901,7 @@ def test_mach_e_manual_turn_waits_for_wheel_to_unwind(controller): for _ in range(4): result = controller.update(CC, car_state(steering_angle=-10.0), actuators) assert not result.active - result = controller.update(CC, car_state(steering_angle=-10.0), actuators) + result = controller.update(CC, car_state(curvature=0.006, steering_angle=-10.0), actuators) assert result.active @@ -921,10 +921,42 @@ def test_mach_e_left_manual_turn_waits_for_wheel_to_unwind(controller): for _ in range(4): result = controller.update(CC, car_state(steering_angle=10.0), actuators) assert not result.active - result = controller.update(CC, car_state(steering_angle=10.0), actuators) + result = controller.update(CC, car_state(curvature=-0.006, steering_angle=10.0), actuators) assert result.active +@pytest.mark.parametrize("sign", (-1.0, 1.0)) +def test_mach_e_manual_turn_waits_for_path_agreement_after_driver_release(controller, sign): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + CC = SimpleNamespace(latActive=True, actuators=SimpleNamespace(curvature=sign * 0.009)) + turning = car_state(steering_pressed=True, steering_angle=-sign * 30.0, + steering_torque=-sign * 2.0, left_blinker=sign < 0.0, + right_blinker=sign > 0.0) + assert not controller.update(CC, turning, CC.actuators).active + for _ in range(40): + result = controller.update(CC, car_state(curvature=sign * 0.001, + steering_angle=-sign * 5.0), CC.actuators) + assert not result.active + assert result.curvature == 0.0 + assert controller.manual_turn_direction == sign + + result = controller.update(CC, car_state(curvature=sign * 0.008, + steering_angle=-sign * 5.0), CC.actuators) + assert result.active + assert controller.manual_turn_direction == 0.0 + + +def test_mach_e_manual_turn_releases_for_opposite_path_request(controller): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + CC = SimpleNamespace(latActive=True, actuators=SimpleNamespace(curvature=-0.009)) + assert not controller.update(CC, car_state(steering_pressed=True, steering_angle=30.0, + steering_torque=2.0, left_blinker=True), CC.actuators).active + CC.actuators.curvature = 0.009 + for _ in range(4): + assert not controller.update(CC, car_state(curvature=-0.001), CC.actuators).active + assert controller.update(CC, car_state(curvature=-0.001), CC.actuators).active + + def test_non_mach_e_signaled_turn_does_not_latch(controller): CC = SimpleNamespace(latActive=True) result = controller.update(CC, car_state( @@ -988,5 +1020,7 @@ def test_mach_e_manual_turn_latch_resets_with_lateral_control(controller): right_blinker=True) assert not controller.update(SimpleNamespace(latActive=True), turning, actuators).active + assert controller.manual_turn_direction == 1.0 assert not controller.update(SimpleNamespace(latActive=False), turning, actuators).active + assert controller.manual_turn_direction == 0.0 assert controller.update(SimpleNamespace(latActive=True), car_state(), actuators).active