mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-30 11:23:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Long day

Browse Source
This commit is contained in:
firestar5683
2026-09-24 17:12:32 -05:00
parent aaf1061111
commit 0c6ee69362
12 changed files with 125 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+10 -10
View File
@@ -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,
)
opendbc_repo/opendbc/car/hyundai/interface.py
+2 -2
View File
@@ -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
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+2 -2
View File
@@ -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()
opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py
+11 -5
View File
@@ -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
opendbc_repo/opendbc/safety/modes/hyundai.h
+1 -1
View File
@@ -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) ||
opendbc_repo/opendbc/safety/tests/test_hyundai_ray_pedal.py
+2 -2
View File
@@ -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:
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+10 -1
View File
@@ -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):
selfdrive/controls/lib/longitudinal_vehicle_tunes.py
+4 -1
View File
@@ -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"
selfdrive/controls/tests/test_g70_friction_threshold.py
+26
View File
@@ -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
selfdrive/controls/tests/test_starpilot_planner.py
+1
View File
@@ -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
starpilot/car/ford/lateral.py
+19 -4
View File
@@ -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
starpilot/car/ford/tests/test_lateral.py
+37 -3
View File
@@ -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