mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-10 00:03:50 +08:00
Cabo
This commit is contained in:
firestar5683
@@ -11,7 +11,7 @@ from opendbc.car.interfaces import CarControllerBase
|
||||
from opendbc.car.toyota import toyotacan
|
||||
from opendbc.car.toyota.values import CAR, MIN_ACC_SPEED, NO_STOP_TIMER_CAR, PEDAL_TRANSITION, TSS2_CAR, \
|
||||
CarControllerParams, ToyotaFlags, ToyotaSafetyFlags, \
|
||||
UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, TOYOTA_AUTO_HOLD_AEB_CARS
|
||||
UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, uses_toyota_auto_hold_aeb
|
||||
from opendbc.can import CANPacker
|
||||
|
||||
Ecu = structs.CarParams.Ecu
|
||||
@@ -373,7 +373,7 @@ class CarController(CarControllerBase):
|
||||
return []
|
||||
|
||||
def reset_auto_hold_state(self):
|
||||
if self.brake_hold_active and self.CP.carFingerprint not in TOYOTA_AUTO_HOLD_AEB_CARS:
|
||||
if self.brake_hold_active and not uses_toyota_auto_hold_aeb(self.CP):
|
||||
self.standstill_req = False
|
||||
self._brake_hold_counter = 0
|
||||
self.brake_hold_active = False
|
||||
@@ -475,7 +475,7 @@ class CarController(CarControllerBase):
|
||||
|
||||
self._update_standstill_request(CC, CS, actuators, starpilot_toggles)
|
||||
if supports_toyota_auto_hold(self.CP, getattr(starpilot_toggles, "toyota_auto_hold", False)):
|
||||
if self.CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS:
|
||||
if uses_toyota_auto_hold_aeb(self.CP):
|
||||
can_sends.extend(self.create_auto_brake_hold_messages(CS))
|
||||
else:
|
||||
self.update_auto_hold_state(CS, pcm_cancel_cmd, long_active=CC.longActive, stopping=stopping)
|
||||
@@ -590,7 +590,7 @@ class CarController(CarControllerBase):
|
||||
|
||||
pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
|
||||
|
||||
if self.brake_hold_active and self.CP.carFingerprint not in TOYOTA_AUTO_HOLD_AEB_CARS:
|
||||
if self.brake_hold_active and not uses_toyota_auto_hold_aeb(self.CP):
|
||||
pcm_accel_cmd = TOYOTA_AUTO_HOLD_ACCEL
|
||||
self.permit_braking = True
|
||||
self.standstill_req = True
|
||||
|
||||
@@ -5,7 +5,7 @@ from opendbc.car.toyota.radar_interface import RadarInterface
|
||||
from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
|
||||
MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \
|
||||
ToyotaSafetyFlags, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, \
|
||||
TOYOTA_AUTO_HOLD_AEB_CARS
|
||||
uses_toyota_auto_hold_aeb
|
||||
from opendbc.car.disable_ecu import disable_ecu
|
||||
from opendbc.car.interfaces import CarInterfaceBase
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
@@ -167,7 +167,7 @@ class CarInterface(CarInterfaceBase):
|
||||
toyota_auto_hold = Params(return_defaults=True).get_bool("ToyotaAutoHold")
|
||||
if toyota_auto_hold and ret.openpilotLongitudinalControl and candidate in TOYOTA_AUTO_HOLD_CARS:
|
||||
ret.alternativeExperience |= (ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
if candidate in TOYOTA_AUTO_HOLD_AEB_CARS
|
||||
if uses_toyota_auto_hold_aeb(ret)
|
||||
else ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD)
|
||||
ret.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
|
||||
|
||||
@@ -25,7 +25,7 @@ from opendbc.car.toyota.radar_interface import RadarInterface, TSSP_RADAR_EGO_SP
|
||||
from opendbc.car.toyota.values import CAR, DBC, MIN_ACC_SPEED, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \
|
||||
FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \
|
||||
ToyotaFlags, ToyotaSafetyFlags, ToyotaStarPilotFlags, TOYOTA_AUTO_HOLD_CARS, \
|
||||
TOYOTA_AUTO_HOLD_AEB_CARS, \
|
||||
uses_toyota_auto_hold_aeb, \
|
||||
get_platform_codes
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
from openpilot.common.params import Params
|
||||
@@ -193,8 +193,15 @@ class TestToyotaInterfaces:
|
||||
if car_model in TSS2_CAR and car_model not in SECOC_CAR:
|
||||
assert dbc[Bus.pt] == "toyota_nodsu_pt_generated"
|
||||
|
||||
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H])
|
||||
def test_auto_hold_sets_flag_on_supported_toyota(self, candidate):
|
||||
@pytest.mark.parametrize("candidate,hybrid", [
|
||||
(CAR.TOYOTA_CAMRY_TSS2, False),
|
||||
(CAR.TOYOTA_RAV4, False),
|
||||
(CAR.TOYOTA_RAV4H, True),
|
||||
(CAR.TOYOTA_RAV4_TSS2, False),
|
||||
(CAR.TOYOTA_RAV4_TSS2, True),
|
||||
(CAR.TOYOTA_COROLLA_TSS2, True),
|
||||
])
|
||||
def test_auto_hold_sets_flag_on_supported_toyota(self, candidate, hybrid):
|
||||
params = Params()
|
||||
try:
|
||||
params.put_bool("ToyotaAutoHold", True)
|
||||
@@ -202,7 +209,7 @@ class TestToyotaInterfaces:
|
||||
candidate,
|
||||
{bus: ({0x2FF: 8} if candidate in (CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H) and bus == 0 else {})
|
||||
for bus in range(8)},
|
||||
[],
|
||||
[CarParams.CarFw(ecu=Ecu.hybrid, address=0x7D2, fwVersion=b"test")] if hybrid else [],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
@@ -212,7 +219,9 @@ class TestToyotaInterfaces:
|
||||
params.remove("ToyotaAutoHold")
|
||||
|
||||
assert car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
if candidate in TOYOTA_AUTO_HOLD_AEB_CARS:
|
||||
legacy_hold = candidate == CAR.TOYOTA_CAMRY_TSS2 or (candidate == CAR.TOYOTA_RAV4_TSS2 and hybrid)
|
||||
assert uses_toyota_auto_hold_aeb(car_params) == legacy_hold
|
||||
if legacy_hold:
|
||||
assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
|
||||
else:
|
||||
@@ -222,16 +231,24 @@ class TestToyotaInterfaces:
|
||||
can_parsers = CarState.get_can_parsers(car_params)
|
||||
car_state = CarState(car_params, SimpleNamespace(flags=0))
|
||||
car_state.update(can_parsers, SimpleNamespace(cluster_offset=1.0))
|
||||
assert (0x344 in can_parsers[Bus.cam].vl) == (candidate in TOYOTA_AUTO_HOLD_AEB_CARS)
|
||||
assert (0x344 in can_parsers[Bus.cam].vl) == legacy_hold
|
||||
assert car_state.auto_brake_hold == legacy_hold
|
||||
|
||||
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H])
|
||||
def test_auto_hold_is_disabled_by_default(self, candidate):
|
||||
@pytest.mark.parametrize("candidate,hybrid", [
|
||||
(CAR.TOYOTA_CAMRY_TSS2, False),
|
||||
(CAR.TOYOTA_RAV4, False),
|
||||
(CAR.TOYOTA_RAV4H, True),
|
||||
(CAR.TOYOTA_RAV4_TSS2, False),
|
||||
(CAR.TOYOTA_RAV4_TSS2, True),
|
||||
(CAR.TOYOTA_RAV4_TSS2_2022, True),
|
||||
])
|
||||
def test_auto_hold_is_disabled_by_default(self, candidate, hybrid):
|
||||
params = Params()
|
||||
params.remove("ToyotaAutoHold")
|
||||
car_params = CarInterface.get_params(
|
||||
candidate,
|
||||
{bus: {} for bus in range(8)},
|
||||
[],
|
||||
[CarParams.CarFw(ecu=Ecu.hybrid, address=0x7D2, fwVersion=b"test")] if hybrid else [],
|
||||
alpha_long=False,
|
||||
is_release=False,
|
||||
docs=False,
|
||||
@@ -240,6 +257,8 @@ class TestToyotaInterfaces:
|
||||
|
||||
assert not car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD
|
||||
assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
assert 0x344 not in CarState.get_can_parsers(car_params)[Bus.cam].vl
|
||||
|
||||
def test_prius_openpilot_long_uses_hybrid_long_defaults(self):
|
||||
car_params = CarInterface.get_params(
|
||||
@@ -1239,11 +1258,13 @@ class TestToyotaCarController:
|
||||
|
||||
class TestToyotaAutoHoldCruise:
|
||||
@staticmethod
|
||||
def _make_car(*, candidate=CAR.TOYOTA_RAV4_TSS2, enabled=True, capability=True):
|
||||
def _make_car(*, candidate=CAR.TOYOTA_RAV4_TSS2, hybrid=False, enabled=True, capability=True):
|
||||
cp = CarInterface.get_non_essential_params(candidate)
|
||||
cp.flags &= ~ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
cp.flags &= ~(ToyotaFlags.AUTO_BRAKE_HOLD.value | ToyotaFlags.HYBRID.value)
|
||||
if capability:
|
||||
cp.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value
|
||||
if hybrid:
|
||||
cp.flags |= ToyotaFlags.HYBRID.value
|
||||
controller = CarController(DBC[candidate], cp)
|
||||
cc = structs.CarControl(enabled=True, longActive=True)
|
||||
cc.actuators.accel = -0.7
|
||||
@@ -1263,16 +1284,18 @@ class TestToyotaAutoHoldCruise:
|
||||
pre_collision_2={},
|
||||
)
|
||||
toggles = SimpleNamespace(toyota_auto_hold=enabled, sng_hack=False, lock_doors=False, unlock_doors=False)
|
||||
parser = CANParser(DBC[candidate][Bus.pt], [("ACC_CONTROL", 0)], 0)
|
||||
parser = CANParser(DBC[candidate][Bus.pt], [("ACC_CONTROL", 0), ("PRE_COLLISION_2", 0)], 0)
|
||||
return SimpleNamespace(controller=controller, cc=cc, cs=cs, toggles=toggles, parser=parser)
|
||||
|
||||
@staticmethod
|
||||
def _tick(car):
|
||||
# Run through a complete ACC_CONTROL send interval and decode the actual
|
||||
# controller output, including the ordinary standstill and PID paths.
|
||||
car.can_sends = []
|
||||
for _ in range(3):
|
||||
now_nanos = car.controller.frame * 10_000_000
|
||||
_, messages = car.controller.update(car.cc.as_reader(), car.cs, now_nanos, car.toggles)
|
||||
car.can_sends.extend(messages)
|
||||
car.parser.update([(now_nanos, messages)])
|
||||
return car.parser.vl["ACC_CONTROL"]
|
||||
|
||||
@@ -1412,6 +1435,92 @@ class TestToyotaAutoHoldCruise:
|
||||
assert not car.controller.brake_hold_active
|
||||
|
||||
|
||||
class TestToyotaAutoHoldAeb:
|
||||
_make_car = staticmethod(TestToyotaAutoHoldCruise._make_car)
|
||||
_tick = staticmethod(TestToyotaAutoHoldCruise._tick)
|
||||
|
||||
@pytest.mark.parametrize("candidate", list(CAR))
|
||||
@pytest.mark.parametrize("hybrid", [False, True])
|
||||
def test_legacy_path_is_scoped_to_camry_and_early_rav4_hybrid(self, candidate, hybrid):
|
||||
cp = SimpleNamespace(carFingerprint=candidate, flags=ToyotaFlags.HYBRID.value if hybrid else 0)
|
||||
expected = candidate == CAR.TOYOTA_CAMRY_TSS2 or (candidate == CAR.TOYOTA_RAV4_TSS2 and hybrid)
|
||||
assert uses_toyota_auto_hold_aeb(cp) == expected
|
||||
|
||||
@pytest.mark.parametrize("candidate,hybrid", [(CAR.TOYOTA_CAMRY_TSS2, False), (CAR.TOYOTA_RAV4_TSS2, True)])
|
||||
def test_manual_stop_uses_legacy_hold_and_releases_on_gas(self, candidate, hybrid):
|
||||
car = self._make_car(candidate=candidate, hybrid=hybrid)
|
||||
car.cc.enabled = False
|
||||
car.cc.longActive = False
|
||||
car.cc.latActive = True
|
||||
car.cc.actuators.accel = 0.0
|
||||
car.cs.out.cruiseState.enabled = False
|
||||
car.cs.out.cruiseState.standstill = False
|
||||
car.cs.out.brakePressed = True
|
||||
car.cs.pre_collision_2 = {"DSS1GDRV": -0.1, "PBRTRGR": 0}
|
||||
|
||||
# Pass through the camera message until the pedal has been held at a stop.
|
||||
command = self._tick(car)
|
||||
assert not car.controller.brake_hold_active
|
||||
assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -0.1
|
||||
for _ in range(34):
|
||||
command = self._tick(car)
|
||||
assert car.controller.brake_hold_active
|
||||
assert any(msg[0] == 0x344 for msg in car.can_sends)
|
||||
assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0
|
||||
assert car.parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 1
|
||||
# Do not simultaneously inject an ACC_CONTROL hold on legacy-path cars.
|
||||
assert command["ACCEL_CMD"] == 0.0
|
||||
assert command["RELEASE_STANDSTILL"] == 1
|
||||
|
||||
car.cs.out.brakePressed = False
|
||||
for _ in range(50):
|
||||
self._tick(car)
|
||||
assert car.controller.brake_hold_active
|
||||
assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0
|
||||
|
||||
car.cs.out.gasPressed = True
|
||||
self._tick(car)
|
||||
assert not car.controller.brake_hold_active
|
||||
assert car.parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -0.1
|
||||
assert car.parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 0
|
||||
|
||||
@pytest.mark.parametrize("enabled,capability", [(False, True), (True, False)])
|
||||
def test_rav4_hybrid_does_not_inject_legacy_messages_without_opt_in(self, enabled, capability):
|
||||
car = self._make_car(hybrid=True, enabled=enabled, capability=capability)
|
||||
car.cc.longActive = False
|
||||
car.cc.actuators.accel = 0.0
|
||||
car.cs.out.cruiseState.enabled = False
|
||||
car.cs.out.brakePressed = True
|
||||
for _ in range(50):
|
||||
self._tick(car)
|
||||
assert not car.controller.brake_hold_active
|
||||
assert all(msg[0] != 0x344 for msg in car.can_sends)
|
||||
|
||||
@pytest.mark.parametrize("release", ["toggle", "main", "park", "reverse", "moving", "cruise"])
|
||||
def test_rav4_hybrid_legacy_hold_releases_when_conditions_change(self, release):
|
||||
car = self._make_car(hybrid=True)
|
||||
car.cc.longActive = False
|
||||
car.cc.actuators.accel = 0.0
|
||||
car.cs.out.cruiseState.enabled = False
|
||||
car.cs.out.brakePressed = True
|
||||
for _ in range(35):
|
||||
self._tick(car)
|
||||
assert car.controller.brake_hold_active
|
||||
|
||||
if release == "toggle":
|
||||
car.toggles.toyota_auto_hold = False
|
||||
elif release == "main":
|
||||
car.cs.out.cruiseState.available = False
|
||||
elif release in ("park", "reverse"):
|
||||
car.cs.out.gearShifter = getattr(structs.CarState.GearShifter, release)
|
||||
elif release == "moving":
|
||||
car.cs.out.standstill = False
|
||||
else:
|
||||
car.cs.out.cruiseState.enabled = True
|
||||
self._tick(car)
|
||||
assert not car.controller.brake_hold_active
|
||||
|
||||
|
||||
class TestToyotaCarState:
|
||||
@pytest.mark.parametrize("candidate", [CAR.TOYOTA_PRIUS, CAR.TOYOTA_PRIUS_RETROFIT])
|
||||
def test_legacy_prius_distance_button_generates_events(self, candidate):
|
||||
|
||||
@@ -629,10 +629,16 @@ TOYOTA_AUTO_HOLD_CARS = (TSS2_CAR - RADAR_ACC_CAR - SECOC_CAR) | {
|
||||
CAR.TOYOTA_RAV4H,
|
||||
}
|
||||
|
||||
# The Camry uses the legacy camera AEB replacement for Auto Hold. Other
|
||||
# supported Toyota models use the ACC_CONTROL hold request.
|
||||
# The Camry uses the legacy camera AEB replacement for Auto Hold. The
|
||||
# 2019-2021 RAV4 uses it only with the detected hybrid powertrain.
|
||||
TOYOTA_AUTO_HOLD_AEB_CARS = {CAR.TOYOTA_CAMRY_TSS2}
|
||||
|
||||
|
||||
def uses_toyota_auto_hold_aeb(CP: CarParams) -> bool:
|
||||
return (CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS or
|
||||
(CP.carFingerprint == CAR.TOYOTA_RAV4_TSS2 and bool(CP.flags & ToyotaFlags.HYBRID.value)))
|
||||
|
||||
|
||||
# no resume button press required
|
||||
NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER)
|
||||
|
||||
|
||||
@@ -13,6 +13,7 @@ from openpilot.common.pid import PIDController
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import * # noqa: F403
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import get_genesis_g70_center_measurement_damping_gain
|
||||
|
||||
# At higher speeds (25+mph) we can assume:
|
||||
# Lateral acceleration achieved by a specific car correlates to
|
||||
@@ -561,6 +562,11 @@ class LatControlTorque(LatControl):
|
||||
ff, self.gv70_previous_feedforward, setpoint, desired_lateral_jerk, CS.vEgo, self.dt,
|
||||
)
|
||||
self.gv70_previous_feedforward = ff
|
||||
if self.is_genesis_g70:
|
||||
damping_gain = 0.0 if CS.steeringPressed else get_genesis_g70_center_measurement_damping_gain(
|
||||
CS.vEgo, setpoint, measurement, desired_lateral_jerk,
|
||||
)
|
||||
self.pid._k_d = [[0.0], [damping_gain]]
|
||||
freeze_integrator = (steer_limited_by_safety or CS.steeringPressed or
|
||||
CS.vEgo < self.low_speed_reset_threshold or unwind_detected)
|
||||
error_rate = 0.0 if self.is_genesis_gv70 and CS.steeringPressed else -measurement_rate
|
||||
|
||||
@@ -366,6 +366,10 @@ 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_CENTER_MEASUREMENT_DAMPING_MAX = 0.06
|
||||
GENESIS_G70_CENTER_MEASUREMENT_DAMPING_SPEED_BP = [50.0 * CV.MPH_TO_MS, 60.0 * CV.MPH_TO_MS]
|
||||
GENESIS_G70_CENTER_MEASUREMENT_DAMPING_LAT_BP = [0.15, 0.35]
|
||||
GENESIS_G70_CENTER_MEASUREMENT_DAMPING_JERK_BP = [0.20, 0.50]
|
||||
GENESIS_G70_HIGHWAY_STABILIZER_CENTER_LAT_BP = [1.4, 1.8]
|
||||
GENESIS_G70_HIGHWAY_STABILIZER_CURVE_EXIT_LAT = 0.15
|
||||
GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN = 0.45
|
||||
@@ -3574,6 +3578,15 @@ def get_genesis_g70_highway_turn_in_output_scale(output_torque: float, setpoint:
|
||||
return 1.0 - GENESIS_G70_HIGHWAY_TURN_IN_OUTPUT_REDUCTION * speed_weight * curve_weight * jerk_weight * tracking_weight
|
||||
|
||||
|
||||
def get_genesis_g70_center_measurement_damping_gain(v_ego: float, desired_lateral_accel: float,
|
||||
measured_lateral_accel: float, desired_lateral_jerk: float) -> float:
|
||||
speed_weight = np.interp(v_ego, GENESIS_G70_CENTER_MEASUREMENT_DAMPING_SPEED_BP, [0.0, 1.0])
|
||||
center_weight = np.interp(max(abs(desired_lateral_accel), abs(measured_lateral_accel)),
|
||||
GENESIS_G70_CENTER_MEASUREMENT_DAMPING_LAT_BP, [1.0, 0.0])
|
||||
jerk_weight = np.interp(abs(desired_lateral_jerk), GENESIS_G70_CENTER_MEASUREMENT_DAMPING_JERK_BP, [1.0, 0.0])
|
||||
return float(GENESIS_G70_CENTER_MEASUREMENT_DAMPING_MAX * speed_weight * center_weight * jerk_weight)
|
||||
|
||||
|
||||
def get_genesis_g70_stabilized_output(output_torque: float, prev_output_torque: float,
|
||||
desired_lateral_accel: float, measured_lateral_accel: float,
|
||||
desired_lateral_jerk: float, v_ego: float, dt: float) -> float:
|
||||
|
||||
@@ -61,6 +61,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
|
||||
get_genesis_gv70_low_speed_center_overshoot_scale,
|
||||
get_genesis_gv70_stabilized_output,
|
||||
get_genesis_g70_stabilized_output,
|
||||
get_genesis_g70_center_measurement_damping_gain,
|
||||
normalize_flm_overrides,
|
||||
set_flm_runtime_overrides,
|
||||
)
|
||||
@@ -1041,6 +1042,73 @@ class TestLatControl:
|
||||
assert lac_log.active
|
||||
assert output == pytest.approx(-0.123)
|
||||
|
||||
@pytest.mark.parametrize("mph,desired,measured,jerk,expected", [
|
||||
(65.0, 0.0, 0.10, 0.0, 0.06),
|
||||
(50.0, 0.0, 0.10, 0.0, 0.0),
|
||||
(55.0, 0.0, 0.10, 0.0, 0.03),
|
||||
(65.0, 0.25, 0.10, 0.0, 0.03),
|
||||
(65.0, 0.0, 0.25, 0.0, 0.03),
|
||||
(65.0, 0.35, 0.10, 0.0, 0.0),
|
||||
(65.0, 0.0, 0.35, 0.0, 0.0),
|
||||
(65.0, 0.0, 0.10, 0.35, 0.03),
|
||||
(65.0, 0.0, 0.10, 0.50, 0.0),
|
||||
])
|
||||
def test_genesis_g70_center_measurement_damping_gates(self, mph, desired, measured, jerk, expected):
|
||||
for direction in [-1.0, 1.0]:
|
||||
gain = get_genesis_g70_center_measurement_damping_gain(
|
||||
mph * 0.44704, desired * direction, measured * direction, jerk * direction,
|
||||
)
|
||||
assert gain == pytest.approx(expected)
|
||||
|
||||
@pytest.mark.parametrize("direction", [-1.0, 1.0])
|
||||
def test_genesis_g70_center_measurement_damping_update_path(self, direction):
|
||||
controller, VM, CS, params, toggles = self._build_torque_controller(HYUNDAI.GENESIS_G70_2020)
|
||||
CS.vEgo = 65.0 * 0.44704
|
||||
CS.steeringAngleDeg = 0.0
|
||||
controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
|
||||
CS.steeringAngleDeg = -direction * 0.5
|
||||
output, _, moving_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
|
||||
assert moving_log.d * direction < 0.0
|
||||
assert abs(moving_log.d) <= 0.15
|
||||
assert abs(output) <= controller.steer_max
|
||||
|
||||
for _ in range(150):
|
||||
_, _, steady_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
|
||||
assert steady_log.d == pytest.approx(0.0, abs=1e-6)
|
||||
|
||||
CS.steeringPressed = True
|
||||
CS.steeringAngleDeg = 0.0
|
||||
_, _, driver_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
|
||||
assert driver_log.d == 0.0
|
||||
|
||||
CS.steeringPressed = False
|
||||
controller.update(False, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
|
||||
_, _, resumed_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
|
||||
assert resumed_log.d == 0.0
|
||||
|
||||
CS.vEgo = 40.0 * 0.44704
|
||||
CS.steeringAngleDeg = -direction * 0.5
|
||||
_, _, low_speed_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
|
||||
assert low_speed_log.d == 0.0
|
||||
|
||||
CS.vEgo = 65.0 * 0.44704
|
||||
curvature = direction * 0.8 / CS.vEgo ** 2
|
||||
controller.curvature_request_buffer = deque([curvature] * controller.request_buffer_len,
|
||||
maxlen=controller.request_buffer_len)
|
||||
_, _, curve_log = controller.update(True, CS, VM, params, False, curvature, False, 0.2, None, None, toggles)
|
||||
assert curve_log.d == 0.0
|
||||
|
||||
@pytest.mark.parametrize("car_name", [HYUNDAI.GENESIS_GV70_1ST_GEN, HYUNDAI.KIA_EV6, HYUNDAI.HYUNDAI_IONIQ_6])
|
||||
def test_genesis_g70_center_measurement_damping_does_not_change_other_cars(self, monkeypatch, car_name):
|
||||
def unexpected_damping(*_args):
|
||||
pytest.fail("G70 center damping reached another vehicle")
|
||||
|
||||
monkeypatch.setattr(latcontrol_torque, "get_genesis_g70_center_measurement_damping_gain", unexpected_damping)
|
||||
controller, VM, CS, params, toggles = self._build_torque_controller(car_name)
|
||||
CS.vEgo = 65.0 * 0.44704
|
||||
controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles)
|
||||
assert controller.pid.d == 0.0
|
||||
|
||||
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)
|
||||
|
||||
@@ -264,7 +264,17 @@ class FordLateralController:
|
||||
[MACH_E_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE, MACH_E_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE],
|
||||
[0.0, 1.0],
|
||||
))
|
||||
return base + (MACH_E_CURVATURE_ERROR_MAX - base) * speed_weight * max(deficit_weight, reversal_weight)
|
||||
unwind_weight = 0.0
|
||||
if (self.model is not None and len(self.model.orientationRate.z) >= 17 and
|
||||
requested * current >= 0.0 and desired * current >= 0.0 and abs(desired) < abs(current)):
|
||||
unwind_deficit = np.sign(current) * (current - requested)
|
||||
preview_deficit = np.sign(current) * (current - predicted)
|
||||
unwind_weight = float(np.interp(
|
||||
min(unwind_deficit, preview_deficit),
|
||||
[MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT, MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT],
|
||||
[0.0, 1.0],
|
||||
))
|
||||
return base + (MACH_E_CURVATURE_ERROR_MAX - base) * speed_weight * max(deficit_weight, reversal_weight, unwind_weight)
|
||||
|
||||
def _path_angle_assist(self, requested: float, desired: float, applied: float, current: float, v_ego: float,
|
||||
steering_pressed: bool, lane_change: bool) -> float:
|
||||
|
||||
@@ -130,6 +130,75 @@ def test_understeer_error_preserves_other_fords(controller):
|
||||
assert controller._curvature_error_limit(0.012, 0.012, 0.004, 12.0, False, False) == 0.002
|
||||
|
||||
|
||||
@pytest.mark.parametrize("sign", (-1, 1))
|
||||
@pytest.mark.parametrize("speed,expected", ((8.0, 0.002), (8.5, 0.004), (9.0, 0.006), (12.0, 0.006),
|
||||
(14.0, 0.006), (15.0, 0.004), (16.0, 0.002)))
|
||||
def test_mach_e_unwind_error_tracks_opening_path_before_direction_changes(controller, sign, speed, expected):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33))
|
||||
assert controller._curvature_error_limit(
|
||||
sign * 0.001, sign * 0.002, sign * 0.005, speed, False, False, sign * 0.001) == pytest.approx(expected)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("sign", (-1, 1))
|
||||
@pytest.mark.parametrize("requested,preview,expected", ((0.003, 0.001, 0.002), (0.002, 0.001, 0.004),
|
||||
(0.001, 0.001, 0.006), (0.001, 0.004, 0.002),
|
||||
(0.001, 0.003, 0.002), (0.001, 0.002, 0.004),
|
||||
(0.001, -0.0002, 0.006), (0.001, 0.0, 0.006),
|
||||
(0.0, 0.0, 0.006)))
|
||||
def test_mach_e_unwind_error_requires_measured_lag_and_opening_preview(controller, sign, requested, preview, expected):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33))
|
||||
assert controller._curvature_error_limit(
|
||||
sign * requested, sign * 0.002, sign * 0.005, 12.0, False, False, sign * preview) == pytest.approx(expected)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("fingerprint,flags,driver,lane_change", (
|
||||
(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, True, False),
|
||||
(CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, False, True),
|
||||
(CAR.FORD_MUSTANG_MACH_E_MK1, 0, False, False),
|
||||
(CAR.FORD_EDGE_MK2, FordFlags.CANFD, False, False),
|
||||
(CAR.FORD_EXPLORER_MK6, FordFlags.CANFD, False, False),
|
||||
(CAR.FORD_F_150_MK14, FordFlags.CANFD, False, False),
|
||||
))
|
||||
def test_unwind_error_preserves_takeover_lane_changes_and_other_fords(controller, fingerprint, flags, driver, lane_change):
|
||||
controller.CP.carFingerprint = fingerprint
|
||||
controller.CP.flags = flags
|
||||
controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33))
|
||||
assert controller._curvature_error_limit(0.001, 0.002, 0.005, 12.0, driver, lane_change, 0.001) == 0.002
|
||||
|
||||
|
||||
@pytest.mark.parametrize("model", (None, SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 16))))
|
||||
def test_mach_e_unwind_error_requires_model_preview(controller, model):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
controller.model = model
|
||||
assert controller._curvature_error_limit(0.001, 0.002, 0.005, 12.0, False, False, 0.001) == 0.002
|
||||
|
||||
|
||||
@pytest.mark.parametrize("sign", (-1, 1))
|
||||
def test_mach_e_curve_exit_releases_without_waiting_for_left_right_reversal(controller, monkeypatch, sign):
|
||||
controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
|
||||
controller.CP.flags = FordFlags.CANFD
|
||||
controller.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.0] * 33),
|
||||
meta=SimpleNamespace(laneChangeState=0, laneChangeDirection=0))
|
||||
controller.curvature_last = sign * 0.004
|
||||
controller.desired_curvature_last = sign * 0.003
|
||||
monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.001)
|
||||
commands = []
|
||||
for _ in range(4):
|
||||
result = controller.update(SimpleNamespace(latActive=True), car_state(speed=12.0, curvature=sign * 0.005),
|
||||
SimpleNamespace(curvature=sign * 0.002))
|
||||
assert result.active
|
||||
assert result.path_angle == 0.0
|
||||
commands.append(sign * result.curvature)
|
||||
assert commands[0] == pytest.approx(0.004 - 0.0018)
|
||||
assert commands[-1] == pytest.approx(0.0016)
|
||||
assert commands[-1] < 0.005 - 0.002
|
||||
|
||||
|
||||
@pytest.mark.parametrize("sign", (-1, 1))
|
||||
@pytest.mark.parametrize("speed,expected", ((8.0, 0.002), (8.5, 0.004), (9.0, 0.006), (9.5, 0.006), (10.0, 0.006),
|
||||
(12.0, 0.006), (15.0, 0.004), (16.0, 0.002)))
|
||||
|
||||
Reference in New Issue
Block a user