mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-07-26 20:32:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mount Wannahockaloogie

Browse Source
This commit is contained in:
firestar5683
2026-06-09 14:11:13 -05:00
parent 4f48c2ec27
commit 63db39082b
22 changed files with 324 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params_keys.h
+1
View File
@@ -627,6 +627,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"VeryLongStarButtonControl", {PERSISTENT, INT, "0", "0", 2}},
{"VoltSNG", {PERSISTENT, BOOL, "0", "0", 2}},
{"GMAutoHold", {PERSISTENT, BOOL, "0", "0", 2}},
{"ToyotaAutoHold", {PERSISTENT, BOOL, "0", "0", 2}},
{"WarningImmediateVolume", {PERSISTENT, INT, "101", "101", 2}},
{"WarningSoftVolume", {PERSISTENT, INT, "101", "101", 2}},
{"WeatherPresets", {PERSISTENT, BOOL, "0", "0", 2}},
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+29 -5
View File
@@ -709,13 +709,13 @@ class TestHyundaiFingerprint:
def update(lkas_button: int, frame: int):
msg = packer.make_can_msg("CLU13", 1, {
"CF_Clu_SWL_Stat": lkas_button,
"CF_Clu_LdwsLkasSW": lkas_button,
})
can_parsers[Bus.alt].update([(frame, [msg])])
return car_state.update(can_parsers, toggles)[0]
update(0, 1)
ret = update(4, 2)
ret = update(1, 2)
assert any(be.type == ButtonType.lkas and be.pressed for be in ret.buttonEvents)
assert any(be.type == ButtonType.lkas and not be.pressed for be in ret.buttonEvents)
@@ -733,18 +733,42 @@ class TestHyundaiFingerprint:
def update(lkas_button: int, frame: int):
msg = packer.make_can_msg("CLU13", 1, {
"CF_Clu_SWL_Stat": lkas_button,
"CF_Clu_LdwsLkasSW": lkas_button,
})
can_parsers[Bus.alt].update([(frame, [msg])])
return car_state.update(can_parsers, toggles)[0]
ret = update(4, 1)
ret = update(1, 1)
assert not any(be.type == ButtonType.lkas for be in ret.buttonEvents)
ret = update(0, 2)
assert not any(be.type == ButtonType.lkas for be in ret.buttonEvents)
ret = update(4, 3)
ret = update(1, 3)
assert any(be.type == ButtonType.lkas and be.pressed for be in ret.buttonEvents)
assert any(be.type == ButtonType.lkas and not be.pressed for be in ret.buttonEvents)
def test_sonata_alt_bus_clu13_swl_stat_lkas_button_event(self):
toggles = get_test_toggles()
fingerprint = gen_empty_fingerprint()
fingerprint[0][0x391] = 8
fingerprint[1][0x50C] = 8
CP = CarInterface.get_params(CAR.HYUNDAI_SONATA, fingerprint, [], False, False, False, toggles)
FPCP = CarInterface.get_starpilot_params(CAR.HYUNDAI_SONATA, fingerprint, [], CP, toggles)
car_state = CarState(CP, FPCP)
can_parsers = car_state.get_can_parsers(CP)
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
def update(lkas_button: int, frame: int):
msg = packer.make_can_msg("CLU13", 1, {
"CF_Clu_SWL_Stat": lkas_button,
})
can_parsers[Bus.alt].update([(frame, [msg])])
return car_state.update(can_parsers, toggles)[0]
update(0, 1)
ret = update(4, 2)
assert any(be.type == ButtonType.lkas and be.pressed for be in ret.buttonEvents)
assert any(be.type == ButtonType.lkas and not be.pressed for be in ret.buttonEvents)
opendbc_repo/opendbc/car/hyundai/values.py
-1
View File
@@ -919,7 +919,6 @@ ALT_BUS_LDA_BUTTON_CARS = frozenset({
# steering-wheel-status field instead of the dedicated LKAS bit.
ALT_BUS_LDA_BUTTON_SWL_STAT_CARS = frozenset({
CAR.HYUNDAI_SONATA,
CAR.HYUNDAI_SONATA_HYBRID,
})
opendbc_repo/opendbc/car/toyota/carcontroller.py
+30
View File
@@ -43,6 +43,7 @@ MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before torque can be cut
MAX_USER_TORQUE = 500
PARK = structs.CarState.GearShifter.park
REVERSE = structs.CarState.GearShifter.reverse
# Lock / unlock door commands - Credit goes to AlexandreSato!
LOCK_CMD = b"\x40\x05\x30\x11\x00\x80\x00\x00"
@@ -192,6 +193,11 @@ class CarController(CarControllerBase):
self.secoc_prev_reset_counter = 0
self.doors_locked = False
self.auto_brake_hold = bool(self.CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value)
self.brake_hold_active = False
self._brake_hold_counter = 0
self._brake_hold_reset = False
self._prev_brake_pressed = False
def _compute_interceptor_gas_cmd(self, CC, CS):
if not (self.CP.enableGasInterceptorDEPRECATED and self.CP.openpilotLongitudinalControl and CC.longActive):
@@ -237,6 +243,27 @@ class CarController(CarControllerBase):
self.last_standstill = CS.out.standstill
def create_auto_brake_hold_messages(self, CS: structs.CarState, brake_hold_allowed_timer: int = 100):
can_sends = []
brake_hold_allowed = (CS.out.standstill and CS.out.cruiseState.available and
not CS.out.gasPressed and not CS.out.cruiseState.enabled and
CS.out.gearShifter not in (PARK, REVERSE))
if brake_hold_allowed:
self._brake_hold_counter += 1
self.brake_hold_active = self._brake_hold_counter > brake_hold_allowed_timer and not self._brake_hold_reset
self._brake_hold_reset = not self._prev_brake_pressed and CS.out.brakePressed and not self._brake_hold_reset
else:
self._brake_hold_counter = 0
self.brake_hold_active = False
self._brake_hold_reset = False
self._prev_brake_pressed = CS.out.brakePressed
if self.frame % 2 == 0:
can_sends.append(toyotacan.create_brake_hold_command(self.packer, self.frame, CS.pre_collision_2, self.brake_hold_active))
return can_sends
def update(self, CC, CS, now_nanos, starpilot_toggles):
actuators = CC.actuators
stopping = actuators.longControlState == LongCtrlState.stopping
@@ -330,6 +357,9 @@ class CarController(CarControllerBase):
# *** gas and brake ***
self._update_standstill_request(CC, CS, actuators, starpilot_toggles)
if self.auto_brake_hold:
can_sends.extend(self.create_auto_brake_hold_messages(CS))
interceptor_gas_cmd = self._compute_interceptor_gas_cmd(CC, CS)
# handle UI messages
opendbc_repo/opendbc/car/toyota/carstate.py
+5
View File
@@ -80,6 +80,8 @@ class CarState(CarStateBase):
self.has_can_filter = self.FPCP.flags & ToyotaStarPilotFlags.RADAR_CAN_FILTER.value
self.has_SDSU = self.FPCP.flags & ToyotaStarPilotFlags.SMART_DSU.value
self.has_ZSS = self.FPCP.flags & ToyotaStarPilotFlags.ZSS.value
self.auto_brake_hold = bool(self.CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value)
self.pre_collision_2 = {}
def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
cp = can_parsers[Bus.pt]
@@ -211,6 +213,9 @@ class CarState(CarStateBase):
if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
if self.auto_brake_hold:
self.pre_collision_2 = copy.copy(cp_cam.vl["PRE_COLLISION_2"])
if self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR:
self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"]
opendbc_repo/opendbc/car/toyota/interface.py
+8 -1
View File
@@ -2,11 +2,13 @@ from opendbc.car import Bus, structs, get_safety_config, uds
from opendbc.car.toyota.carstate import CarState
from opendbc.car.toyota.carcontroller import CarController
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, \
from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, SECOC_CAR, NO_DSU_CAR, \
MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \
ToyotaSafetyFlags
from opendbc.car.disable_ecu import disable_ecu
from opendbc.car.interfaces import CarInterfaceBase
from opendbc.safety import ALTERNATIVE_EXPERIENCE
from openpilot.common.params import Params
SteerControlType = structs.CarParams.SteerControlType
@@ -138,6 +140,11 @@ class CarInterface(CarInterfaceBase):
ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
ret.enableGasInterceptorDEPRECATED = 0x201 in fingerprint[0] and ret.openpilotLongitudinalControl
toyota_auto_hold = Params(return_defaults=True).get_bool("ToyotaAutoHold")
if toyota_auto_hold and candidate in (TSS2_CAR - RADAR_ACC_CAR - SECOC_CAR):
ret.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
ret.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value
if not ret.openpilotLongitudinalControl:
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.STOCK_LONGITUDINAL.value
opendbc_repo/opendbc/car/toyota/tests/test_toyota.py
+50 -2
View File
@@ -2,7 +2,7 @@ from types import SimpleNamespace
from hypothesis import given, settings, strategies as st
from opendbc.car import Bus
from opendbc.car import Bus, structs
from opendbc.can import CANPacker, CANParser
from opendbc.car.structs import CarParams
from opendbc.car.fw_versions import build_fw_dict
@@ -14,7 +14,9 @@ from opendbc.car.toyota.fingerprints import FW_VERSIONS
from opendbc.car.toyota.interface import CarInterface
from opendbc.car.toyota.values import CAR, DBC, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \
FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \
get_platform_codes
ToyotaFlags, get_platform_codes
from opendbc.safety import ALTERNATIVE_EXPERIENCE
from openpilot.common.params import Params
Ecu = CarParams.Ecu
@@ -40,6 +42,25 @@ class TestToyotaInterfaces:
if car_model in TSS2_CAR and car_model not in SECOC_CAR:
assert dbc[Bus.pt] == "toyota_nodsu_pt_generated"
def test_auto_hold_sets_flag_on_supported_tss2(self):
params = Params()
try:
params.put_bool("ToyotaAutoHold", True)
car_params = CarInterface.get_params(
CAR.TOYOTA_CAMRY_TSS2,
{bus: {} for bus in range(8)},
[],
alpha_long=False,
is_release=False,
docs=False,
starpilot_toggles=SimpleNamespace(),
)
finally:
params.remove("ToyotaAutoHold")
assert car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value
assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB
def test_essential_ecus(self, subtests):
# Asserts standard ECUs exist for each platform
common_ecus = {Ecu.fwdRadar, Ecu.fwdCamera}
@@ -297,6 +318,33 @@ class TestToyotaCarController:
assert parser.vl["LKAS_HUD"]["LEFT_LINE"] == 0
assert parser.vl["LKAS_HUD"]["RIGHT_LINE"] == 0
def test_auto_brake_hold_sends_modified_pre_collision_after_timer(self):
controller = self._make_controller()
controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt])
controller.frame = 0
controller.brake_hold_active = False
controller._brake_hold_counter = 0
controller._brake_hold_reset = False
controller._prev_brake_pressed = False
cs = SimpleNamespace(
out=SimpleNamespace(
standstill=True,
cruiseState=SimpleNamespace(available=True, enabled=False),
gasPressed=False,
brakePressed=False,
gearShifter=structs.CarState.GearShifter.drive,
),
pre_collision_2={},
)
can_sends = controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0)
parser = CANParser(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt], [("PRE_COLLISION_2", 0)], 0)
parser.update([(1, can_sends)])
assert controller.brake_hold_active
assert parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0
assert parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 1
def test_interceptor_stop_and_go_holds_small_launch_at_standstill(self):
controller = self._make_controller()
controller.CP.enableGasInterceptorDEPRECATED = True
opendbc_repo/opendbc/car/toyota/toyotacan.py
+32
View File
@@ -86,6 +86,38 @@ def create_pcs_commands(packer, accel, active, mass):
return [msg1, msg2]
def create_brake_hold_command(packer, frame, pre_collision_2, brake_hold_active):
values = {s: pre_collision_2[s] for s in [
"DSS1GDRV",
"DS1STAT2",
"DS1STBK2",
"PCSWAR",
"PCSALM",
"PCSOPR",
"PCSABK",
"PBATRGR",
"PPTRGR",
"IBTRGR",
"CLEXTRGR",
"IRLT_REQ",
"BRKHLD",
"AVSTRGR",
"VGRSTRGR",
"PREFILL",
"PBRTRGR",
"PCSDIS",
"PBPREPMP",
] if s in pre_collision_2}
if brake_hold_active:
values = {
"DSS1GDRV": 0x3FF,
"PBRTRGR": frame % 730 < 727,
}
return packer.make_can_msg("PRE_COLLISION_2", 0, values)
def create_acc_cancel_command(packer):
values = {
"GAS_RELEASED": 0,
opendbc_repo/opendbc/car/toyota/values.py
+1
View File
@@ -75,6 +75,7 @@ class ToyotaFlags(IntFlag):
# these cars can utilize 2.0 m/s^2
RAISED_ACCEL_LIMIT = 1024
SECOC = 2048
AUTO_BRAKE_HOLD = 4096
# deprecated flags
# these cars are speculated to allow stop and go when the DSU is unplugged
opendbc_repo/opendbc/dbc/generator/toyota/_toyota_adas_standard.dbc
+13
View File
@@ -37,11 +37,24 @@ BO_ 740 STEERING_LKA: 5 XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ DSS1GDRV : 7|10@0- (0.1,0) [0|0] "m/s^2" Vector__XXX
SG_ DS1STAT2 : 13|3@0+ (1,0) [0|0] "" Vector__XXX
SG_ DS1STBK2 : 10|3@0+ (1,0) [0|0] "" Vector__XXX
SG_ PCSWAR : 18|1@0+ (1,0) [0|0] "" FCM
SG_ PCSALM : 17|1@0+ (1,0) [0|0] "" FCM
SG_ PCSOPR : 16|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PCSABK : 31|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ IBTRGR : 27|1@0+ (1,0) [0|0] "" FCM
SG_ PBATRGR : 30|2@0+ (1,0) [0|0] "" Vector__XXX
SG_ PPTRGR : 28|1@0+ (1,0) [0|0] "" FCM
SG_ CLEXTRGR : 26|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ IRLT_REQ : 25|2@0+ (1,0) [0|0] "" Vector__XXX
SG_ BRKHLD : 37|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PREFILL : 33|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ AVSTRGR : 36|1@0+ (1,0) [0|0] "" SCS
SG_ VGRSTRGR : 35|2@0+ (1,0) [0|0] "" Vector__XXX
SG_ PBRTRGR : 32|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PCSDIS : 43|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PBPREPMP : 40|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input";
opendbc_repo/opendbc/dbc/toyota_new_mc_pt_generated.dbc
+13
View File
@@ -650,11 +650,24 @@ BO_ 740 STEERING_LKA: 5 XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ DSS1GDRV : 7|10@0- (0.1,0) [0|0] "m/s^2" Vector__XXX
SG_ DS1STAT2 : 13|3@0+ (1,0) [0|0] "" Vector__XXX
SG_ DS1STBK2 : 10|3@0+ (1,0) [0|0] "" Vector__XXX
SG_ PCSWAR : 18|1@0+ (1,0) [0|0] "" FCM
SG_ PCSALM : 17|1@0+ (1,0) [0|0] "" FCM
SG_ PCSOPR : 16|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PCSABK : 31|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ IBTRGR : 27|1@0+ (1,0) [0|0] "" FCM
SG_ PBATRGR : 30|2@0+ (1,0) [0|0] "" Vector__XXX
SG_ PPTRGR : 28|1@0+ (1,0) [0|0] "" FCM
SG_ CLEXTRGR : 26|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ IRLT_REQ : 25|2@0+ (1,0) [0|0] "" Vector__XXX
SG_ BRKHLD : 37|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PREFILL : 33|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ AVSTRGR : 36|1@0+ (1,0) [0|0] "" SCS
SG_ VGRSTRGR : 35|2@0+ (1,0) [0|0] "" Vector__XXX
SG_ PBRTRGR : 32|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PCSDIS : 43|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PBPREPMP : 40|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input";
opendbc_repo/opendbc/dbc/toyota_nodsu_pt_generated.dbc
+13
View File
@@ -650,11 +650,24 @@ BO_ 740 STEERING_LKA: 5 XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ DSS1GDRV : 7|10@0- (0.1,0) [0|0] "m/s^2" Vector__XXX
SG_ DS1STAT2 : 13|3@0+ (1,0) [0|0] "" Vector__XXX
SG_ DS1STBK2 : 10|3@0+ (1,0) [0|0] "" Vector__XXX
SG_ PCSWAR : 18|1@0+ (1,0) [0|0] "" FCM
SG_ PCSALM : 17|1@0+ (1,0) [0|0] "" FCM
SG_ PCSOPR : 16|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PCSABK : 31|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ IBTRGR : 27|1@0+ (1,0) [0|0] "" FCM
SG_ PBATRGR : 30|2@0+ (1,0) [0|0] "" Vector__XXX
SG_ PPTRGR : 28|1@0+ (1,0) [0|0] "" FCM
SG_ CLEXTRGR : 26|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ IRLT_REQ : 25|2@0+ (1,0) [0|0] "" Vector__XXX
SG_ BRKHLD : 37|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PREFILL : 33|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ AVSTRGR : 36|1@0+ (1,0) [0|0] "" SCS
SG_ VGRSTRGR : 35|2@0+ (1,0) [0|0] "" Vector__XXX
SG_ PBRTRGR : 32|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PCSDIS : 43|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PBPREPMP : 40|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input";
opendbc_repo/opendbc/dbc/toyota_tnga_k_pt_generated.dbc
+13
View File
@@ -650,11 +650,24 @@ BO_ 740 STEERING_LKA: 5 XXX
BO_ 836 PRE_COLLISION_2: 8 DSU
SG_ DSS1GDRV : 7|10@0- (0.1,0) [0|0] "m/s^2" Vector__XXX
SG_ DS1STAT2 : 13|3@0+ (1,0) [0|0] "" Vector__XXX
SG_ DS1STBK2 : 10|3@0+ (1,0) [0|0] "" Vector__XXX
SG_ PCSWAR : 18|1@0+ (1,0) [0|0] "" FCM
SG_ PCSALM : 17|1@0+ (1,0) [0|0] "" FCM
SG_ PCSOPR : 16|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PCSABK : 31|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ IBTRGR : 27|1@0+ (1,0) [0|0] "" FCM
SG_ PBATRGR : 30|2@0+ (1,0) [0|0] "" Vector__XXX
SG_ PPTRGR : 28|1@0+ (1,0) [0|0] "" FCM
SG_ CLEXTRGR : 26|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ IRLT_REQ : 25|2@0+ (1,0) [0|0] "" Vector__XXX
SG_ BRKHLD : 37|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PREFILL : 33|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ AVSTRGR : 36|1@0+ (1,0) [0|0] "" SCS
SG_ VGRSTRGR : 35|2@0+ (1,0) [0|0] "" Vector__XXX
SG_ PBRTRGR : 32|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PCSDIS : 43|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ PBPREPMP : 40|1@0+ (1,0) [0|0] "" Vector__XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input";
opendbc_repo/opendbc/safety/modes/toyota.h
+17
View File
@@ -351,6 +351,13 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
}
}
}
// Auto brake hold replaces the camera AEB message only while stopped.
if ((msg->addr == 0x344U) && ((alternative_experience & ALT_EXP_ALLOW_AEB) != 0)) {
if (vehicle_moving || gas_pressed || !acc_main_on) {
tx = false;
}
}
}
// UDS: Only tester present ("\x0F\x02\x3E\x00\x00\x00\x00\x00") allowed on diagnostics address
@@ -458,10 +465,20 @@ static safety_config toyota_init(uint16_t param) {
return ret;
}
static bool toyota_fwd_hook(int bus_num, int addr) {
bool block_msg = false;
if (bus_num == 2) {
block_msg = (addr == 0x344) && ((alternative_experience & ALT_EXP_ALLOW_AEB) != 0) &&
!vehicle_moving && !gas_pressed && acc_main_on;
}
return block_msg;
}
const safety_hooks toyota_hooks = {
.init = toyota_init,
.rx = toyota_rx_hook,
.tx = toyota_tx_hook,
.fwd = toyota_fwd_hook,
.get_checksum = toyota_get_checksum,
.compute_checksum = toyota_compute_checksum,
.get_quality_flag_valid = toyota_get_quality_flag_valid,
opendbc_repo/opendbc/safety/tests/test_toyota.py
+25
View File
@@ -9,6 +9,7 @@ from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
from opendbc.safety import ALTERNATIVE_EXPERIENCE
TOYOTA_COMMON_TX_MSGS = [[0x2E4, 0], [0x191, 0], [0x412, 0], [0x343, 0], [0x1D2, 0], [0x1D3, 0]] # LKAS + LTA + ACC & PCM cancel cmds
TOYOTA_SECOC_TX_MSGS = [[0x131, 0], [0x183, 0]] + TOYOTA_COMMON_TX_MSGS
@@ -94,6 +95,30 @@ class TestToyotaSafetyBase(common.CarSafetyTest, common.LongitudinalAccelSafetyT
msg = libsafety_py.make_CANPacket(0x283, 0, bytes(dat))
self.assertEqual(not bad and not stock_longitudinal, self._tx(msg))
def test_auto_brake_hold_aeb_replacement_only_at_standstill(self):
self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALLOW_AEB)
hold_msg = libsafety_py.make_CANPacket(0x344, 0, b"\xfd\x80\x00\x00\x00\x00\x00\xcc")
self._rx(self._speed_msg(0))
self._rx(self._toggle_aol(True))
self._rx(self._user_gas_msg(False))
self.assertTrue(self._tx(hold_msg))
self.assertEqual(-1, self.safety.safety_fwd_hook(2, 0x344))
self._rx(self._speed_msg(1.0))
self.assertFalse(self._tx(hold_msg))
self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344))
self._rx(self._speed_msg(0))
self._rx(self._user_gas_msg(True))
self.assertFalse(self._tx(hold_msg))
self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344))
self._rx(self._user_gas_msg(False))
self._rx(self._toggle_aol(False))
self.assertFalse(self._tx(hold_msg))
self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344))
# Only allow LTA msgs with no actuation
def test_lta_steer_cmd(self):
for engaged, req, req2, torque_wind_down, angle in itertools.product([True, False],
selfdrive/controls/lib/longitudinal_planner.py
+4 -4
View File
@@ -2307,7 +2307,7 @@ class LongitudinalPlanner:
if close_final_guard_caps:
close_final_guard_cap = min(close_final_guard_caps)
if lead_one_active:
if allow_complex_follow_logic and lead_one_active:
lead_catchup_accel_cap = self.get_lead_catchup_accel_cap(self.lead_one, scene_v_ego, effective_t_follow)
if lead_catchup_accel_cap is not None:
self.a_desired = min(self.a_desired, lead_catchup_accel_cap)
@@ -2328,7 +2328,7 @@ class LongitudinalPlanner:
effective_t_follow,
allow_optional_far_lead_logic=allow_complex_follow_logic,
)
if follow_control_lead is not None and not panic_bypass:
if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass:
if not output_should_stop and not vision_low_speed_stop_active:
tracked_vision_model_brake_floor = self.get_tracked_vision_model_brake_floor(
follow_control_lead,
@@ -2366,7 +2366,7 @@ class LongitudinalPlanner:
self.a_desired = max(self.a_desired, far_lead_brake_cap)
output_a_target = max(output_a_target, far_lead_brake_cap)
if follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active:
if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active:
tracked_vision_model_brake_cap = self.get_tracked_vision_model_brake_cap(
follow_control_lead,
scene_v_ego,
@@ -2411,7 +2411,7 @@ class LongitudinalPlanner:
self.a_desired = max(self.a_desired, near_duplicate_transition_target)
output_a_target = near_duplicate_transition_target
if follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active:
if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active:
cruise_tracking_lead_accel_cap = self.get_cruise_tracking_lead_accel_cap(
follow_control_lead,
scene_v_ego,
selfdrive/controls/tests/test_longitudinal_planner.py
+55 -1
View File
@@ -111,7 +111,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta
}
def make_toggles(model_version: str = "v11", radar_takeoffs: bool = False):
def make_toggles(model_version: str = "v11", radar_takeoffs: bool = False, prioritize_smooth_following: bool = False):
return SimpleNamespace(
taco_tune=False,
classic_model=False,
@@ -120,6 +120,7 @@ def make_toggles(model_version: str = "v11", radar_takeoffs: bool = False):
stop_distance=6.0,
vEgoStopping=0.5,
radar_takeoffs=radar_takeoffs,
prioritize_smooth_following=prioritize_smooth_following,
)
@@ -2190,6 +2191,59 @@ def test_cruise_tracking_lead_accel_transition_target_skips_clear_pullaway():
assert smoothed is None
def test_prioritize_smooth_following_skips_post_097_follow_nudges():
v_ego = 16.2
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
lead = make_lead(status=True, d_rel=33.4, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.12)
def run_once(prioritize_smooth_following: bool) -> list[str]:
planner = LongitudinalPlanner(CP, init_v=v_ego)
sm = make_sm(
v_ego,
desired_accel=0.8,
min_accel=-0.5,
experimental_mode=False,
tracking_lead=True,
lead_one=lead,
)
sm["starpilotPlan"].vCruise = v_ego + 8.0
calls = []
def record(name, return_value=None):
def _inner(self, *args, **kwargs):
calls.append(name)
return return_value
return types.MethodType(_inner, planner)
planner.get_follow_control_lead = record("get_follow_control_lead", lead)
planner.get_lead_catchup_accel_cap = record("get_lead_catchup_accel_cap", None)
planner.get_tracked_vision_model_brake_floor = record("get_tracked_vision_model_brake_floor", None)
planner.get_low_speed_follow_transition_brake_cap = record("get_low_speed_follow_transition_brake_cap", None)
planner.get_tracked_vision_model_brake_cap = record("get_tracked_vision_model_brake_cap", None)
planner.get_cruise_tracking_lead_accel_cap = record("get_cruise_tracking_lead_accel_cap", None)
planner.get_cruise_tracking_lead_accel_transition_target = record("get_cruise_tracking_lead_accel_transition_target", None)
planner.update(sm, make_toggles(prioritize_smooth_following=prioritize_smooth_following))
return calls
calls_default = run_once(False)
calls_smooth = run_once(True)
expected_calls = {
"get_lead_catchup_accel_cap",
"get_follow_control_lead",
"get_tracked_vision_model_brake_floor",
"get_low_speed_follow_transition_brake_cap",
"get_tracked_vision_model_brake_cap",
"get_cruise_tracking_lead_accel_cap",
"get_cruise_tracking_lead_accel_transition_target",
}
assert expected_calls.issubset(set(calls_default))
assert not expected_calls.intersection(set(calls_smooth))
def test_near_duplicate_lead_source_hysteresis_prefers_previous_source():
v_ego = 27.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
selfdrive/test/longitudinal_maneuvers/maneuver.py
+2
View File
@@ -25,6 +25,7 @@ class Maneuver:
self.e2e = kwargs.get("e2e", False)
self.personality = kwargs.get("personality", 0)
self.force_decel = kwargs.get("force_decel", False)
self.prioritize_smooth_following = kwargs.get("prioritize_smooth_following", False)
self.duration = duration
self.title = title
@@ -41,6 +42,7 @@ class Maneuver:
e2e=self.e2e,
personality=self.personality,
force_decel=self.force_decel,
prioritize_smooth_following=self.prioritize_smooth_following,
)
valid = True
selfdrive/test/longitudinal_maneuvers/plant.py
+4 -1
View File
@@ -56,7 +56,8 @@ class Plant:
def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0,
enabled=True, only_lead2=False, only_radar=False, track_lead_with_gate=False,
e2e=False, personality=0, force_decel=False):
e2e=False, personality=0, force_decel=False,
prioritize_smooth_following=False):
self.rate = 1. / DT_MDL
current_prefix = os.environ.get("OPENPILOT_PREFIX")
@@ -91,6 +92,7 @@ class Plant:
self.e2e = e2e
self.personality = personality
self.force_decel = force_decel
self.prioritize_smooth_following = prioritize_smooth_following
self.tracking_lead_filter = FirstOrderFilter(0.0, 0.5, DT_MDL)
self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
@@ -111,6 +113,7 @@ class Plant:
stop_distance=6.0,
longitudinalActuatorDelay=0.2,
vEgoStopping=0.5,
prioritize_smooth_following=self.prioritize_smooth_following,
)
@property
starpilot/common/safe_mode.py
+1
View File
@@ -180,6 +180,7 @@ SAFE_MODE_MANAGED_KEYS = (
"RelaxedJerkSpeedDecrease",
"FrogsGoMoosTweak",
"SNGHack",
"ToyotaAutoHold",
"SubaruSNG",
"SubaruSNGManualParkingBrake",
"VoltSNG",
starpilot/common/starpilot_variables.py
+1
View File
@@ -1190,6 +1190,7 @@ class StarPilotVariables:
toggle.standby_mode = self.get_value("StandbyMode", condition=screen_management)
toggle.sng_hack = self.get_value("SNGHack", condition=toggle.openpilot_longitudinal and toggle.car_make == "toyota" and not toggle.has_pedal and not has_sng)
toggle.toyota_auto_hold = self.get_value("ToyotaAutoHold", condition=toggle.car_make == "toyota")
toggle.speed_limit_controller = toggle.openpilot_longitudinal and self.get_value("SpeedLimitController")
speed_limit_display = toggle.show_speed_limits or toggle.speed_limit_controller
starpilot/system/the_pond/assets/components/tools/device_settings_layout.json
+7
View File
@@ -2502,6 +2502,13 @@
"data_type": "bool",
"ui_type": "toggle"
},
{
"key": "ToyotaAutoHold",
"label": "Toyota Auto Hold",
"description": "Hold the brakes at a stop on supported Toyota/Lexus TSS2 vehicles when cruise main is available and cruise is not active.",
"data_type": "bool",
"ui_type": "toggle"
},
{
"key": "RemapCancelToDistance",
"label": "Remap Cancel Button",