diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index f710b3ede7..667f16f6e8 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -42,9 +42,11 @@ 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.70 +RAY_PEDAL_COMMAND_CAP = 0.55 RAY_PEDAL_RATE_UP = 0.02 RAY_PEDAL_RATE_DOWN = 0.06 +RAY_PEDAL_OVERSPEED_CUTOFF = 0.5 +RAY_PEDAL_TAPER_BELOW_TARGET = 0.75 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] GENESIS_G90_STOP_HOLD_RELAX_SPEED_BP = [0.0, 0.08, 0.16, 0.3, 0.5, 0.8, 1.2, 2.0, 3.0] @@ -831,13 +833,27 @@ class CarController(CarControllerBase): pedal_active = (CC.longActive and pedal_ready and not CC.cruiseControl.override and not CS.out.gasPressed and not CS.out.brakePressed) if pedal_active: - pedal_offset = float(np.interp(CS.out.vEgo, [0., 2., 4., 8., 12., 20.], - [0.08, 0.13, 0.25, 0.44, 0.63, 0.68])) - pedal_gain = 0.65 if accel < 0.0 else 0.22 - target = float(np.clip(pedal_offset + accel * pedal_gain, 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, - ) + set_speed = hud_control.setSpeed + if not np.isfinite(set_speed) or not 1.0 <= set_speed <= 40.0: + self._ray_pedal_gas_last = 0.0 + else: + speed_error = set_speed - CS.out.vEgo + if speed_error <= -RAY_PEDAL_OVERSPEED_CUTOFF: + self._ray_pedal_gas_last = 0.0 + else: + pedal_offset = float(np.interp(CS.out.vEgo, [0., 2., 4., 8., 12., 20.], + [0.08, 0.13, 0.20, 0.32, 0.42, 0.48])) + pedal_gain = 2.0 if accel < 0.0 else 0.22 + target = float(np.clip(pedal_offset + accel * pedal_gain, 0.0, RAY_PEDAL_COMMAND_CAP)) + if speed_error < 0.0: + target *= float(np.clip(0.65 * (1.0 + speed_error / RAY_PEDAL_OVERSPEED_CUTOFF), 0.0, 1.0)) + elif speed_error < RAY_PEDAL_TAPER_BELOW_TARGET: + target *= 0.65 + 0.35 * speed_error / RAY_PEDAL_TAPER_BELOW_TARGET + if target <= 0.001: + self._ray_pedal_gas_last = 0.0 + else: + next_gas = rate_limit(target, self._ray_pedal_gas_last, -RAY_PEDAL_RATE_DOWN, RAY_PEDAL_RATE_UP) + self._ray_pedal_gas_last = min(next_gas, target) else: self._ray_pedal_gas_last = 0.0 can_sends.append(create_gas_interceptor_command( 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 a4ea0da8ed..af9148cdc7 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py @@ -145,6 +145,7 @@ def test_ray_controller_heartbeats_and_only_actuates_when_ready(speed): ) hud = SimpleNamespace( visualAlert=CarControl.HUDControl.VisualAlert.none, + setSpeed=20.0, leftLaneVisible=True, rightLaneVisible=True, leftLaneDepart=False, rightLaneDepart=False, ) @@ -206,11 +207,35 @@ def test_ray_controller_heartbeats_and_only_actuates_when_ready(speed): CS.out.vEgo = 12.0 for frame in range(80, 80 + 4 * 40, 4): pedal_msg(1.5, frame) - assert 0.69 <= controller._ray_pedal_gas_last <= 0.70 + assert controller._ray_pedal_gas_last == pytest.approx(0.55) for frame in range(240, 240 + 4 * 12, 4): dat = pedal_msg(-1.5, frame) assert dat[:4] == bytes(4) + for frame in range(288, 288 + 4 * 40, 4): + pedal_msg(1.5, frame) + assert controller._ray_pedal_gas_last == pytest.approx(0.55) + hud.setSpeed = 12.0 + pedal_msg(1.5, 448) + assert controller._ray_pedal_gas_last == pytest.approx(0.55 * 0.65) + hud.setSpeed = 11.8 + dat = pedal_msg(1.5, 452) + assert controller._ray_pedal_gas_last == pytest.approx(0.55 * 0.65 * 0.6) + assert dat[4] & 0x80 + hud.setSpeed = 11.4 + assert pedal_msg(1.5, 456)[:4] == bytes(4) + hud.setSpeed = float('nan') + assert pedal_msg(1.5, 460)[:4] == bytes(4) + hud.setSpeed = 20.0 + assert pedal_msg(-0.3, 464)[:4] == bytes(4) + CS.out.vEgo = 15.0 + hud.setSpeed = 53.0 / 3.6 + pedal_msg(-0.16, 468) + assert controller._ray_pedal_gas_last < 0.1 + CS.out.vEgo = 12.0 + hud.setSpeed = 8.0 / 3.6 + assert pedal_msg(-0.3, 472)[: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): @@ -229,6 +254,7 @@ def test_ray_stock_cruise_cancellation_survives_accelerator_override(candidate): ) hud = SimpleNamespace( visualAlert=CarControl.HUDControl.VisualAlert.none, + setSpeed=20.0, leftLaneVisible=True, rightLaneVisible=True, leftLaneDepart=False, rightLaneDepart=False, ) actuators = SimpleNamespace(longControlState=CarControl.Actuators.LongControlState.off) diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index 2dd3352bd4..0cdcbe8ed8 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -46,6 +46,7 @@ class CarController(CarControllerBase): self.angle_override_confirm_frames = 0 self.angle_lkas_active = False self.angle_handoff_active = False + self.ascent_angle_initialized = False self.ascent_aol_arm_frames = 0 self.cruise_button_prev = 0 @@ -204,6 +205,10 @@ class CarController(CarControllerBase): return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus) if self.CP.carFingerprint in (CAR.SUBARU_ASCENT_2023, CAR.SUBARU_OUTBACK_2023): + if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023 and not self.ascent_angle_initialized: + self.apply_steer_last = CS.out.steeringAngleDeg + self.ascent_angle_initialized = True + mads_only = CC.latActive and not CC.enabled mads_only_ok = CS.out.vEgoRaw > _ANGLE_MADS_MIN_SPEED and \ abs(CS.out.steeringAngleDeg) < _ANGLE_MADS_MAX_STEER_ANGLE @@ -223,7 +228,7 @@ class CarController(CarControllerBase): 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: + if lkas_active and not self.angle_lkas_active and self.CP.carFingerprint != CAR.SUBARU_ASCENT_2023: self.apply_steer_last = CS.out.steeringAngleDeg apply_steer = apply_std_steer_angle_limits( diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index df8e66cebd..40560276e5 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py @@ -682,6 +682,28 @@ def test_ascent_angle_controller_reengages_immediately_after_manual_steering_sto assert CS.out.steeringAngleDeg < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < CC.actuators.steeringAngleDeg +def test_ascent_reentry_rate_uses_last_transmitted_angle(): + CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023) + controller = CarController({}, CP) + controller.angle_handoff_active = True + CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-1.45)) + CS = SimpleNamespace(out=SimpleNamespace( + vEgoRaw=30.3, steeringAngleDeg=0.78, steeringRateDeg=-1.5, steeringTorque=56.0, + gearShifter=structs.CarState.GearShifter.drive, standstill=False, + )) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main) + + parser.update([(1, [controller.lateral_angle(CC, CS)])]) + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0 + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(0.78) + + CS.out.steeringAngleDeg = 0.74 + CS.out.steeringRateDeg = -1.99 + parser.update([(2, [controller.lateral_angle(CC, CS)])]) + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1 + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(0.53, abs=0.01) + + def test_ascent_angle_controller_waits_for_parking_lot_safety_envelope(): CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023) controller = CarController({}, CP) diff --git a/opendbc_repo/opendbc/safety/modes/hyundai.h b/opendbc_repo/opendbc/safety/modes/hyundai.h index e0826819b6..df43a36542 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 > 530U || track2 < 497U || track2 > 1035U || + (enabled && (track1 < 264U || track1 > 473U || track2 < 497U || track2 > 919U || 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 1bd36754c7..98c0a71574 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,9 @@ 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.70) is has_ray_signature - assert not tx(0.71) + assert tx(0.55) is has_ray_signature + assert not tx(0.56) + assert not tx(0.70) 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 64c42db75b..d2ec1f2d1f 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -3341,8 +3341,10 @@ def get_genesis_g70_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK_WIDTH ) overshoot_weight = _sigmoid((overshoot - 0.08) / 0.10) + boundary_weight = get_genesis_g70_overshoot_blend(desired_lateral_accel, measured_lateral_accel) + boundary_weight *= min(abs(desired_lateral_jerk) / 0.15, 1.0) deadzone += (GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_MAX * curve_speed_weight * - curve_onset_weight * curve_cutoff_weight * jerk_weight * overshoot_weight) + curve_onset_weight * curve_cutoff_weight * jerk_weight * overshoot_weight * boundary_weight) return deadzone @@ -3398,6 +3400,13 @@ 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_overshoot_blend(setpoint: float, measured_lateral_accel: float) -> float: + if setpoint * measured_lateral_accel <= 0.0: + return 0.0 + overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0) + return float(np.interp(abs(setpoint), [0.10, 0.35], [0.0, 1.0]) * min(overshoot / 0.15, 1.0)) + + 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: @@ -3412,7 +3421,9 @@ def get_genesis_g70_unwind_ff_scale(setpoint: float, measured_lateral_accel: flo GENESIS_G70_UNWIND_FF_JERK_WIDTH) speed_weight = _sigmoid((v_ego - GENESIS_G70_UNWIND_FF_SPEED) / GENESIS_G70_UNWIND_FF_SPEED_WIDTH) - return 1.0 - GENESIS_G70_UNWIND_FF_REDUCTION_MAX * overshoot_weight * jerk_weight * speed_weight + boundary_weight = get_genesis_g70_overshoot_blend(setpoint, measured_lateral_accel) + boundary_weight *= min(abs(desired_lateral_jerk) / 0.15, 1.0) + return 1.0 - GENESIS_G70_UNWIND_FF_REDUCTION_MAX * overshoot_weight * jerk_weight * speed_weight * boundary_weight def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_accel: float, @@ -3427,9 +3438,11 @@ def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_acc GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH) jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK) / GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH) - phase_weight = 1.0 if setpoint * desired_lateral_jerk < 0.0 else 0.45 + unwind_jerk = -math.copysign(1.0, setpoint) * desired_lateral_jerk + phase_weight = float(np.interp(unwind_jerk, [0.0, 0.15], [0.45, 1.0])) reduction = (GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_MAX * speed_weight * error_weight * - (0.35 + (0.65 * jerk_weight)) * phase_weight) + (0.35 + (0.65 * jerk_weight)) * phase_weight * + get_genesis_g70_overshoot_blend(setpoint, measured_lateral_accel)) return 1.0 - reduction diff --git a/selfdrive/controls/tests/test_g70_transition_continuity.py b/selfdrive/controls/tests/test_g70_transition_continuity.py new file mode 100644 index 0000000000..9f3bdc57ad --- /dev/null +++ b/selfdrive/controls/tests/test_g70_transition_continuity.py @@ -0,0 +1,56 @@ +import pytest + +from openpilot.selfdrive.controls.lib import latcontrol_vehicle_tunes as tunes + + +@pytest.mark.parametrize('direction', [-1, 1]) +@pytest.mark.parametrize('boundary', ['overshoot', 'jerk', 'center']) +@pytest.mark.parametrize('helper', ['get_genesis_g70_unwind_ff_scale', 'get_genesis_g70_high_speed_error_scale']) +def test_no_step_at_phase_boundary(direction, boundary, helper): + values = [] + for epsilon in [-1e-7, 1e-7]: + desired, actual, jerk = 0.8, 1.0, -0.5 + if boundary == 'overshoot': + actual = desired + epsilon + elif boundary == 'jerk': + jerk = epsilon + else: + desired = epsilon + values.append(getattr(tunes, helper)(direction * desired, direction * actual, direction * jerk, 30)) + assert abs(values[1] - values[0]) < 1e-5 + + +@pytest.mark.parametrize('direction', [-1, 1]) +def test_unwind_deadzone_continuity(direction): + for boundary in ['overshoot', 'jerk', 'center']: + values = [] + for epsilon in [-1e-7, 1e-7]: + desired, actual, jerk = 0.8, 1.0, -0.5 + if boundary == 'overshoot': + actual = desired + epsilon + elif boundary == 'jerk': + jerk = epsilon + else: + desired = epsilon + values.append(tunes.get_genesis_g70_friction_jerk_deadzone( + 30, direction * desired, direction * jerk, direction * actual)) + assert abs(values[1] - values[0]) < 1e-5 + + +@pytest.mark.parametrize('helper', ['get_genesis_g70_unwind_ff_scale', 'get_genesis_g70_high_speed_error_scale']) +def test_scales_bounded_symmetric_and_inactive_without_overshoot(helper): + fn = getattr(tunes, helper) + for desired in [0, 0.05, 0.2, 0.8, 2.0]: + for actual in [-1, 0, 0.1, 1.0, 2.5]: + for jerk in [-1, -0.1, 0, 0.1, 1]: + scale = fn(desired, actual, jerk, 30) + assert 0.66 <= scale <= 1.0 + assert scale == pytest.approx(fn(-desired, -actual, -jerk, 30)) + if actual <= desired or desired <= 0.1: + assert scale == 1.0 + + +def test_full_overshoot_blend_preserves_large_error_protection(): + assert tunes.get_genesis_g70_overshoot_blend(0.8, 1.0) == 1.0 + assert tunes.get_genesis_g70_overshoot_blend(-0.8, -1.0) == 1.0 + assert tunes.get_genesis_g70_unwind_ff_scale(0.8, 1.0, -0.5, 30) < 1.0