mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-18 13:33:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

dopa

Browse Source
This commit is contained in:
firestar5683
2026-09-16 12:19:59 -05:00
parent 7316871e62
commit 14370fe9cf
15 changed files with 206 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+8 -2
View File
@@ -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,
opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
+2 -2
View File
@@ -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
opendbc_repo/opendbc/car/subaru/carcontroller.py
+18 -28
View File
@@ -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,
opendbc_repo/opendbc/car/subaru/tests/test_subaru.py
+13 -10
View File
@@ -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():
opendbc_repo/opendbc/car/tesla/interface.py
+1
View File
@@ -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:
opendbc_repo/opendbc/car/tesla/tests/test_hw1.py
+2
View File
@@ -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
opendbc_repo/opendbc/safety/modes/hyundai.h
+6
View File
@@ -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,
};
opendbc_repo/opendbc/safety/tests/test_hyundai.py
+2
View File
@@ -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):
selfdrive/controls/lib/latcontrol_torque.py
+1 -5
View File
@@ -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)
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+17 -89
View File
@@ -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))
selfdrive/controls/lib/longitudinal_vehicle_tunes.py
+24 -2
View File
@@ -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 (
selfdrive/controls/tests/test_latcontrol.py
+31 -44
View File
@@ -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]
selfdrive/controls/tests/test_starpilot_planner.py
+27
View File
@@ -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(
starpilot/controls/starpilot_card.py
+7 -3
View File
@@ -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)
starpilot/controls/tests/test_starpilot_card.py
+47 -13
View File
@@ -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