mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-30 19:33:45 +08:00
leaky faucet
This commit is contained in:
firestar5683
@@ -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(
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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(
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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) ||
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user