mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-20 15:54:13 +08:00
Ich bin müde.
This commit is contained in:
firestar5683
@@ -24,7 +24,7 @@ def ecu_log(msg):
|
||||
|
||||
|
||||
def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_req=b'\x28\x83\x01', timeout=0.1, retry=10, reset=False,
|
||||
require_response=False, diag_request=EXT_DIAG_REQUEST, diag_response=EXT_DIAG_RESPONSE):
|
||||
require_response=False, diag_request=EXT_DIAG_REQUEST, diag_response=EXT_DIAG_RESPONSE, response_offset=0x8):
|
||||
"""Silence an ECU by disabling sending and receiving messages using UDS 0x28.
|
||||
The ECU will stay silent as long as openpilot keeps sending Tester Present.
|
||||
Set require_response for takeovers that must fail closed unless the ECU confirms communication control.
|
||||
@@ -36,7 +36,8 @@ def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_r
|
||||
if reset:
|
||||
try:
|
||||
ecu_log("sending ECU reset before communication control...")
|
||||
reset_query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [RESET_REQUEST], [RESET_RESPONSE])
|
||||
reset_query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [RESET_REQUEST], [RESET_RESPONSE],
|
||||
response_offset=response_offset)
|
||||
reset_query.get_data(timeout=timeout)
|
||||
time.sleep(0.2)
|
||||
except Exception as e:
|
||||
@@ -47,7 +48,8 @@ def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_r
|
||||
try:
|
||||
# Enter extended diagnostic session
|
||||
ecu_log(f"attempt {i+1}/{retry}: diag session...")
|
||||
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [diag_request], [diag_response])
|
||||
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [diag_request], [diag_response],
|
||||
response_offset=response_offset)
|
||||
|
||||
for _, _ in query.get_data(timeout).items():
|
||||
ecu_log("diag session OK")
|
||||
@@ -57,7 +59,8 @@ def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_r
|
||||
|
||||
# Send CC command and log the response
|
||||
ecu_log("sending CC...")
|
||||
cc_query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [com_cont_req], [b''])
|
||||
cc_query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [com_cont_req], [b''],
|
||||
response_offset=response_offset)
|
||||
cc_response = cc_query.get_data(timeout)
|
||||
|
||||
# Log what we got back
|
||||
|
||||
@@ -55,7 +55,7 @@ class CarController(CarControllerBase):
|
||||
CC.longActive and brake_pressure > 0, brake_mode))
|
||||
|
||||
if self.frame % 100 == 0:
|
||||
can_sends.append(make_tester_present_msg(0x707, 1, suppress_response=True))
|
||||
can_sends.append(make_tester_present_msg(0x707, 0, suppress_response=True))
|
||||
|
||||
### STEER ###
|
||||
steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
|
||||
|
||||
@@ -4,12 +4,12 @@ from opendbc.car.interfaces import CarInterfaceBase
|
||||
from opendbc.car.nissan.carcontroller import CarController
|
||||
from opendbc.car.nissan.carstate import CarState
|
||||
from opendbc.car.nissan.values import CAR, CarControllerParams, NissanSafetyFlags, \
|
||||
NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_DIAGNOSTIC_RESPONSE_KWP
|
||||
NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_RX_OFFSET
|
||||
|
||||
|
||||
LEAF_LONGITUDINAL_CARS = (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC)
|
||||
LEAF_ADAS_ECU_ADDR = 0x707
|
||||
LEAF_ADAS_ECU_BUS = 1
|
||||
LEAF_ADAS_ECU_BUS = 0
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@@ -64,13 +64,14 @@ class CarInterface(CarInterfaceBase):
|
||||
uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX,
|
||||
uds.MESSAGE_TYPE.NORMAL])
|
||||
ecu_disabled = disable_ecu(can_recv, can_send, bus=LEAF_ADAS_ECU_BUS, addr=LEAF_ADAS_ECU_ADDR,
|
||||
com_cont_req=communication_control, require_response=True)
|
||||
com_cont_req=communication_control, require_response=True, response_offset=NISSAN_RX_OFFSET)
|
||||
if not ecu_disabled:
|
||||
# Nissan firmware queries use the KWP-style default session. Try it after
|
||||
# standard UDS extended-session control, but still require a positive 0x68 response.
|
||||
ecu_disabled = disable_ecu(can_recv, can_send, bus=LEAF_ADAS_ECU_BUS, addr=LEAF_ADAS_ECU_ADDR,
|
||||
com_cont_req=communication_control, require_response=True,
|
||||
diag_request=NISSAN_DIAGNOSTIC_REQUEST_KWP, diag_response=NISSAN_DIAGNOSTIC_RESPONSE_KWP)
|
||||
diag_request=NISSAN_DIAGNOSTIC_REQUEST_KWP, diag_response=NISSAN_DIAGNOSTIC_RESPONSE_KWP,
|
||||
response_offset=NISSAN_RX_OFFSET)
|
||||
params.put_bool("EcuDisableFailed", not ecu_disabled)
|
||||
if ecu_disabled:
|
||||
ecu_log("Nissan Leaf ADAS TX disabled; experimental longitudinal control enabled")
|
||||
@@ -89,4 +90,4 @@ class CarInterface(CarInterfaceBase):
|
||||
0x80 | uds.CONTROL_TYPE.ENABLE_RX_ENABLE_TX,
|
||||
uds.MESSAGE_TYPE.NORMAL])
|
||||
disable_ecu(can_recv, can_send, bus=LEAF_ADAS_ECU_BUS, addr=LEAF_ADAS_ECU_ADDR,
|
||||
com_cont_req=communication_control)
|
||||
com_cont_req=communication_control, response_offset=NISSAN_RX_OFFSET)
|
||||
|
||||
@@ -65,7 +65,8 @@ def test_alpha_long_controller_sends_stock_shaped_commands_and_keepalive():
|
||||
assert can_sends[0x2B0][1].hex() == "ff6090ac5b000e03"
|
||||
assert can_sends[0x1C3][1].hex() == "000000006400ff27"
|
||||
assert can_sends[0x707][1].hex() == "023e800000000000"
|
||||
assert all(can_sends[addr][2] == 1 for addr in (0x2B0, 0x1C3, 0x707))
|
||||
assert all(can_sends[addr][2] == 1 for addr in (0x2B0, 0x1C3))
|
||||
assert can_sends[0x707][2] == 0
|
||||
|
||||
|
||||
def test_alpha_long_controller_clamps_to_panda_accel_limit():
|
||||
@@ -123,7 +124,8 @@ def test_leaf_ecu_disable_is_strict_and_falls_back(monkeypatch, ecu_disabled):
|
||||
|
||||
assert len(calls) == (1 if ecu_disabled else 2)
|
||||
assert calls[0]["addr"] == 0x707
|
||||
assert calls[0]["bus"] == 1
|
||||
assert calls[0]["bus"] == 0
|
||||
assert calls[0]["response_offset"] == 0x20
|
||||
assert calls[0]["require_response"] is True
|
||||
assert calls[0]["com_cont_req"] == bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL,
|
||||
uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX,
|
||||
|
||||
@@ -16,6 +16,12 @@ _SNG_ACC_MIN_DIST = 3
|
||||
_SNG_ACC_MAX_DIST = 4.5
|
||||
_LEGACY_2025_MADS_MIN_SPEED = 0.44704
|
||||
_LEGACY_2025_MADS_MAX_STEER_ANGLE = 120.0
|
||||
_LEGACY_2025_OVERRIDE_HOLD_FRAMES = 10
|
||||
_LEGACY_2025_REENGAGE_SETTLE_FRAMES = 8
|
||||
_LEGACY_2025_REENGAGE_MAX_STEER_RATE = 2.0
|
||||
_LEGACY_2025_REENGAGE_MAX_ANGLE_DELTA = 1.0
|
||||
_LEGACY_2025_RECLAIM_FRAMES = 36
|
||||
_LEGACY_2025_RECLAIM_EXPONENT = 2.5
|
||||
_ANGLE_REENGAGE_MAX_STEER_RATE = 3.0
|
||||
_ANGLE_REENGAGE_SETTLE_FRAMES = 2
|
||||
|
||||
@@ -33,6 +39,12 @@ class CarController(CarControllerBase):
|
||||
self.driver_override = False
|
||||
self.angle_reengage_settle_frames = 0
|
||||
self.legacy_2025_lkas_active = False
|
||||
self.legacy_2025_handoff_active = False
|
||||
self.legacy_2025_override_hold_frames = 0
|
||||
self.legacy_2025_reengage_settle_frames = 0
|
||||
self.legacy_2025_reengage_reference_angle = 0.0
|
||||
self.legacy_2025_reclaim_frames = 0
|
||||
self.legacy_2025_reclaim_start_angle = 0.0
|
||||
|
||||
self.cruise_button_prev = 0
|
||||
self.steer_rate_counter = 0
|
||||
@@ -50,6 +62,69 @@ class CarController(CarControllerBase):
|
||||
self.epb_resume_frames_remaining = -1
|
||||
self.last_standstill_frame = 0
|
||||
|
||||
def _reset_legacy_2025_handoff(self):
|
||||
self.legacy_2025_handoff_active = False
|
||||
self.legacy_2025_override_hold_frames = 0
|
||||
self.legacy_2025_reengage_settle_frames = 0
|
||||
self.legacy_2025_reengage_reference_angle = 0.0
|
||||
self.legacy_2025_reclaim_frames = 0
|
||||
self.legacy_2025_reclaim_start_angle = 0.0
|
||||
|
||||
def _legacy_2025_manual_handoff(self, CS, lkas_available):
|
||||
if not lkas_available:
|
||||
self._reset_legacy_2025_handoff()
|
||||
return False
|
||||
|
||||
if CS.out.steeringPressed:
|
||||
self.legacy_2025_handoff_active = True
|
||||
self.legacy_2025_override_hold_frames = _LEGACY_2025_OVERRIDE_HOLD_FRAMES
|
||||
self.legacy_2025_reengage_settle_frames = 0
|
||||
self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
self.legacy_2025_reclaim_frames = 0
|
||||
return True
|
||||
|
||||
if not self.legacy_2025_handoff_active and not self.legacy_2025_lkas_active and \
|
||||
abs(CS.out.steeringRateDeg) > _LEGACY_2025_REENGAGE_MAX_STEER_RATE:
|
||||
self.legacy_2025_handoff_active = True
|
||||
self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
|
||||
if not self.legacy_2025_handoff_active:
|
||||
return False
|
||||
|
||||
if self.legacy_2025_override_hold_frames > 0:
|
||||
self.legacy_2025_override_hold_frames -= 1
|
||||
if self.legacy_2025_override_hold_frames == 0:
|
||||
self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
return True
|
||||
|
||||
wheel_stable = abs(CS.out.steeringRateDeg) <= _LEGACY_2025_REENGAGE_MAX_STEER_RATE and \
|
||||
abs(CS.out.steeringAngleDeg - self.legacy_2025_reengage_reference_angle) <= _LEGACY_2025_REENGAGE_MAX_ANGLE_DELTA
|
||||
if wheel_stable:
|
||||
self.legacy_2025_reengage_settle_frames += 1
|
||||
else:
|
||||
self.legacy_2025_reengage_settle_frames = 0
|
||||
self.legacy_2025_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
|
||||
if self.legacy_2025_reengage_settle_frames < _LEGACY_2025_REENGAGE_SETTLE_FRAMES:
|
||||
return True
|
||||
|
||||
self.legacy_2025_handoff_active = False
|
||||
self.legacy_2025_reengage_settle_frames = 0
|
||||
self.legacy_2025_reclaim_frames = _LEGACY_2025_RECLAIM_FRAMES
|
||||
self.legacy_2025_reclaim_start_angle = CS.out.steeringAngleDeg
|
||||
return True
|
||||
|
||||
def _legacy_2025_reclaim_target(self, target_angle):
|
||||
if self.legacy_2025_reclaim_frames <= 0:
|
||||
return target_angle
|
||||
|
||||
progress = (_LEGACY_2025_RECLAIM_FRAMES - self.legacy_2025_reclaim_frames + 1) / _LEGACY_2025_RECLAIM_FRAMES
|
||||
eased_progress = progress ** _LEGACY_2025_RECLAIM_EXPONENT
|
||||
target_angle = self.legacy_2025_reclaim_start_angle + eased_progress * \
|
||||
(target_angle - self.legacy_2025_reclaim_start_angle)
|
||||
self.legacy_2025_reclaim_frames -= 1
|
||||
return target_angle
|
||||
|
||||
def lateral_angle(self, CC, CS):
|
||||
if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
|
||||
mads_only = CC.latActive and not CC.enabled
|
||||
@@ -58,16 +133,15 @@ class CarController(CarControllerBase):
|
||||
lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
|
||||
CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill
|
||||
|
||||
manual_handoff = CS.out.steeringPressed or (
|
||||
not self.legacy_2025_lkas_active and abs(CS.out.steeringRateDeg) > _ANGLE_REENGAGE_MAX_STEER_RATE
|
||||
)
|
||||
manual_handoff = self._legacy_2025_manual_handoff(CS, lkas_available)
|
||||
lkas_active = lkas_available and not manual_handoff
|
||||
|
||||
if lkas_active and not self.legacy_2025_lkas_active:
|
||||
self.apply_steer_last = CS.out.steeringAngleDeg
|
||||
|
||||
steer_target = self._legacy_2025_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg
|
||||
apply_steer = apply_std_steer_angle_limits(
|
||||
CC.actuators.steeringAngleDeg,
|
||||
steer_target,
|
||||
self.apply_steer_last,
|
||||
CS.out.vEgoRaw,
|
||||
CS.out.steeringAngleDeg,
|
||||
|
||||
@@ -258,11 +258,76 @@ def test_legacy_2025_waits_for_manual_steering_to_settle_before_reengaging():
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
||||
|
||||
CS.out.steeringRateDeg = 2.0
|
||||
for i in range(9):
|
||||
CS.out.steeringAngleDeg += 0.5
|
||||
CS.out.steeringRateDeg = 20.0
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(3 + i, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
||||
|
||||
for i in range(6):
|
||||
if i % 2:
|
||||
CS.out.steeringAngleDeg += 0.5
|
||||
CS.out.steeringRateDeg = 0.0 if i % 2 == 0 else 20.0
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(12 + i, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
||||
|
||||
CS.out.steeringRateDeg = 0.0
|
||||
for i in range(8):
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(18 + i, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
|
||||
measured_angle = CS.out.steeringAngleDeg
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(3, [msg])])
|
||||
parser.update([(26, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||
assert abs(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] - CS.out.steeringAngleDeg) < 1.0
|
||||
assert abs(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] - measured_angle) < 0.1
|
||||
|
||||
|
||||
def test_legacy_2025_manual_handoff_reclaim_is_gradual():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(
|
||||
enabled=False,
|
||||
latActive=True,
|
||||
actuators=SimpleNamespace(steeringAngleDeg=-20.0),
|
||||
)
|
||||
CS = SimpleNamespace(out=SimpleNamespace(
|
||||
vEgoRaw=3.7,
|
||||
steeringAngleDeg=2.5,
|
||||
steeringRateDeg=-45.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])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
|
||||
CS.out.steeringPressed = False
|
||||
CS.out.steeringRateDeg = 0.0
|
||||
for i in range(19):
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(2 + i, [msg])])
|
||||
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||
first_reclaim_angle = parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"]
|
||||
assert first_reclaim_angle == pytest.approx(CS.out.steeringAngleDeg, abs=0.1)
|
||||
|
||||
reclaim_angles = []
|
||||
for i in range(6):
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(20 + i, [msg])])
|
||||
reclaim_angles.append(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"])
|
||||
|
||||
assert all(reclaim_angles[i] >= reclaim_angles[i + 1] for i in range(len(reclaim_angles) - 1))
|
||||
assert reclaim_angles[-1] > CC.actuators.steeringAngleDeg
|
||||
|
||||
|
||||
def test_ascent_2023_uses_gen2_angle_bus_layout():
|
||||
|
||||
@@ -173,7 +173,7 @@ static bool nissan_tx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
}
|
||||
|
||||
if (nissan_longitudinal && (msg->addr == 0x707U) && (msg->bus == 1U)) {
|
||||
if (nissan_longitudinal && (msg->addr == 0x707U) && (msg->bus == 0U)) {
|
||||
violation |= (msg->data[0] != 0x02U) || (msg->data[1] != 0x3EU) || (msg->data[2] != 0x80U);
|
||||
for (int i = 3; i < 8; i++) {
|
||||
violation |= msg->data[i] != 0U;
|
||||
@@ -207,7 +207,7 @@ static safety_config nissan_init(uint16_t param) {
|
||||
{0x280, 2, 8, .check_relay = true}, // CANCEL_MSG (Leaf)
|
||||
{0x2b0, 1, 8, .check_relay = true}, // Leaf propulsion/regen request
|
||||
{0x1c3, 1, 8, .check_relay = true}, // Leaf friction-brake request
|
||||
{0x707, 1, 8, .check_relay = false}, // Leaf ADAS ECU tester present
|
||||
{0x707, 0, 8, .check_relay = false}, // Leaf ADAS ECU tester present
|
||||
};
|
||||
|
||||
// Signals duplicated below due to the fact that these messages can come in on either CAN bus, depending on car model.
|
||||
|
||||
@@ -145,7 +145,7 @@ class TestNissanLeafSafety(TestNissanSafety):
|
||||
|
||||
class TestNissanLeafLongSafety(TestNissanLeafSafety):
|
||||
|
||||
TX_MSGS = [*TestNissanLeafSafety.TX_MSGS, [0x2B0, 1], [0x1C3, 1], [0x707, 1]]
|
||||
TX_MSGS = [*TestNissanLeafSafety.TX_MSGS, [0x2B0, 1], [0x1C3, 1], [0x707, 0]]
|
||||
RELAY_MALFUNCTION_ADDRS = {0: (0x169, 0x2B1, 0x4CC), 1: (0x2B0, 0x1C3), 2: (0x280,)}
|
||||
FWD_BLACKLISTED_ADDRS = {0: [0x280], 2: [0x169, 0x2B1, 0x4CC]}
|
||||
|
||||
@@ -261,13 +261,13 @@ class TestNissanLeafLongSafety(TestNissanLeafSafety):
|
||||
self.assertTrue(self._tx(self._brake_msg(0, active=False, brake_mode=False)))
|
||||
|
||||
def test_tester_present(self):
|
||||
tester_present = make_tester_present_msg(0x707, 1, suppress_response=True)
|
||||
tester_present = make_tester_present_msg(0x707, 0, suppress_response=True)
|
||||
self.assertTrue(self._tx(self._make_msg(tester_present)))
|
||||
|
||||
for index in range(8):
|
||||
dat = bytearray(tester_present.dat)
|
||||
dat[index] ^= 0x1
|
||||
self.assertFalse(self._tx(common.make_msg(1, 0x707, 8, dat)), index)
|
||||
self.assertFalse(self._tx(common.make_msg(0, 0x707, 8, dat)), index)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
@@ -5,7 +5,12 @@ from opendbc.car.honda.carcontroller import get_civic_bosch_modified_steering_pr
|
||||
from opendbc.car.honda.values import CAR as HONDA, HondaFlags
|
||||
from openpilot.starpilot.common.testing_grounds import testing_ground
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import SUBARU_IMPREZA_CARS, get_subaru_impreza_pid_output_scale
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
|
||||
RAV4_TSS2_CARS,
|
||||
SUBARU_IMPREZA_CARS,
|
||||
get_rav4_tss2_pid_output,
|
||||
get_subaru_impreza_pid_output_scale,
|
||||
)
|
||||
from openpilot.common.pid import PIDController
|
||||
|
||||
HONDA_PID_GAIN_SCALE_MIN = 0.1
|
||||
@@ -99,6 +104,7 @@ class LatControlPID(LatControl):
|
||||
self.honda_lateral_pid_ki_scale = 1.0
|
||||
self.is_civic_bosch_modified = CP.carFingerprint == HONDA.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED)
|
||||
self.is_subaru_impreza = CP.carFingerprint in SUBARU_IMPREZA_CARS
|
||||
self.is_rav4_tss2 = CP.carFingerprint in RAV4_TSS2_CARS
|
||||
self.prev_angle_steers_des_no_offset = 0.0
|
||||
self.modified_civic_steering_pressed_filter_s = 0.0
|
||||
self.modified_civic_steering_pressed_prev = False
|
||||
@@ -165,6 +171,11 @@ class LatControlPID(LatControl):
|
||||
output_torque = raw_output_torque * get_subaru_impreza_pid_output_scale(error)
|
||||
output_torque = float(max(min(output_torque, self.steer_max), -self.steer_max))
|
||||
|
||||
if self.is_rav4_tss2:
|
||||
output_torque = get_rav4_tss2_pid_output(output_torque, self.prev_output_torque,
|
||||
angle_steers_des_no_offset, CS.vEgo)
|
||||
output_torque = float(max(min(output_torque, self.steer_max), -self.steer_max))
|
||||
|
||||
if self.is_civic_bosch_modified and civic_bosch_modified_lateral_testing_ground_active():
|
||||
desired_angle_delta = angle_steers_des_no_offset - self.prev_angle_steers_des_no_offset
|
||||
output_torque *= get_civic_bosch_modified_pid_output_scale(angle_steers_des_no_offset, desired_angle_delta, CS.vEgo)
|
||||
|
||||
@@ -469,6 +469,10 @@ class LatControlTorque(LatControl):
|
||||
ff *= get_genesis_gv70_unwind_ff_scale(
|
||||
setpoint, measurement, desired_lateral_jerk, CS.vEgo,
|
||||
)
|
||||
if self.is_genesis_g70:
|
||||
ff *= get_genesis_g70_unwind_ff_scale(
|
||||
setpoint, measurement, desired_lateral_jerk, CS.vEgo,
|
||||
)
|
||||
if ioniq_6_active:
|
||||
vehicle_friction_jerk_deadzone = (
|
||||
IONIQ_6_2025_FRICTION_JERK_DEADZONE if self.is_ioniq_6_2025 else IONIQ_6_FRICTION_JERK_DEADZONE
|
||||
@@ -535,6 +539,13 @@ class LatControlTorque(LatControl):
|
||||
-low_speed_output_limit,
|
||||
low_speed_output_limit,
|
||||
))
|
||||
elif ioniq_5_active:
|
||||
low_speed_output_limit = get_ioniq_5_low_speed_output_limit(setpoint, desired_lateral_jerk, CS.vEgo)
|
||||
output_torque = float(np.clip(
|
||||
output_torque,
|
||||
-low_speed_output_limit,
|
||||
low_speed_output_limit,
|
||||
))
|
||||
elif self.is_ram_1500 and output_torque * setpoint > 0.0:
|
||||
output_torque *= get_ram_1500_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
|
||||
elif self.is_kona_non_scc:
|
||||
|
||||
@@ -255,13 +255,20 @@ 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_BOOST = 0.14
|
||||
GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST = 0.06
|
||||
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.20
|
||||
GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.12
|
||||
GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.12
|
||||
GENESIS_G70_UNWIND_FF_JERK = 0.10
|
||||
GENESIS_G70_UNWIND_FF_JERK_WIDTH = 0.10
|
||||
GENESIS_G70_UNWIND_FF_SPEED = 18.0
|
||||
GENESIS_G70_UNWIND_FF_SPEED_WIDTH = 3.0
|
||||
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_MAX = 0.15
|
||||
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED = 50.0 * CV.MPH_TO_MS
|
||||
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH = 8.0 * CV.MPH_TO_MS
|
||||
@@ -669,6 +676,14 @@ IONIQ_5_SUSTAINED_TURN_IN_FF_SPEED_WIDTH = 1.8
|
||||
IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_START = 1.10
|
||||
IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_END = 3.60
|
||||
IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_WIDTH = 0.30
|
||||
IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_BASE = 0.05
|
||||
IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_TURN_RELIEF = 0.95
|
||||
IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED = 8.0
|
||||
IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 0.8
|
||||
IONIQ_5_LOW_SPEED_CENTER_LAT = 0.40
|
||||
IONIQ_5_LOW_SPEED_CENTER_LAT_WIDTH = 0.10
|
||||
IONIQ_5_LOW_SPEED_CENTER_JERK = 0.40
|
||||
IONIQ_5_LOW_SPEED_CENTER_JERK_WIDTH = 0.12
|
||||
|
||||
IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT = 1.16
|
||||
IONIQ_EV_OLD_FF_REDUCTION_LEFT = 0.16
|
||||
@@ -1055,6 +1070,16 @@ SUBARU_IMPREZA_PID_TAPER_START_DEG = 0.75
|
||||
SUBARU_IMPREZA_PID_TAPER_FULL_DEG = 4.0
|
||||
SUBARU_IMPREZA_PID_TAPER_MIN = 0.58
|
||||
|
||||
RAV4_TSS2_CARS = (
|
||||
TOYOTA_CAR.TOYOTA_RAV4_TSS2,
|
||||
)
|
||||
RAV4_TSS2_PID_LOW_SPEED = 12.0 * CV.MPH_TO_MS
|
||||
RAV4_TSS2_PID_LOW_SPEED_WIDTH = 2.0 * CV.MPH_TO_MS
|
||||
RAV4_TSS2_PID_CENTER_ANGLE = 14.0
|
||||
RAV4_TSS2_PID_CENTER_ANGLE_WIDTH = 3.0
|
||||
RAV4_TSS2_PID_OUTPUT_SCALE_MIN = 0.62
|
||||
RAV4_TSS2_PID_OUTPUT_ALPHA_MIN = 0.28
|
||||
|
||||
RAM_1500_TRANSITION_TAPER_MAX = 0.34
|
||||
RAM_1500_TRANSITION_SPEED_ONSET = 10.0
|
||||
RAM_1500_TRANSITION_SPEED_FULL = 15.0
|
||||
@@ -1539,6 +1564,21 @@ def get_subaru_impreza_pid_output_scale(angle_error_deg: float) -> float:
|
||||
return 1.0 - ((1.0 - SUBARU_IMPREZA_PID_TAPER_MIN) * error_weight)
|
||||
|
||||
|
||||
def get_rav4_tss2_pid_output(output_torque: float, prev_output_torque: float,
|
||||
desired_angle_deg: float, v_ego: float) -> float:
|
||||
"""Damp low-speed RAV4 center reversals without blunting real turns."""
|
||||
speed_weight = _sigmoid((RAV4_TSS2_PID_LOW_SPEED - max(v_ego, 0.0)) /
|
||||
RAV4_TSS2_PID_LOW_SPEED_WIDTH)
|
||||
center_weight = _sigmoid((RAV4_TSS2_PID_CENTER_ANGLE - abs(desired_angle_deg)) /
|
||||
RAV4_TSS2_PID_CENTER_ANGLE_WIDTH)
|
||||
envelope = speed_weight * center_weight
|
||||
|
||||
output_scale = 1.0 - ((1.0 - RAV4_TSS2_PID_OUTPUT_SCALE_MIN) * envelope)
|
||||
output_alpha = 1.0 - ((1.0 - RAV4_TSS2_PID_OUTPUT_ALPHA_MIN) * envelope)
|
||||
limited_output = output_torque * output_scale
|
||||
return float(prev_output_torque + output_alpha * (limited_output - prev_output_torque))
|
||||
|
||||
|
||||
def get_ram_1500_transition_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
|
||||
speed_weight = float(np.interp(v_ego, [RAM_1500_TRANSITION_SPEED_ONSET, RAM_1500_TRANSITION_SPEED_FULL], [0.0, 1.0]))
|
||||
jerk_weight = float(np.interp(abs(desired_lateral_jerk),
|
||||
@@ -2752,6 +2792,23 @@ def get_genesis_g70_curve_unwind_output_scale(desired_lateral_accel: float, desi
|
||||
return 1.0 + GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST * speed_weight * lateral_weight * jerk_weight
|
||||
|
||||
|
||||
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:
|
||||
return 1.0
|
||||
|
||||
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
|
||||
if overshoot <= 0.0:
|
||||
return 1.0
|
||||
overshoot_weight = _sigmoid((overshoot - GENESIS_G70_UNWIND_FF_OVERSHOOT) /
|
||||
GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH)
|
||||
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_G70_UNWIND_FF_JERK) /
|
||||
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
|
||||
|
||||
|
||||
def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_accel: float,
|
||||
desired_lateral_jerk: float, v_ego: float) -> float:
|
||||
tracking_error = abs(measured_lateral_accel - setpoint)
|
||||
@@ -2861,6 +2918,22 @@ def get_ioniq_5_center_taper_scale(desired_lateral_accel: float, v_ego: float) -
|
||||
return 1.0 - reduction
|
||||
|
||||
|
||||
def get_ioniq_5_low_speed_output_limit(desired_lateral_accel: float,
|
||||
desired_lateral_jerk: float, v_ego: float) -> float:
|
||||
"""Bound stop-transition center chatter without blunting actual turns."""
|
||||
speed_weight = _ioniq_5_sigmoid((IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED - max(v_ego, 0.0)) /
|
||||
IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH)
|
||||
center_weight = _ioniq_5_sigmoid((IONIQ_5_LOW_SPEED_CENTER_LAT - abs(desired_lateral_accel)) /
|
||||
IONIQ_5_LOW_SPEED_CENTER_LAT_WIDTH)
|
||||
calm_weight = _ioniq_5_sigmoid((IONIQ_5_LOW_SPEED_CENTER_JERK - abs(desired_lateral_jerk)) /
|
||||
IONIQ_5_LOW_SPEED_CENTER_JERK_WIDTH)
|
||||
center_weight *= calm_weight
|
||||
center_limit = (IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_BASE +
|
||||
IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_TURN_RELIEF * (1.0 - center_weight))
|
||||
limit = 1.0 - speed_weight * (1.0 - center_limit)
|
||||
return float(np.clip(limit, IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_BASE, 1.0))
|
||||
|
||||
|
||||
def _ioniq_ev_old_sigmoid(x: float) -> float:
|
||||
return _sigmoid(x)
|
||||
|
||||
|
||||
@@ -295,6 +295,9 @@ class LongControl:
|
||||
a_target = self.vehicle_tuning.shape_toyota_sienna_accel_target(
|
||||
a_target, CS.vEgo, should_stop, leads=leads,
|
||||
)
|
||||
a_target = self.vehicle_tuning.shape_hyundai_elantra_lead_target(
|
||||
a_target, CS.vEgo, should_stop, leads,
|
||||
)
|
||||
error = a_target - CS.aEgo
|
||||
self.update_mpc_mode(self.experimental_mode)
|
||||
self.vehicle_tuning.shape_volt_test_tune_integrator(self.pid, error, CS.vEgo)
|
||||
@@ -327,6 +330,9 @@ class LongControl:
|
||||
should_stop,
|
||||
has_lead,
|
||||
)
|
||||
raw_output_accel = self.vehicle_tuning.cap_hyundai_elantra_lead_output(
|
||||
raw_output_accel, CS.vEgo, should_stop, leads,
|
||||
)
|
||||
|
||||
if self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended':
|
||||
if raw_output_accel < 0 and raw_output_accel < self.last_output_accel:
|
||||
|
||||
@@ -48,6 +48,10 @@ VOLT_CRUISE_INTEGRATOR_LEAK = 0.995
|
||||
SUBARU_IMPREZA_STOP_RELEASE_TIME = 0.75
|
||||
SUBARU_IMPREZA_STOP_RELEASE_MAX_ACCEL = 0.8
|
||||
HYUNDAI_ELANTRA_STOPPING_HOLD_TARGET_GAP = 0.25
|
||||
HYUNDAI_ELANTRA_STOPPED_LEAD_MAX_EGO_SPEED = 2.0
|
||||
HYUNDAI_ELANTRA_STOPPED_LEAD_MAX_SPEED = 0.5
|
||||
HYUNDAI_ELANTRA_STOPPED_LEAD_MIN_CLOSING_SPEED = 0.25
|
||||
HYUNDAI_ELANTRA_STOPPED_LEAD_MAX_CREEP_ACCEL = 0.05
|
||||
|
||||
|
||||
def get_bolt_acc_pedal_friction_bias(output_accel, a_target, v_ego):
|
||||
@@ -157,6 +161,31 @@ class LongControlVehicleTuning:
|
||||
return output_accel
|
||||
return max(float(output_accel), float(stop_accel))
|
||||
|
||||
def is_hyundai_elantra_closing_on_stopped_lead(self, v_ego, should_stop, leads):
|
||||
if not self.is_hyundai_elantra_2021 or should_stop or v_ego > HYUNDAI_ELANTRA_STOPPED_LEAD_MAX_EGO_SPEED:
|
||||
return False
|
||||
|
||||
lead = leads[0] if leads else None
|
||||
if lead is None or not bool(getattr(lead, "status", False)):
|
||||
return False
|
||||
|
||||
lead_speed = max(0.0, float(getattr(lead, "vLead", 0.0)))
|
||||
closing_speed = float(v_ego) - lead_speed
|
||||
return (
|
||||
lead_speed <= HYUNDAI_ELANTRA_STOPPED_LEAD_MAX_SPEED and
|
||||
closing_speed >= HYUNDAI_ELANTRA_STOPPED_LEAD_MIN_CLOSING_SPEED
|
||||
)
|
||||
|
||||
def shape_hyundai_elantra_lead_target(self, a_target, v_ego, should_stop, leads):
|
||||
if self.is_hyundai_elantra_closing_on_stopped_lead(v_ego, should_stop, leads):
|
||||
return min(float(a_target), HYUNDAI_ELANTRA_STOPPED_LEAD_MAX_CREEP_ACCEL)
|
||||
return a_target
|
||||
|
||||
def cap_hyundai_elantra_lead_output(self, output_accel, v_ego, should_stop, leads):
|
||||
if self.is_hyundai_elantra_closing_on_stopped_lead(v_ego, should_stop, leads):
|
||||
return min(float(output_accel), HYUNDAI_ELANTRA_STOPPED_LEAD_MAX_CREEP_ACCEL)
|
||||
return output_accel
|
||||
|
||||
def cap_subaru_stop_release_accel(self, output_accel, stopping_handoff, should_stop):
|
||||
"""Prevent an Impreza stop-sign handoff from stepping straight into full throttle."""
|
||||
if not self.is_subaru_impreza_2020:
|
||||
|
||||
@@ -22,7 +22,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
|
||||
get_far_follow_output_slew_rates,
|
||||
get_follow_prebrake_min_headway,
|
||||
is_gm_silverado_early_follow_lead,
|
||||
is_toyota_rav4_tss2_2023,
|
||||
is_toyota_rav4_tss2_post_departure_tune,
|
||||
get_toyota_sienna_post_departure_restop_cap,
|
||||
get_untracked_slow_lead_decel_scale,
|
||||
)
|
||||
@@ -2663,7 +2663,7 @@ class LongitudinalPlanner:
|
||||
# cruise branch request full acceleration before the next lead was stable.
|
||||
# Keep the normal catch-up cap on this car; urgent braking remains outside
|
||||
# this comfort policy and is still allowed through unchanged.
|
||||
post_departure_bypass = post_departure_active and not is_toyota_rav4_tss2_2023(self.CP)
|
||||
post_departure_bypass = post_departure_active and not is_toyota_rav4_tss2_post_departure_tune(self.CP)
|
||||
follow_result = apply_follow_policy(
|
||||
self.lead_one,
|
||||
self.lead_two,
|
||||
|
||||
@@ -20,11 +20,11 @@ TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MIN_BRAKE = 0.18
|
||||
TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_BRAKE = 0.32
|
||||
|
||||
|
||||
def is_toyota_rav4_tss2_2023(CP):
|
||||
"""Identify the RAV4 TSS2 controller that needs normal catch-up caps after departure."""
|
||||
def is_toyota_rav4_tss2_post_departure_tune(CP):
|
||||
"""Identify RAV4 TSS2 variants that need normal catch-up caps after departure."""
|
||||
return (
|
||||
getattr(CP, "brand", "") == "toyota" and
|
||||
str(getattr(CP, "carFingerprint", "")) == "TOYOTA_RAV4_TSS2_2023"
|
||||
str(getattr(CP, "carFingerprint", "")) in ("TOYOTA_RAV4_TSS2", "TOYOTA_RAV4_TSS2_2023")
|
||||
)
|
||||
|
||||
|
||||
|
||||
@@ -41,6 +41,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
|
||||
get_gmc_yukon_cc_ff_scale,
|
||||
get_ram_1500_transition_output_scale,
|
||||
get_ram_1500_ff_scale,
|
||||
get_rav4_tss2_pid_output,
|
||||
get_subaru_impreza_pid_output_scale,
|
||||
normalize_flm_overrides,
|
||||
set_flm_runtime_overrides,
|
||||
@@ -78,6 +79,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
|
||||
get_genesis_g70_high_speed_error_scale,
|
||||
get_genesis_g70_low_speed_angle_damping,
|
||||
get_genesis_g70_low_speed_output_limit,
|
||||
get_genesis_g70_unwind_ff_scale,
|
||||
get_genesis_gv70_friction_threshold,
|
||||
get_genesis_gv70_high_speed_error_scale,
|
||||
get_genesis_gv70_unwind_ff_scale,
|
||||
@@ -107,6 +109,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
|
||||
get_ioniq_5_friction_scale,
|
||||
get_ioniq_5_friction_threshold,
|
||||
get_ioniq_5_center_taper_scale,
|
||||
get_ioniq_5_low_speed_output_limit,
|
||||
get_ioniq_ev_old_center_taper_scale,
|
||||
get_ioniq_ev_old_ff_scale,
|
||||
get_ioniq_6_center_taper_scale,
|
||||
@@ -822,6 +825,9 @@ class TestLatControl:
|
||||
assert get_genesis_g70_low_speed_angle_damping(0.0, 20.0, 0.0, 2.0) > 0.0
|
||||
assert 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_unwind_ff_scale(-0.7, -0.95, 0.5, 25.0) < 1.0
|
||||
assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, -0.5, 25.0) == 1.0
|
||||
assert get_genesis_g70_unwind_ff_scale(-0.7, 0.2, 0.5, 25.0) == 1.0
|
||||
|
||||
assert get_genesis_g70_high_speed_error_scale(0.2, 0.2, 0.8, 20.0) == 1.0
|
||||
assert get_genesis_g70_high_speed_error_scale(0.2, 0.9, 0.8, 20.0) < 1.0
|
||||
@@ -1112,6 +1118,15 @@ class TestLatControl:
|
||||
assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.0, 10.0)
|
||||
assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.20, 25.0) <= 1.0
|
||||
|
||||
def test_ioniq_5_low_speed_output_limit_preserves_turn_relief(self):
|
||||
center = get_ioniq_5_low_speed_output_limit(0.02, 0.05, 3.0)
|
||||
turn = get_ioniq_5_low_speed_output_limit(0.70, 0.70, 3.0)
|
||||
highway = get_ioniq_5_low_speed_output_limit(0.02, 0.05, 15.0)
|
||||
|
||||
assert center < 0.20
|
||||
assert turn > center
|
||||
assert highway > center
|
||||
|
||||
def test_ioniq_ev_old_ff_scale_curve(self):
|
||||
assert get_ioniq_ev_old_ff_scale(0.0, 0.0, 20.0) == 1.0
|
||||
assert get_ioniq_ev_old_ff_scale(0.35, 0.0, 20.0) > get_ioniq_ev_old_ff_scale(-0.35, 0.0, 20.0)
|
||||
@@ -1444,6 +1459,18 @@ class TestLatControl:
|
||||
assert lac_log.active
|
||||
assert controller.torque_params.latAccelFactor == pytest.approx(CP.lateralTuning.torque.latAccelFactor * 1.22)
|
||||
|
||||
def test_ioniq_5_low_speed_output_guard_update_path(self, monkeypatch):
|
||||
monkeypatch.setattr(latcontrol_torque, "get_ioniq_5_low_speed_output_limit", lambda *_args: 0.05)
|
||||
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_5)
|
||||
CS.vEgo = 3.2
|
||||
|
||||
output, _, lac_log = controller.update(
|
||||
True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles,
|
||||
)
|
||||
|
||||
assert lac_log.active
|
||||
assert abs(output) <= 0.05
|
||||
|
||||
def test_ioniq_6_default_update_path(self):
|
||||
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_6)
|
||||
|
||||
@@ -1612,6 +1639,31 @@ class TestLatControl:
|
||||
assert get_subaru_impreza_pid_output_scale(4.0) == pytest.approx(0.58)
|
||||
assert get_subaru_impreza_pid_output_scale(-4.0) == pytest.approx(0.58)
|
||||
|
||||
def test_rav4_tss2_pid_output_damps_low_speed_center_reversals(self):
|
||||
low_speed = get_rav4_tss2_pid_output(1.0, -1.0, 4.0, 6.0 * 0.44704)
|
||||
large_turn = get_rav4_tss2_pid_output(1.0, -1.0, 24.0, 6.0 * 0.44704)
|
||||
highway = get_rav4_tss2_pid_output(1.0, -1.0, 4.0, 25.0 * 0.44704)
|
||||
|
||||
assert abs(low_speed) < 0.50
|
||||
assert abs(large_turn) > abs(low_speed)
|
||||
assert highway > low_speed
|
||||
|
||||
def test_rav4_tss2_pid_output_update_path(self, monkeypatch):
|
||||
controller, VM, CS, params, starpilot_toggles = self._build_pid_controller(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
CS.vEgo = 6.0 * 0.44704
|
||||
CS.steeringAngleDeg = 8.0
|
||||
|
||||
tuned_output, _, lac_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, starpilot_toggles)
|
||||
|
||||
monkeypatch.setattr(latcontrol_pid, "get_rav4_tss2_pid_output", lambda output, *_args: output)
|
||||
base_controller, VM, CS, params, starpilot_toggles = self._build_pid_controller(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
CS.vEgo = 6.0 * 0.44704
|
||||
CS.steeringAngleDeg = 8.0
|
||||
base_output, _, _ = base_controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, starpilot_toggles)
|
||||
|
||||
assert lac_log.active
|
||||
assert abs(tuned_output) < abs(base_output)
|
||||
|
||||
def test_subaru_impreza_pid_output_taper_path(self, monkeypatch):
|
||||
controller, VM, CS, params, starpilot_toggles = self._build_pid_controller(SUBARU.SUBARU_IMPREZA)
|
||||
CS.steeringAngleDeg = 3.0
|
||||
|
||||
@@ -764,6 +764,31 @@ def test_elantra_lead_stop_releases_stale_hard_brake_after_target_eases():
|
||||
assert tuning.shape_stopping_accel(-1.20, -0.25, True, 1.0, False, -0.85) == pytest.approx(-1.20)
|
||||
|
||||
|
||||
def test_elantra_stopped_lead_handoff_holds_braking_direction_without_touching_brakes():
|
||||
CP = make_longcontrol_cp(brand="hyundai", carFingerprint="HYUNDAI_ELANTRA_2021")
|
||||
tuning = vehicle_tunes.LongControlVehicleTuning(CP)
|
||||
stopped_lead = SimpleNamespace(status=True, vLead=0.1, dRel=14.0)
|
||||
|
||||
assert tuning.shape_hyundai_elantra_lead_target(0.14, 1.1, False, (stopped_lead,)) == pytest.approx(0.05)
|
||||
assert tuning.cap_hyundai_elantra_lead_output(0.14, 1.1, False, (stopped_lead,)) == pytest.approx(0.05)
|
||||
assert tuning.cap_hyundai_elantra_lead_output(-0.5, 1.1, False, (stopped_lead,)) == pytest.approx(-0.5)
|
||||
|
||||
|
||||
def test_elantra_stopped_lead_handoff_releases_for_moving_lead_and_other_cars():
|
||||
elantra = vehicle_tunes.LongControlVehicleTuning(
|
||||
make_longcontrol_cp(brand="hyundai", carFingerprint="HYUNDAI_ELANTRA_2021")
|
||||
)
|
||||
other_car = vehicle_tunes.LongControlVehicleTuning(
|
||||
make_longcontrol_cp(brand="hyundai", carFingerprint="HYUNDAI_SONATA")
|
||||
)
|
||||
moving_lead = SimpleNamespace(status=True, vLead=0.8, dRel=14.0)
|
||||
stopped_lead = SimpleNamespace(status=True, vLead=0.1, dRel=14.0)
|
||||
|
||||
assert elantra.shape_hyundai_elantra_lead_target(0.14, 1.1, False, (moving_lead,)) == pytest.approx(0.14)
|
||||
assert elantra.shape_hyundai_elantra_lead_target(0.14, 1.1, True, (stopped_lead,)) == pytest.approx(0.14)
|
||||
assert other_car.shape_hyundai_elantra_lead_target(0.14, 1.1, False, (stopped_lead,)) == pytest.approx(0.14)
|
||||
|
||||
|
||||
def test_volt_testing_ground_handoff_freezes_integrator(monkeypatch):
|
||||
CP = car.CarParams.new_message()
|
||||
CP.brand = "gm"
|
||||
|
||||
@@ -22,7 +22,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
|
||||
get_follow_prebrake_min_headway,
|
||||
get_toyota_sienna_post_departure_restop_cap,
|
||||
is_gm_silverado_early_follow_lead,
|
||||
is_toyota_rav4_tss2_2023,
|
||||
is_toyota_rav4_tss2_post_departure_tune,
|
||||
)
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
|
||||
from openpilot.selfdrive.modeld import modeld
|
||||
@@ -2770,12 +2770,14 @@ def test_publish_has_lead_includes_second_mpc_lead():
|
||||
assert pm.sent["longitudinalPlan"].longitudinalPlan.hasLead
|
||||
|
||||
|
||||
def test_rav4_tss2_2023_is_the_car_specific_post_departure_tune():
|
||||
rav4_cp = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023)
|
||||
def test_rav4_tss2_variants_use_the_car_specific_post_departure_tune():
|
||||
rav4_2019_cp = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2)
|
||||
rav4_2023_cp = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023)
|
||||
other_cp = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2022)
|
||||
|
||||
assert is_toyota_rav4_tss2_2023(rav4_cp)
|
||||
assert not is_toyota_rav4_tss2_2023(other_cp)
|
||||
assert is_toyota_rav4_tss2_post_departure_tune(rav4_2019_cp)
|
||||
assert is_toyota_rav4_tss2_post_departure_tune(rav4_2023_cp)
|
||||
assert not is_toyota_rav4_tss2_post_departure_tune(other_cp)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
|
||||
|
||||
+25
-10
@@ -122,6 +122,12 @@ def _canonical_model_id(model_id: str) -> str:
|
||||
return MODEL_ID_ALIASES.get(key, key)
|
||||
|
||||
|
||||
def _select_builtin_model(params: Params) -> None:
|
||||
params.put("Model", BUILTIN_MODEL_KEY)
|
||||
params.put("DrivingModel", BUILTIN_MODEL_KEY)
|
||||
params.put("DrivingModelName", "Regret Driven Framework")
|
||||
|
||||
|
||||
def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
|
||||
lat_action_t: float, long_action_t: float, v_ego: float, mlsim: bool,
|
||||
is_v9: bool, is_v14: bool, is_v15: bool, starpilot_toggles,
|
||||
@@ -448,6 +454,24 @@ class ModelState:
|
||||
return parsed
|
||||
|
||||
|
||||
def _load_model_state(cam_w: int, cam_h: int, selected_model: str, external_gpu_requested: bool,
|
||||
params: Params) -> ModelState:
|
||||
try:
|
||||
return ModelState(cam_w, cam_h, external_gpu_requested)
|
||||
except Exception:
|
||||
if selected_model == BUILTIN_MODEL_KEY:
|
||||
raise
|
||||
|
||||
cloudlog.exception(f"Failed to load model {selected_model}; falling back to {BUILTIN_MODEL_KEY}")
|
||||
_select_builtin_model(params)
|
||||
if external_gpu_requested:
|
||||
from tinygrad.helpers import DEV
|
||||
device_config = tinygrad_dev_config(False, TICI)
|
||||
DEV.value = device_config
|
||||
os.environ["DEV"] = device_config
|
||||
return ModelState(cam_w, cam_h, False)
|
||||
|
||||
|
||||
def main(demo=False):
|
||||
cloudlog.warning("modeld init")
|
||||
|
||||
@@ -500,16 +524,7 @@ def main(demo=False):
|
||||
cloudlog.warning("loading model")
|
||||
if external_gpu_requested:
|
||||
wait_usbgpu_link()
|
||||
try:
|
||||
model = ModelState(vipc_client_main.width, vipc_client_main.height, external_gpu_requested)
|
||||
except Exception:
|
||||
if not external_gpu_requested:
|
||||
raise
|
||||
cloudlog.exception(f"Failed to load external-GPU model {selected_model}; falling back to {BUILTIN_MODEL_KEY}")
|
||||
device_config = tinygrad_dev_config(False, TICI)
|
||||
DEV.value = device_config
|
||||
os.environ["DEV"] = device_config
|
||||
model = ModelState(vipc_client_main.width, vipc_client_main.height, False)
|
||||
model = _load_model_state(vipc_client_main.width, vipc_client_main.height, selected_model, external_gpu_requested, params)
|
||||
external_gpu_active = model.uses_external_gpu
|
||||
params.put_bool("UsbGpuCompiled", external_model_selected and file_chunked_exists(external_artifact))
|
||||
params.put_bool("UsbGpuActive", external_gpu_active)
|
||||
|
||||
@@ -0,0 +1,41 @@
|
||||
import pytest
|
||||
|
||||
from openpilot.selfdrive.modeld import modeld
|
||||
|
||||
|
||||
class FakeParams:
|
||||
def __init__(self):
|
||||
self.values = {}
|
||||
|
||||
def put(self, key, value):
|
||||
self.values[key] = value
|
||||
|
||||
|
||||
def test_incompatible_downloaded_model_falls_back_to_builtin(monkeypatch):
|
||||
calls = []
|
||||
builtin_model = object()
|
||||
|
||||
def load_model(cam_w, cam_h, external_gpu_active):
|
||||
calls.append((cam_w, cam_h, external_gpu_active))
|
||||
if len(calls) == 1:
|
||||
raise TypeError("incompatible artifact")
|
||||
return builtin_model
|
||||
|
||||
params = FakeParams()
|
||||
monkeypatch.setattr(modeld, "ModelState", load_model)
|
||||
monkeypatch.setattr(modeld.cloudlog, "exception", lambda *_args, **_kwargs: None)
|
||||
|
||||
assert modeld._load_model_state(1928, 1208, "custom-model", False, params) is builtin_model
|
||||
assert calls == [(1928, 1208, False), (1928, 1208, False)]
|
||||
assert params.values == {
|
||||
"Model": modeld.BUILTIN_MODEL_KEY,
|
||||
"DrivingModel": modeld.BUILTIN_MODEL_KEY,
|
||||
"DrivingModelName": "Regret Driven Framework",
|
||||
}
|
||||
|
||||
|
||||
def test_builtin_model_load_failure_is_not_hidden(monkeypatch):
|
||||
monkeypatch.setattr(modeld, "ModelState", lambda *_args: (_ for _ in ()).throw(TypeError("bad builtin")))
|
||||
|
||||
with pytest.raises(TypeError, match="bad builtin"):
|
||||
modeld._load_model_state(1928, 1208, modeld.BUILTIN_MODEL_KEY, False, FakeParams())
|
||||
Reference in New Issue
Block a user