diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 8d553ac60f..69313438b0 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -17,7 +17,7 @@ from opendbc.car.hyundai.values import HyundaiFlags, HyundaiSafetyFlags, Hyundai from opendbc.car.interfaces import CarControllerBase from opendbc.car.vehicle_model import VehicleModel from openpilot.common.params import Params -from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_hyundai_canfd_scc_jerk_limits +from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_hyundai_canfd_scc_jerk_limits, shape_hyundai_canfd_scc_accel from openpilot.starpilot.common.testing_grounds import testing_ground VisualAlert = structs.CarControl.HUDControl.VisualAlert @@ -1064,8 +1064,14 @@ class CarController(CarControllerBase): if self.frame % 2 == 0: lead_visible, lead_distance, lead_rel_speed = self._get_canfd_scc_lead_state(CC, CS, now_nanos) if self.CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN: - scc_jerk_limits = get_hyundai_canfd_scc_jerk_limits(self.CP) + scc_jerk_limits = get_hyundai_canfd_scc_jerk_limits(self.CP, stopping, accel) + raw_accel = accel + accel = shape_hyundai_canfd_scc_accel( + self.CP, CC.enabled, CC.cruiseControl.override, stopping, accel, self.accel_last, + ) acc_kwargs = { + "direct_accel": True, + "raw_accel": raw_accel, "jerk_upper": scc_jerk_limits[0], "jerk_lower": scc_jerk_limits[1], "lead_distance": lead_distance, diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index 75a681fb31..c4a7ef23de 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -704,13 +704,13 @@ def create_ioniq_6_cluster_lane_change_messages(CAN, frame, side=None): def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, main_mode_acc=1, jerk_lower=None, jerk_upper=None, direct_accel=False, - lead_distance=None, lead_rel_speed=None, lead_visible=None, cruise_info=None): + lead_distance=None, lead_rel_speed=None, lead_visible=None, cruise_info=None, raw_accel=None): jerk = 5 jn = jerk / 50 if not enabled or gas_override: a_val, a_raw = 0, 0 elif direct_accel: - a_raw = accel + a_raw = accel if raw_accel is None else raw_accel a_val = accel else: a_raw = accel diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index f721b0e9a6..74c95eac17 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -130,14 +130,12 @@ class CarController(CarControllerBase): self.angle_override_confirm_frames = 0 self.angle_handoff_active = False - def _angle_manual_handoff(self, CS, lat_active, use_steering_pressed=False): + def _angle_manual_handoff(self, CS, lat_active): if not lat_active: self._reset_angle_handoff() return False driver_override = self._update_angle_driver_override(CS) - if use_steering_pressed: - driver_override = driver_override or getattr(CS.out, "steeringPressed", False) steering_rate = abs(getattr(CS.out, "steeringRateDeg", 0.0)) if driver_override: self.angle_handoff_active = True @@ -216,33 +214,23 @@ class CarController(CarControllerBase): else: self.ascent_aol_arm_frames = _ASCENT_AOL_ARM_FRAMES if lkas_available else 0 - manual_handoff = self._angle_manual_handoff( - CS, lkas_available, use_steering_pressed=self.CP.carFingerprint == CAR.SUBARU_OUTBACK_2023, - ) + if self.CP.carFingerprint == CAR.SUBARU_OUTBACK_2023: + manual_handoff = False + else: + manual_handoff = self._angle_manual_handoff(CS, lkas_available) lkas_active = lkas_available and not manual_handoff if lkas_active and not self.angle_lkas_active: self.apply_steer_last = CS.out.steeringAngleDeg - if self.CP.carFingerprint in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023): - apply_steer = apply_std_steer_angle_limits( - CC.actuators.steeringAngleDeg, - self.apply_steer_last, - CS.out.vEgoRaw, - CS.out.steeringAngleDeg, - lkas_active, - self.p.FIXED_ANGLE_LIMITS, - ) - else: - apply_steer = apply_steer_angle_limits_vm( - steer_target, - self.apply_steer_last, - CS.out.vEgoRaw, - CS.out.steeringAngleDeg, - lkas_active, - self.p, - self.VM, - ) + apply_steer = apply_std_steer_angle_limits( + CC.actuators.steeringAngleDeg, + self.apply_steer_last, + CS.out.vEgoRaw, + CS.out.steeringAngleDeg, + lkas_active, + self.p.FIXED_ANGLE_LIMITS, + ) self.apply_steer_last = apply_steer self.angle_lkas_active = lkas_active return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus) @@ -380,9 +368,11 @@ class CarController(CarControllerBase): CC.longActive, hud_control.leadVisible, self.status_bus)) - can_sends.append(subarucan.create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, self._lkas_status_active(CC), hud_control.visualAlert, - hud_control.leftLaneVisible, hud_control.rightLaneVisible, - hud_control.leftLaneDepart, hud_control.rightLaneDepart, self.status_bus)) + can_sends.append(subarucan.create_es_lkas_state( + self.packer, self.frame // 10, CS.es_lkas_state_msg, self._lkas_status_active(CC), hud_control.visualAlert, + hud_control.leftLaneVisible, hud_control.rightLaneVisible, + hud_control.leftLaneDepart, hud_control.rightLaneDepart, self.status_bus, + )) if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index b9ccce9c95..bd051712f7 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py @@ -643,8 +643,8 @@ def test_angle_controller_uses_fixed_angle_rate_limits(platform): assert CS.out.steeringAngleDeg < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < -25.0 -@pytest.mark.parametrize("platform", (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023)) -def test_angle_controller_reengages_immediately_after_manual_steering_stops(platform): +def test_ascent_angle_controller_reengages_immediately_after_manual_steering_stops(): + platform = CAR.SUBARU_ASCENT_2023 CP = CarInterface.get_non_essential_params(platform) controller = CarController({}, CP) CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-10.0)) @@ -775,7 +775,7 @@ def test_ascent_angle_controller_does_not_delay_normal_engagement(): def test_lkas_hud_state_uses_angle_request_state(): update_source = inspect.getsource(CarController.update) - assert "create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, self._lkas_status_active(CC)" in update_source + assert "CS.es_lkas_state_msg, self._lkas_status_active(CC), hud_control.visualAlert" in update_source assert "create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.enabled" not in update_source @@ -795,7 +795,7 @@ def test_lkas_hud_active_bit_follows_lateral_state(enabled, expected): assert parser.vl["ES_LKAS_State"]["LKAS_ACTIVE"] == expected -def test_outback_manual_steering_releases_angle_request_before_lkas_fault(): +def test_outback_manual_steering_keeps_cooperative_angle_request(): CP = CarInterface.get_non_essential_params(CAR.SUBARU_OUTBACK_2023) controller = CarController({}, CP) CC = SimpleNamespace( @@ -807,19 +807,22 @@ def test_outback_manual_steering_releases_angle_request_before_lkas_fault(): vEgoRaw=0.9, steeringAngleDeg=-57.0, steeringRateDeg=-45.0, - steeringTorque=-127.0, + steeringTorque=0.0, steeringPressed=True, gearShifter=structs.CarState.GearShifter.drive, standstill=False, )) parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main) - msg = controller.lateral_angle(CC, CS) - parser.update([(1, [msg])]) + for frame, steering_torque in enumerate((-79.0, -81.0, -170.0, -250.0, -250.0, 79.0, 81.0, 170.0, 250.0, 250.0), start=1): + CS.out.steeringTorque = steering_torque + CS.out.steeringPressed = abs(steering_torque) > 80.0 + msg = controller.lateral_angle(CC, CS) + parser.update([(frame, [msg])]) - assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0 - assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg) - assert not controller._lkas_status_active(CC) + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1 + assert CC.actuators.steeringAngleDeg < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < CS.out.steeringAngleDeg + assert controller._lkas_status_active(CC) def test_ascent_hud_waits_for_angle_request(): diff --git a/opendbc_repo/opendbc/car/tesla/interface.py b/opendbc_repo/opendbc/car/tesla/interface.py index b08058abd9..fbaa9e6675 100644 --- a/opendbc_repo/opendbc/car/tesla/interface.py +++ b/opendbc_repo/opendbc/car/tesla/interface.py @@ -39,6 +39,7 @@ class CarInterface(CarInterfaceBase): ret.steerAtStandstill = True ret.steerControlType = structs.CarParams.SteerControlType.angle ret.radarUnavailable = Bus.radar not in DBC[candidate] + ret.radarTimeStepDEPRECATED = 0.125 ret.alphaLongitudinalAvailable = True if alpha_long: diff --git a/opendbc_repo/opendbc/car/tesla/tests/test_hw1.py b/opendbc_repo/opendbc/car/tesla/tests/test_hw1.py index ca610a3344..0b90ee498a 100644 --- a/opendbc_repo/opendbc/car/tesla/tests/test_hw1.py +++ b/opendbc_repo/opendbc/car/tesla/tests/test_hw1.py @@ -20,6 +20,8 @@ def test_hw1_is_distinct_from_preap_and_does_not_change_can_buses(): assert preap.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.teslaPreAP assert hw1.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.tesla assert hw1.safetyConfigs[0].safetyParam == TeslaSafetyFlags.FLAG_HW1.value + assert hw1.radarTimeStepDEPRECATED == pytest.approx(0.125) + assert preap.radarTimeStepDEPRECATED == pytest.approx(0.05) assert CANBUS.party == 0 and CANBUS.autopilot_party == 2 assert CarState.get_can_parsers(hw1)[Bus.ap_pt].bus == 2 assert CarState.get_can_parsers(hw1)[Bus.chassis].message_states[0x211].ignore_alive diff --git a/opendbc_repo/opendbc/safety/modes/hyundai.h b/opendbc_repo/opendbc/safety/modes/hyundai.h index 09c324e67d..d60ffac42c 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai.h @@ -438,6 +438,10 @@ static bool hyundai_tx_hook(const CANPacket_t *msg) { return tx; } +static bool hyundai_fwd_hook(int bus_num, int addr) { + return (bus_num == 2) && (addr == 0x53E) && hyundai_has_lkas12; +} + static safety_config hyundai_init(uint16_t param) { static const CanMsg HYUNDAI_CAMERA_SCC_TX_MSGS[] = { HYUNDAI_COMMON_TX_MSGS(2, false) @@ -775,6 +779,7 @@ const safety_hooks hyundai_hooks = { .get_counter = hyundai_get_counter, .get_checksum = hyundai_get_checksum, .compute_checksum = hyundai_compute_checksum, + .fwd = hyundai_fwd_hook, }; const safety_hooks hyundai_legacy_hooks = { @@ -785,4 +790,5 @@ const safety_hooks hyundai_legacy_hooks = { .get_counter = hyundai_get_counter, .get_checksum = hyundai_get_checksum, .compute_checksum = hyundai_compute_checksum, + .fwd = hyundai_fwd_hook, }; diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai.py b/opendbc_repo/opendbc/safety/tests/test_hyundai.py index cfd4f6c7ea..b82cccdd79 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai.py @@ -434,9 +434,11 @@ def test_hyundai_lkas12_tx_requires_stock_camera_message(): lkas12 = libsafety_py.make_CANPacket(0x53E, 0, bytes(6)) assert not safety.safety_tx_hook(lkas12) + assert safety.safety_fwd_hook(2, 0x53E) == 0 safety.safety_rx_hook(libsafety_py.make_CANPacket(0x53E, 2, bytes(6))) assert safety.safety_tx_hook(lkas12) + assert safety.safety_fwd_hook(2, 0x53E) == -1 class TestHyundaiLongitudinalSafety(HyundaiLongitudinalBase, TestHyundaiSafety): diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 0ce7de9a97..261d49eb2a 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -647,10 +647,6 @@ class LatControlTorque(LatControl): output_torque *= tucson_4th_gen_center_taper elif genesis_g70_active: output_torque *= genesis_g70_center_output_taper - output_torque *= get_genesis_g70_high_speed_transition_scale( - setpoint, desired_lateral_jerk, CS.vEgo, - ) - output_torque *= get_genesis_g70_curve_unwind_output_scale(setpoint, desired_lateral_jerk, CS.vEgo) output_torque *= get_genesis_g70_high_speed_error_scale( setpoint, measurement, desired_lateral_jerk, CS.vEgo, ) @@ -659,7 +655,7 @@ class LatControlTorque(LatControl): output_torque = float(np.clip(output_torque, -low_speed_output_limit, low_speed_output_limit)) if not CS.steeringPressed: output_torque = get_genesis_g70_stabilized_output( - output_torque, self.prev_output_torque, setpoint, desired_lateral_jerk, CS.vEgo, self.dt, + output_torque, self.prev_output_torque, setpoint, measurement, desired_lateral_jerk, CS.vEgo, self.dt, ) elif self.is_genesis_gv70: output_torque *= get_genesis_gv70_center_output_scale(setpoint, CS.vEgo) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 29e0eeb40b..9c3997f3c1 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -265,11 +265,11 @@ GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_LAT = 0.12 GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_LAT_WIDTH = 0.10 GENESIS_GV70_OUTPUT_SMOOTHING_SPEED = 38.0 * CV.MPH_TO_MS GENESIS_GV70_OUTPUT_SMOOTHING_SPEED_WIDTH = 6.0 * CV.MPH_TO_MS -GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT = 0.48 +GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT = 0.60 GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH = 0.16 GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_RC = 0.42 -GENESIS_GV70_OUTPUT_SMOOTHING_CURVE_RC = 0.20 -GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_RC = 0.16 +GENESIS_GV70_OUTPUT_SMOOTHING_CURVE_RC = 0.14 +GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_RC = 0.12 GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE = 0.04 GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08 GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.55 @@ -303,13 +303,6 @@ GENESIS_G70_CENTER_OUTPUT_TAPER_LAT = 0.30 GENESIS_G70_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10 GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED = 18.0 GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 3.0 -GENESIS_G70_HIGH_SPEED_TRANSITION_DAMPING_MAX = 0.24 -GENESIS_G70_HIGH_SPEED_TRANSITION_DAMPING_SPEED = 45.0 * CV.MPH_TO_MS -GENESIS_G70_HIGH_SPEED_TRANSITION_DAMPING_SPEED_WIDTH = 8.0 * CV.MPH_TO_MS -GENESIS_G70_HIGH_SPEED_TRANSITION_DAMPING_LAT = 0.45 -GENESIS_G70_HIGH_SPEED_TRANSITION_DAMPING_LAT_WIDTH = 0.15 -GENESIS_G70_HIGH_SPEED_TRANSITION_DAMPING_JERK = 0.35 -GENESIS_G70_HIGH_SPEED_TRANSITION_DAMPING_JERK_WIDTH = 0.15 GENESIS_G70_LOW_SPEED_CENTER_TAPER_MAX = 0.06 GENESIS_G70_LOW_SPEED_CENTER_TAPER_LAT = 0.14 GENESIS_G70_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.05 @@ -328,13 +321,6 @@ GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT = 0.14 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT_WIDTH = 0.05 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED = 6.0 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 1.5 -GENESIS_G70_CURVE_UNWIND_OUTPUT_REDUCTION_MAX = 0.10 -GENESIS_G70_CURVE_UNWIND_SPEED = 18.0 -GENESIS_G70_CURVE_UNWIND_SPEED_WIDTH = 3.0 -GENESIS_G70_CURVE_UNWIND_LAT = 0.25 -GENESIS_G70_CURVE_UNWIND_LAT_WIDTH = 0.12 -GENESIS_G70_CURVE_UNWIND_JERK = 0.08 -GENESIS_G70_CURVE_UNWIND_JERK_WIDTH = 0.08 GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.34 GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.13 GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.17 @@ -353,20 +339,10 @@ GENESIS_G70_OUTPUT_SMOOTHING_SPEED = 40.0 * CV.MPH_TO_MS GENESIS_G70_OUTPUT_SMOOTHING_SPEED_WIDTH = 6.0 * CV.MPH_TO_MS GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT = 0.42 GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH = 0.14 -GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC = 0.45 -GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC = 0.22 -GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_RC = 0.20 -GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE = 0.04 -GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08 -GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.45 -GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC = 0.28 -GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_SPEED = 45.0 * CV.MPH_TO_MS -GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_SPEED_WIDTH = 5.0 * CV.MPH_TO_MS -GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT = 0.35 -GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT_WIDTH = 0.15 -GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_JERK = 0.25 -GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_JERK_WIDTH = 0.15 -GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_RC = 0.55 +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_ANGLE_OUTPUT_TAPER_MIN = 0.45 GENESIS_G70_ANGLE_OUTPUT_TAPER_START = 70.0 GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH = 6.0 @@ -3372,18 +3348,6 @@ def get_genesis_g70_center_output_scale(desired_lateral_accel: float, v_ego: flo return 1.0 - reduction -def get_genesis_g70_high_speed_transition_scale(desired_lateral_accel: float, - desired_lateral_jerk: float, v_ego: float) -> float: - speed_weight = _sigmoid((v_ego - GENESIS_G70_HIGH_SPEED_TRANSITION_DAMPING_SPEED) / - GENESIS_G70_HIGH_SPEED_TRANSITION_DAMPING_SPEED_WIDTH) - center_weight = _sigmoid((GENESIS_G70_HIGH_SPEED_TRANSITION_DAMPING_LAT - abs(desired_lateral_accel)) / - GENESIS_G70_HIGH_SPEED_TRANSITION_DAMPING_LAT_WIDTH) - jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_G70_HIGH_SPEED_TRANSITION_DAMPING_JERK) / - GENESIS_G70_HIGH_SPEED_TRANSITION_DAMPING_JERK_WIDTH) - reduction = (GENESIS_G70_HIGH_SPEED_TRANSITION_DAMPING_MAX * speed_weight * center_weight * jerk_weight) - return 1.0 - reduction - - def get_genesis_g70_low_speed_angle_damping(desired_angle_deg: float, actual_angle_deg: float, current_output_torque: float, v_ego: float) -> float: angle_error = desired_angle_deg - actual_angle_deg @@ -3422,20 +3386,6 @@ def get_genesis_g70_angle_output_scale(steering_angle_deg: float, output_torque: return 1.0 - ((1.0 - GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN) * angle_weight) -def get_genesis_g70_curve_unwind_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float, - v_ego: float) -> float: - if desired_lateral_accel * desired_lateral_jerk >= 0.0: - return 1.0 - speed_weight = _sigmoid((max(v_ego, 0.0) - GENESIS_G70_CURVE_UNWIND_SPEED) / - GENESIS_G70_CURVE_UNWIND_SPEED_WIDTH) - lateral_weight = _sigmoid((abs(desired_lateral_accel) - GENESIS_G70_CURVE_UNWIND_LAT) / - GENESIS_G70_CURVE_UNWIND_LAT_WIDTH) - jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_G70_CURVE_UNWIND_JERK) / - GENESIS_G70_CURVE_UNWIND_JERK_WIDTH) - reduction = (GENESIS_G70_CURVE_UNWIND_OUTPUT_REDUCTION_MAX * speed_weight * lateral_weight * jerk_weight) - return 1.0 - reduction - - def get_genesis_g70_unwind_ff_scale(setpoint: float, measured_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: if setpoint * desired_lateral_jerk >= 0.0 or setpoint * measured_lateral_accel <= 0.0: @@ -3472,45 +3422,23 @@ def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_acc def get_genesis_g70_stabilized_output(output_torque: float, prev_output_torque: float, - desired_lateral_accel: float, desired_lateral_jerk: float, - v_ego: float, dt: float) -> float: + desired_lateral_accel: float, measured_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float, dt: float) -> float: speed_weight = _sigmoid((max(v_ego, 0.0) - GENESIS_G70_OUTPUT_SMOOTHING_SPEED) / GENESIS_G70_OUTPUT_SMOOTHING_SPEED_WIDTH) center_weight = _sigmoid((GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT - abs(desired_lateral_accel)) / GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH) - curve_weight = 1.0 - center_weight - response_time = (GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC * curve_weight + + response_time = (GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC * (1.0 - center_weight) + GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC * center_weight) - unwind_phase = -desired_lateral_accel * desired_lateral_jerk - unwind_weight = _sigmoid((unwind_phase - GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE) / - GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH) - response_time += GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_RC * curve_weight * unwind_weight + measured_overshoot = (desired_lateral_accel * measured_lateral_accel > 0.0 and + abs(measured_lateral_accel) > abs(desired_lateral_accel) + GENESIS_G70_OUTPUT_SMOOTHING_OVERSHOOT) + reducing_output = (prev_output_torque * output_torque <= 0.0 or + abs(output_torque) < abs(prev_output_torque)) + if reducing_output or (desired_lateral_accel * desired_lateral_jerk < 0.0 and measured_overshoot): + response_time = GENESIS_G70_OUTPUT_SMOOTHING_RELEASE_RC - changing_direction = (abs(desired_lateral_accel) >= GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT and - prev_output_torque * desired_lateral_accel <= 0.0) - if changing_direction: - response_time = max(response_time, GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC) - - output_reversal = (prev_output_torque * output_torque < -0.0025 and - abs(desired_lateral_accel) >= GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT) - if output_reversal: - reversal_speed_weight = _sigmoid( - (max(v_ego, 0.0) - GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_SPEED) / - GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_SPEED_WIDTH - ) - reversal_lat_weight = _sigmoid( - (abs(desired_lateral_accel) - GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT) / - GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT_WIDTH - ) - reversal_jerk_weight = _sigmoid( - (abs(desired_lateral_jerk) - GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_JERK) / - GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_JERK_WIDTH - ) - reversal_weight = reversal_speed_weight * reversal_lat_weight * reversal_jerk_weight - response_time += reversal_weight * max(GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_RC - response_time, 0.0) - - output_alpha = dt / (max(response_time, 0.0) + dt) + output_alpha = dt / (response_time + dt) smoothed_output = prev_output_torque + output_alpha * (output_torque - prev_output_torque) return float(output_torque + speed_weight * (smoothed_output - output_torque)) diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index 4c3971fb4e..8b9ac5fa6a 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -25,6 +25,9 @@ HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE = 1.25 GENESIS_GV70_ELECTRIFIED_LEAD_FOLLOW_JERK_SCALE = 1.75 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 +GENESIS_GV70_ELECTRIFIED_SCC_URGENT_ACCEL = -1.0 +HYUNDAI_CANFD_SCC_FREQUENCY = 50.0 FORD_LIGHTNING_LEAD_FOLLOW_JERK_SCALE = 1.35 HONDA_CRV_5G_LEAD_FOLLOW_JERK_SCALE = 1.35 GM_SILVERADO_EARLY_FOLLOW_MIN_EGO_SPEED = 18.0 @@ -529,12 +532,31 @@ def get_lead_follow_jerk_scale(CP): return 1.0 -def get_hyundai_canfd_scc_jerk_limits(CP): +def get_hyundai_canfd_scc_jerk_limits(CP, stopping=False, accel=0.0): if str(getattr(CP, "carFingerprint", "")) == "GENESIS_GV70_ELECTRIFIED_1ST_GEN": - return GENESIS_GV70_ELECTRIFIED_SCC_JERK_UPPER, GENESIS_GV70_ELECTRIFIED_SCC_JERK_LOWER + jerk_lower = GENESIS_GV70_ELECTRIFIED_SCC_URGENT_JERK_LOWER \ + if stopping or accel <= GENESIS_GV70_ELECTRIFIED_SCC_URGENT_ACCEL \ + else GENESIS_GV70_ELECTRIFIED_SCC_JERK_LOWER + return GENESIS_GV70_ELECTRIFIED_SCC_JERK_UPPER, jerk_lower return None +def shape_hyundai_canfd_scc_accel(CP, enabled, gas_override, stopping, accel, accel_last): + jerk_limits = get_hyundai_canfd_scc_jerk_limits(CP, stopping, accel) + if jerk_limits is None: + return float(accel) + if not enabled or gas_override: + return 0.0 + + jerk_upper, jerk_lower = jerk_limits + + return float(np.clip( + accel, + accel_last - jerk_lower / HYUNDAI_CANFD_SCC_FREQUENCY, + accel_last + jerk_upper / HYUNDAI_CANFD_SCC_FREQUENCY, + )) + + def get_honda_accord_lead_departure_tune(CP): if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_ACCORD": return ( diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 86eeff200b..b10bca3a09 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -58,7 +58,6 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( get_subaru_impreza_pid_output_scale, get_genesis_gv70_low_speed_center_overshoot_scale, get_genesis_gv70_stabilized_output, - get_genesis_g70_high_speed_transition_scale, get_genesis_g70_stabilized_output, normalize_flm_overrides, set_flm_runtime_overrides, @@ -91,7 +90,6 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_genesis_g90_friction_scale, get_genesis_g90_friction_threshold, get_genesis_g70_center_output_scale, - get_genesis_g70_curve_unwind_output_scale, get_genesis_g70_angle_output_scale, get_genesis_g70_friction_jerk_deadzone, get_genesis_g70_friction_threshold, @@ -993,14 +991,6 @@ class TestLatControl: assert get_genesis_g70_low_speed_output_limit(0.0, 2.0) < 0.30 assert get_genesis_g70_low_speed_angle_damping(0.0, -20.0, 0.0, 2.0) < 0.0 assert get_genesis_g70_low_speed_angle_damping(0.0, 20.0, 0.0, 2.0) > 0.0 - assert get_genesis_g70_high_speed_transition_scale(0.0, 0.8, 65.0 * 0.44704) < \ - get_genesis_g70_high_speed_transition_scale(0.0, 0.1, 65.0 * 0.44704) - assert get_genesis_g70_high_speed_transition_scale(1.0, 0.8, 65.0 * 0.44704) > \ - get_genesis_g70_high_speed_transition_scale(0.0, 0.8, 65.0 * 0.44704) - assert get_genesis_g70_high_speed_transition_scale(0.0, 0.8, 20.0 * 0.44704) > \ - get_genesis_g70_high_speed_transition_scale(0.0, 0.8, 65.0 * 0.44704) - assert 0.88 < get_genesis_g70_curve_unwind_output_scale(0.7, -0.5, 25.0) < 1.0 - assert get_genesis_g70_curve_unwind_output_scale(0.7, 0.5, 25.0) == 1.0 assert get_genesis_g70_angle_output_scale(55.0, 1.0) > get_genesis_g70_angle_output_scale(85.0, 1.0) assert get_genesis_g70_angle_output_scale(85.0, -1.0) == pytest.approx(1.0) assert get_genesis_g70_friction_jerk_deadzone(25.0, 0.0) > 0.25 @@ -1018,6 +1008,37 @@ class TestLatControl: assert get_genesis_g70_high_speed_error_scale(0.7, 0.95, 0.8, 30.0) < \ get_genesis_g70_high_speed_error_scale(0.7, 0.45, 0.8, 30.0) + def test_genesis_g70_output_stabilizer_releases_faster_than_it_builds(self): + high_speed = 65.0 * 0.44704 + build = get_genesis_g70_stabilized_output(0.30, 0.10, 0.15, 0.10, 0.30, high_speed, DT_CTRL) + release = get_genesis_g70_stabilized_output(0.10, 0.30, 0.15, 0.30, -0.30, high_speed, DT_CTRL) + reversal = get_genesis_g70_stabilized_output(-0.20, 0.20, -0.15, 0.20, -0.30, high_speed, DT_CTRL) + low_speed = get_genesis_g70_stabilized_output(0.30, 0.10, 0.15, 0.10, 0.30, 5.0, DT_CTRL) + + assert abs(release - 0.10) < abs(build - 0.30) + assert reversal < 0.20 + assert low_speed == pytest.approx(0.30, abs=0.01) + + def test_genesis_g70_output_stabilizer_update_path(self, monkeypatch): + calls = [] + + def stabilized_output(output_torque, prev_output_torque, desired_lateral_accel, + measured_lateral_accel, desired_lateral_jerk, v_ego, dt): + calls.append((output_torque, prev_output_torque, desired_lateral_accel, + measured_lateral_accel, desired_lateral_jerk, v_ego, dt)) + return 0.123 + + monkeypatch.setattr(latcontrol_torque, "get_genesis_g70_stabilized_output", stabilized_output) + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.GENESIS_G70_2020) + CS.vEgo = 25.0 + output, _, lac_log = controller.update( + True, CS, VM, params, False, 0.0002, False, 0.2, None, None, starpilot_toggles, + ) + + assert calls + assert lac_log.active + assert output == pytest.approx(-0.123) + 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) @@ -1825,40 +1846,6 @@ class TestLatControl: assert lac_log.active assert 0.0 < abs(output) <= get_genesis_g70_low_speed_output_limit(0.0, CS.vEgo) - def test_genesis_g70_output_stabilizer_is_speed_and_phase_aware(self): - low_speed = get_genesis_g70_stabilized_output(-0.2, 0.2, 0.1, -0.4, 5.0, DT_CTRL) - high_speed_center = get_genesis_g70_stabilized_output(-0.2, 0.2, 0.1, -0.4, 30.0, DT_CTRL) - high_speed_wind = get_genesis_g70_stabilized_output(0.1, 0.3, 0.8, 0.5, 30.0, DT_CTRL) - high_speed_unwind = get_genesis_g70_stabilized_output(0.1, 0.3, 0.8, -0.5, 30.0, DT_CTRL) - high_speed_direction_change = get_genesis_g70_stabilized_output(-0.3, 0.3, -0.8, -0.5, 30.0, DT_CTRL) - low_speed_direction_change = get_genesis_g70_stabilized_output(-0.3, 0.3, -0.8, -0.5, 10.0, DT_CTRL) - - assert low_speed == pytest.approx(-0.2, abs=0.005) - assert abs(high_speed_center - 0.2) < abs(low_speed - 0.2) - assert high_speed_unwind > high_speed_wind > 0.1 - assert 0.2 < high_speed_direction_change < 0.3 - assert high_speed_direction_change > low_speed_direction_change - - def test_genesis_g70_output_stabilizer_update_path(self, monkeypatch): - calls = [] - - def stabilized_output(output_torque, prev_output_torque, desired_lateral_accel, - desired_lateral_jerk, v_ego, dt): - calls.append((output_torque, prev_output_torque, desired_lateral_accel, - desired_lateral_jerk, v_ego, dt)) - return 0.123 - - monkeypatch.setattr(latcontrol_torque, "get_genesis_g70_stabilized_output", stabilized_output) - controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.GENESIS_G70_2020) - CS.vEgo = 25.0 - output, _, lac_log = controller.update( - True, CS, VM, params, False, 0.0002, False, 0.2, None, None, starpilot_toggles, - ) - - assert calls - assert lac_log.active - assert output == pytest.approx(-0.123) - def test_ioniq_5_default_update_path(self): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_5) CarInterface = interfaces[HYUNDAI.HYUNDAI_IONIQ_5] diff --git a/selfdrive/controls/tests/test_starpilot_planner.py b/selfdrive/controls/tests/test_starpilot_planner.py index 77d6872ee5..712f96141f 100644 --- a/selfdrive/controls/tests/test_starpilot_planner.py +++ b/selfdrive/controls/tests/test_starpilot_planner.py @@ -2,12 +2,15 @@ import math from pathlib import Path from types import SimpleNamespace +import pytest + from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner, get_force_stop_jerk_scale from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_hyundai_canfd_scc_jerk_limits, get_lead_follow_jerk_scale, + shape_hyundai_canfd_scc_accel, ) import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_module @@ -56,9 +59,33 @@ def test_genesis_gv70_scc_jerk_limits_are_platform_specific(): other = SimpleNamespace(brand="hyundai", carFingerprint="HYUNDAI_IONIQ_6") assert get_hyundai_canfd_scc_jerk_limits(gv70) == (1.5, 2.0) + assert get_hyundai_canfd_scc_jerk_limits(gv70, stopping=True) == (1.5, 5.0) + assert get_hyundai_canfd_scc_jerk_limits(gv70, accel=-1.2) == (1.5, 5.0) assert get_hyundai_canfd_scc_jerk_limits(other) is None +def test_genesis_gv70_scc_accel_is_continuously_rate_limited(): + gv70 = SimpleNamespace(brand="hyundai", carFingerprint="GENESIS_GV70_ELECTRIFIED_1ST_GEN") + + accel = shape_hyundai_canfd_scc_accel(gv70, True, False, False, 1.0, 0.0) + assert accel == pytest.approx(0.03) + accel = shape_hyundai_canfd_scc_accel(gv70, True, False, False, 1.0, accel) + assert accel == pytest.approx(0.06) + accel = shape_hyundai_canfd_scc_accel(gv70, True, False, False, -0.5, accel) + assert accel == pytest.approx(0.02) + + +def test_genesis_gv70_scc_accel_preserves_urgent_braking_and_resets(): + gv70 = SimpleNamespace(brand="hyundai", carFingerprint="GENESIS_GV70_ELECTRIFIED_1ST_GEN") + other = SimpleNamespace(brand="hyundai", carFingerprint="HYUNDAI_IONIQ_6") + + assert shape_hyundai_canfd_scc_accel(gv70, True, False, False, -1.2, 0.0) == pytest.approx(-0.1) + assert shape_hyundai_canfd_scc_accel(gv70, True, False, True, -0.5, 0.0) == pytest.approx(-0.1) + assert shape_hyundai_canfd_scc_accel(gv70, False, False, False, 1.0, 0.5) == 0.0 + assert shape_hyundai_canfd_scc_accel(gv70, True, True, False, 1.0, 0.5) == 0.0 + assert shape_hyundai_canfd_scc_accel(other, True, False, False, 1.0, 0.0) == 1.0 + + def make_sm(planner, *, frame: int, v_ego: float, left_blinker: bool, right_blinker: bool = False, standstill: bool = False): return FakeSM(frame, { "radarState": SimpleNamespace( diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index d2247d89ce..70542f83cd 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -59,11 +59,11 @@ class StarPilotCard: self.accel_pressed = False self.always_on_lateral_allowed = False hyundai_flags = getattr(self.CP, "flags", 0) - kia_forte_non_scc = ( + self.kia_forte_non_scc = ( getattr(self.CP, "carFingerprint", None) in (HYUNDAI_CAR.KIA_FORTE_2019_NON_SCC, HYUNDAI_CAR.KIA_FORTE_2021_NON_SCC) and bool(hyundai_flags & HyundaiFlags.NON_SCC) ) - hyundai_aol_before_engagement = kia_forte_non_scc or getattr(self.CP, "carFingerprint", None) == HYUNDAI_CAR.GENESIS_G90 + hyundai_aol_before_engagement = self.kia_forte_non_scc or getattr(self.CP, "carFingerprint", None) == HYUNDAI_CAR.GENESIS_G90 self.hyundai_preserve_aol_across_reverse = getattr(self.CP, "carFingerprint", None) == HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID self.hyundai_aol_needs_engagement = ( self.CP.brand == "hyundai" and not (hyundai_flags & HyundaiFlags.CANFD) and not hyundai_aol_before_engagement @@ -259,6 +259,7 @@ class StarPilotCard: getattr(self.CP, "carFingerprint", None) == HYUNDAI_CAR.GENESIS_G70_2020 and starpilot_toggles.main_cruise_aol_toggle ) + forte_main_cruise_aol_managed = self.kia_forte_non_scc and starpilot_toggles.main_cruise_aol_toggle if carState.gearShifter in NON_DRIVING_GEARS or not g70_main_cruise_aol_managed: self.g70_main_cruise_aol_pending = False @@ -292,7 +293,7 @@ class StarPilotCard: # Wait for that state change before sending active LKAS11 torque. self.g70_main_cruise_aol_pending = True self.g70_main_cruise_aol_pending_frames = 0 - else: + elif not forte_main_cruise_aol_managed: self.always_on_lateral_allowed = not self.always_on_lateral_allowed elif starpilot_toggles.main_cruise_slc_adopt and starpilot_toggles.speed_limit_controller: self.params_memory.put_bool("SLCAdoptSpeedLimit", True) @@ -316,6 +317,9 @@ class StarPilotCard: self.g70_main_cruise_aol_pending = False self.g70_main_cruise_aol_pending_frames = 0 + if forte_main_cruise_aol_managed: + self.always_on_lateral_allowed = carState.cruiseState.available + if starpilot_toggles.always_on_lateral_main and not button_managed_aol: car_fingerprint = getattr(self.CP, "carFingerprint", None) pcm_cruise = getattr(self.CP, "pcmCruise", False) diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index c1b81c92c7..3eb4cde152 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -626,7 +626,46 @@ def test_hyundai_canfd_lkas_button_wrapped_enum_can_toggle_aol(monkeypatch, tmp_ assert ret.alwaysOnLateralEnabled is False -def test_kia_forte_non_scc_main_cruise_button_toggles_aol_immediately(monkeypatch, tmp_path): +@pytest.mark.parametrize("fingerprint", ( + spc.HYUNDAI_CAR.KIA_FORTE_2019_NON_SCC, + spc.HYUNDAI_CAR.KIA_FORTE_2021_NON_SCC, +)) +def test_kia_forte_non_scc_main_cruise_aol_follows_cruise_state(monkeypatch, tmp_path, fingerprint): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard( + SimpleNamespace( + brand="hyundai", + carFingerprint=fingerprint, + flags=spc.HyundaiFlags.NON_SCC, + ), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + assert card.kia_forte_non_scc + + car_state = make_car_state(button_events=[SimpleNamespace(type=spc.ButtonType.mainCruise, pressed=True)]) + starpilot_car_state = SimpleNamespace(distancePressed=False) + sm = make_sm() + toggles = make_toggles(always_on_lateral=True, main_cruise_aol_toggle=True) + + ret = card.update(car_state, starpilot_car_state, sm, toggles) + assert ret.alwaysOnLateralAllowed is False + assert ret.alwaysOnLateralEnabled is False + + car_state.buttonEvents = [] + car_state.cruiseState.available = True + ret = card.update(car_state, starpilot_car_state, sm, toggles) + assert ret.alwaysOnLateralAllowed is True + assert ret.alwaysOnLateralEnabled is True + + car_state.cruiseState.available = False + ret = card.update(car_state, starpilot_car_state, sm, toggles) + assert ret.alwaysOnLateralAllowed is False + assert ret.alwaysOnLateralEnabled is False + + +def test_kia_forte_non_scc_main_cruise_aol_restores_state_after_boot(monkeypatch, tmp_path): monkeypatch.setattr(spc, "Params", FakeParams) monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) @@ -638,19 +677,14 @@ def test_kia_forte_non_scc_main_cruise_button_toggles_aol_immediately(monkeypatc ), SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), ) + assert card.kia_forte_non_scc - car_state = make_car_state(button_events=[SimpleNamespace(type=spc.ButtonType.mainCruise, pressed=True)]) - starpilot_car_state = SimpleNamespace(distancePressed=False) - sm = make_sm() - toggles = make_toggles(always_on_lateral=True, main_cruise_aol_toggle=True) - - ret = card.update(car_state, starpilot_car_state, sm, toggles) - assert ret.alwaysOnLateralAllowed is True - assert ret.alwaysOnLateralEnabled is True - - car_state.buttonEvents = [] - car_state.cruiseState.available = False - ret = card.update(car_state, starpilot_car_state, sm, toggles) + ret = card.update( + make_car_state(available=True), + SimpleNamespace(distancePressed=False), + make_sm(), + make_toggles(always_on_lateral=True, main_cruise_aol_toggle=True), + ) assert ret.alwaysOnLateralAllowed is True assert ret.alwaysOnLateralEnabled is True