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

Actuator Door

Browse Source
This commit is contained in:
firestar5683
2026-08-06 17:36:20 -05:00
parent 5d5a04416a
commit d51bd65695
26 changed files with 420 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carstate.py
+4
View File
@@ -117,6 +117,7 @@ class CarState(CarStateBase):
"CRUISE_BUTTONS"
self.is_metric = False
self.buttons_counter = 0
self.main_cruise_on = False
self.cruise_info = {}
self.msg_161 = {}
@@ -416,6 +417,9 @@ class CarState(CarStateBase):
*create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise}),
*lkas_button_events]
if getattr(self.FPCP, "flags", 0) & HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING:
ret.cruiseState.available = self.update_main_cruise(ret)
ret.blockPcmEnable = not self.recent_button_interaction()
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
opendbc_repo/opendbc/car/hyundai/interface.py
+5
View File
@@ -214,6 +214,11 @@ class CarInterface(CarInterfaceBase):
if hyundai_cancel_button_enables_cruise(candidate):
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANCEL_BTN_ENABLE.value
if 0x2AA in fingerprint[0]:
ret.minSteerSpeed = 0.0
ret.flags &= ~HyundaiFlags.MIN_STEER_32_MPH.value
ret.steerAtStandstill = True
# Common lateral control setup
ret.centerToFront = ret.wheelbase * 0.4
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+37 -1
View File
@@ -26,7 +26,7 @@ from opendbc.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR
HYBRID_CAR, EV_CAR, FW_QUERY_CONFIG, LEGACY_SAFETY_MODE_CAR, CANFD_FUZZY_WHITELIST, \
UNSUPPORTED_LONGITUDINAL_CAR, PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \
LEGACY_LONGITUDINAL_CAR, DBC, HyundaiFlags, get_platform_codes, HyundaiSafetyFlags, \
HyundaiStarPilotSafetyFlags, Buttons, CarControllerParams, kia_ev6_gt_line_longitudinal_tuning
HyundaiStarPilotFlags, HyundaiStarPilotSafetyFlags, Buttons, CarControllerParams, kia_ev6_gt_line_longitudinal_tuning
LongCtrlState = CarControl.Actuators.LongControlState
from opendbc.car.hyundai.fingerprints import FW_VERSIONS
@@ -312,6 +312,21 @@ class TestHyundaiFingerprint:
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, None)
assert CP.flags & HyundaiFlags.SEND_LFA
def test_smart_mdps_allows_low_speed_steering(self):
candidate = CAR.HYUNDAI_IONIQ_EV_LTD
regular_fingerprint = gen_empty_fingerprint()
regular_cp = CarInterface.get_params(candidate, regular_fingerprint, [], False, False, False, None)
assert regular_cp.flags & HyundaiFlags.MIN_STEER_32_MPH
assert regular_cp.minSteerSpeed > 0.0
smart_mdps_fingerprint = gen_empty_fingerprint()
smart_mdps_fingerprint[0][0x2AA] = 8
smart_mdps_cp = CarInterface.get_params(candidate, smart_mdps_fingerprint, [], False, False, False, None)
assert not (smart_mdps_cp.flags & HyundaiFlags.MIN_STEER_32_MPH)
assert smart_mdps_cp.minSteerSpeed == 0.0
assert smart_mdps_cp.steerAtStandstill
@pytest.mark.parametrize("candidate", CCNC_NON_HDA2_CARS)
def test_ccnc_non_hda2_platforms_set_ccnc_safety(self, candidate):
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], False, False, False, None)
@@ -509,6 +524,27 @@ class TestHyundaiFingerprint:
)
assert not (minimal_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC)
def test_classic_hyundai_long_tracks_main_cruise_state(self):
toggles = get_test_toggles()
classic_cp = CarInterface.get_params(CAR.HYUNDAI_ELANTRA_2021, gen_empty_fingerprint(), [], True, False, False, toggles)
classic_fpcp = CarInterface.get_starpilot_params(
CAR.HYUNDAI_ELANTRA_2021, gen_empty_fingerprint(), [], classic_cp, toggles,
)
assert classic_fpcp.flags & HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING
car_state = CarState(classic_cp, classic_fpcp)
ret = SimpleNamespace(
cruiseState=SimpleNamespace(available=True),
buttonEvents=[structs.CarState.ButtonEvent(pressed=True, type=ButtonType.mainCruise)],
)
assert car_state.update_main_cruise(ret)
ioniq_cp = CarInterface.get_params(CAR.HYUNDAI_IONIQ_6, gen_empty_fingerprint(), [], True, False, False, toggles)
ioniq_fpcp = CarInterface.get_starpilot_params(
CAR.HYUNDAI_IONIQ_6, gen_empty_fingerprint(), [], ioniq_cp, toggles,
)
assert not (ioniq_fpcp.flags & HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING)
def test_non_scc_flag_quirks(self):
elantra_hev = CarInterface.get_params(CAR.HYUNDAI_ELANTRA_HEV_2022_NON_SCC, gen_empty_fingerprint(), [], True, False, False, None)
assert elantra_hev.flags & HyundaiFlags.HYBRID
opendbc_repo/opendbc/car/hyundai/values.py
+1
View File
@@ -118,6 +118,7 @@ class HyundaiStarPilotSafetyFlags(IntFlag):
class HyundaiStarPilotFlags(IntFlag):
SPEED_LIMIT_AVAILABLE = 1
MAIN_CRUISE_STATE_TRACKING = 2 ** 2
class HyundaiFlags(IntFlag):
opendbc_repo/opendbc/car/interfaces.py
+3
View File
@@ -226,6 +226,9 @@ class CarInterfaceBase(ABC):
fp_ret.flags |= int(HondaStarPilotFlags.HAS_CAMERA_MESSAGES)
elif platform in HYUNDAI:
if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD):
fp_ret.flags |= HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING.value
if candidate in CANFD_CAR:
hda2 = Ecu.adas in [fw.ecu for fw in car_fw]
CAN = CanBus(None, fingerprint, bool(CP.flags & HyundaiFlags.CANFD_LKA_STEERING))
opendbc_repo/opendbc/car/subaru/carcontroller.py
+10 -5
View File
@@ -33,6 +33,9 @@ class CarController(CarControllerBase):
self.p = CarControllerParams(CP)
self.packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
self.main_bus = CanBus.main_for_cp(CP)
self.angle_bus = CanBus.angle_for_cp(CP)
self.status_bus = CanBus.camera if CP.flags & SubaruFlags.D_PLATFORM else CanBus.main
if CP.flags & SubaruFlags.LKAS_ANGLE:
self.VM = VehicleModel(get_safety_CP())
@@ -63,7 +66,7 @@ class CarController(CarControllerBase):
apply_steer = CS.out.steeringAngleDeg
self.apply_steer_last = apply_steer
return subarucan.create_steering_control_angle(self.packer, apply_steer, lat_active)
return subarucan.create_steering_control_angle(self.packer, apply_steer, lat_active, self.angle_bus)
def lateral_torque(self, CC, CS):
apply_torque = int(round(CC.actuators.torque * self.p.STEER_MAX))
@@ -154,14 +157,16 @@ class CarController(CarControllerBase):
else:
if self.frame % 10 == 0:
can_sends.append(subarucan.create_es_dashstatus(self.packer, self.frame // 10, CS.es_dashstatus_msg, CC.enabled,
self.CP.openpilotLongitudinalControl, CC.longActive, hud_control.leadVisible))
self.CP.openpilotLongitudinalControl, CC.longActive, hud_control.leadVisible,
self.status_bus))
can_sends.append(subarucan.create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
hud_control.leftLaneDepart, hud_control.rightLaneDepart))
hud_control.leftLaneDepart, hud_control.rightLaneDepart, self.status_bus))
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert))
can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg,
hud_control.visualAlert, self.status_bus))
if starpilot_toggles.subaru_sng:
can_sends.append(subarucan.create_throttle(self.packer, CS.throttle_msg["COUNTER"] + 1, CS.throttle_msg,
@@ -183,7 +188,7 @@ class CarController(CarControllerBase):
else:
if pcm_cancel_cmd:
if not (self.CP.flags & SubaruFlags.HYBRID):
bus = CanBus.alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else CanBus.main
bus = CanBus.alt_for_cp(self.CP) if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else self.main_bus
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg["COUNTER"] + 1, CS.es_distance_msg, bus, pcm_cancel_cmd))
if self.CP.flags & SubaruFlags.DISABLE_EYESIGHT:
opendbc_repo/opendbc/car/subaru/carstate.py
+12 -10
View File
@@ -20,6 +20,7 @@ class CarState(CarStateBase):
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
cp_alt = can_parsers[Bus.alt]
cp_angle = cp_cam if self.CP.flags & SubaruFlags.D_PLATFORM else cp
ret = structs.CarState()
throttle_msg = cp.vl["Throttle"] if not (self.CP.flags & SubaruFlags.HYBRID) else cp_alt.vl["Throttle_Hybrid"]
@@ -54,16 +55,17 @@ class CarState(CarStateBase):
cp.vl["Dashlights"]["RIGHT_BLINKER"])
if self.CP.enableBsm:
ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1)
ret.rightBlindspot = (cp.vl["BSD_RCTA"]["R_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["R_APPROACHING"] == 1)
cp_bsm = cp_cam if self.CP.flags & SubaruFlags.D_PLATFORM else cp
ret.leftBlindspot = (cp_bsm.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp_bsm.vl["BSD_RCTA"]["L_APPROACHING"] == 1)
ret.rightBlindspot = (cp_bsm.vl["BSD_RCTA"]["R_ADJACENT"] == 1) or (cp_bsm.vl["BSD_RCTA"]["R_APPROACHING"] == 1)
cp_transmission = cp_alt if self.CP.flags & SubaruFlags.HYBRID else cp
can_gear = int(cp_transmission.vl["Transmission"]["Gear"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
if self.CP.flags & SubaruFlags.LKAS_ANGLE:
ret.steeringAngleDeg = cp.vl["Steering_2"]["Steering_Angle"]
steering_updated = len(cp.vl_all["Steering_2"]["Steering_Angle"]) > 0
ret.steeringAngleDeg = cp_angle.vl["Steering_2"]["Steering_Angle"]
steering_updated = len(cp_angle.vl_all["Steering_2"]["Steering_Angle"]) > 0
else:
ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
steering_updated = len(cp.vl_all["Steering_Torque"]["Steering_Angle"]) > 0
@@ -72,8 +74,8 @@ class CarState(CarStateBase):
# ideally we get this from the car, but unclear if it exists. diagnostic software doesn't even have it
ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, steering_updated)
ret.steeringTorque = cp.vl["Steering_Torque"]["Steer_Torque_Sensor"]
ret.steeringTorqueEps = cp.vl["Steering_Torque"]["Steer_Torque_Output"]
ret.steeringTorque = cp_angle.vl["Steering_Torque"]["Steer_Torque_Sensor"]
ret.steeringTorqueEps = cp_angle.vl["Steering_Torque"]["Steer_Torque_Output"]
steer_threshold = 75 if self.CP.flags & SubaruFlags.PREGLOBAL else 80
ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold
@@ -100,13 +102,13 @@ class CarState(CarStateBase):
cp.vl["BodyInfo"]["DOOR_OPEN_RL"],
cp.vl["BodyInfo"]["DOOR_OPEN_FR"],
cp.vl["BodyInfo"]["DOOR_OPEN_FL"]])
ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1
ret.steerFaultPermanent = cp_angle.vl["Steering_Torque"]["Steer_Error_1"] == 1
if self.CP.flags & SubaruFlags.PREGLOBAL:
self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"]
self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"]
else:
ret.steerFaultTemporary = cp.vl["Steering_Torque"]["Steer_Warning"] == 1
ret.steerFaultTemporary = cp_angle.vl["Steering_Torque"]["Steer_Warning"] == 1
ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1
ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3
ret.stockFcw = (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 1) or \
@@ -145,7 +147,7 @@ class CarState(CarStateBase):
@staticmethod
def get_can_parsers(CP):
return {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.main),
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.main_for_cp(CP)),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.camera),
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.alt)
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.alt_for_cp(CP))
}
opendbc_repo/opendbc/car/subaru/interface.py
+5 -2
View File
@@ -3,7 +3,7 @@ from opendbc.car.disable_ecu import disable_ecu
from opendbc.car.interfaces import CarInterfaceBase
from opendbc.car.subaru.carcontroller import CarController
from opendbc.car.subaru.carstate import CarState
from opendbc.car.subaru.values import CAR, GLOBAL_ES_ADDR, SubaruFlags, SubaruSafetyFlags
from opendbc.car.subaru.values import CAR, CanBus, GLOBAL_ES_ADDR, SubaruFlags, SubaruSafetyFlags
class CarInterface(CarInterfaceBase):
@@ -29,12 +29,15 @@ class CarInterface(CarInterfaceBase):
ret.enableBsm = 0x25c in fingerprint[0]
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.subaruPreglobal)]
else:
ret.enableBsm = 0x228 in fingerprint[0]
bsm_bus = CanBus.camera if ret.flags & SubaruFlags.D_PLATFORM else CanBus.main
ret.enableBsm = 0x228 in fingerprint[bsm_bus]
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.subaru)]
if ret.flags & SubaruFlags.GLOBAL_GEN2:
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.GEN2.value
if ret.flags & SubaruFlags.LKAS_ANGLE:
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.LKAS_ANGLE.value
if ret.flags & SubaruFlags.D_PLATFORM:
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM.value
ret.steerLimitTimer = 0.4
ret.steerActuatorDelay = 0.1
opendbc_repo/opendbc/car/subaru/subarucan.py
+13 -12
View File
@@ -13,13 +13,13 @@ def create_steering_control(packer, apply_torque, steer_req):
return packer.make_can_msg("ES_LKAS", 0, values)
def create_steering_control_angle(packer, apply_angle, steer_req):
def create_steering_control_angle(packer, apply_angle, steer_req, bus=CanBus.main):
values = {
"LKAS_Output": apply_angle,
"LKAS_Request": steer_req,
"SET_3": 3
}
return packer.make_can_msg("ES_LKAS_ANGLE", 0, values)
return packer.make_can_msg("ES_LKAS_ANGLE", bus, values)
def create_steering_status(packer):
@@ -67,7 +67,8 @@ def create_es_distance(packer, frame, es_distance_msg, bus, pcm_cancel_cmd, long
return packer.make_can_msg("ES_Distance", bus, values)
def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart,
bus=CanBus.main):
values = {s: es_lkas_state_msg[s] for s in [
"CHECKSUM",
"LKAS_Alert_Msg",
@@ -128,10 +129,10 @@ def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert
values["LKAS_Left_Line_Visible"] = int(left_line)
values["LKAS_Right_Line_Visible"] = int(right_line)
return packer.make_can_msg("ES_LKAS_State", CanBus.main, values)
return packer.make_can_msg("ES_LKAS_State", bus, values)
def create_es_dashstatus(packer, frame, dashstatus_msg, enabled, long_enabled, long_active, lead_visible):
def create_es_dashstatus(packer, frame, dashstatus_msg, enabled, long_enabled, long_active, lead_visible, bus=CanBus.main):
values = {s: dashstatus_msg[s] for s in [
"CHECKSUM",
"PCB_Off",
@@ -177,10 +178,10 @@ def create_es_dashstatus(packer, frame, dashstatus_msg, enabled, long_enabled, l
if values["LKAS_State_Msg"] in (2, 3):
values["LKAS_State_Msg"] = 0
return packer.make_can_msg("ES_DashStatus", CanBus.main, values)
return packer.make_can_msg("ES_DashStatus", bus, values)
def create_es_brake(packer, frame, es_brake_msg, long_enabled, long_active, brake_value):
def create_es_brake(packer, frame, es_brake_msg, long_enabled, long_active, brake_value, bus=CanBus.main):
values = {s: es_brake_msg[s] for s in [
"CHECKSUM",
"Signal1",
@@ -204,10 +205,10 @@ def create_es_brake(packer, frame, es_brake_msg, long_enabled, long_active, brak
values["Cruise_Brake_Active"] = brake_value > 0
values["Cruise_Brake_Lights"] = brake_value >= 70
return packer.make_can_msg("ES_Brake", CanBus.main, values)
return packer.make_can_msg("ES_Brake", bus, values)
def create_es_status(packer, frame, es_status_msg, long_enabled, long_active, cruise_rpm):
def create_es_status(packer, frame, es_status_msg, long_enabled, long_active, cruise_rpm, bus=CanBus.main):
values = {s: es_status_msg[s] for s in [
"CHECKSUM",
"Signal1",
@@ -227,10 +228,10 @@ def create_es_status(packer, frame, es_status_msg, long_enabled, long_active, cr
values["Cruise_Activated"] = long_active
return packer.make_can_msg("ES_Status", CanBus.main, values)
return packer.make_can_msg("ES_Status", bus, values)
def create_es_infotainment(packer, frame, es_infotainment_msg, visual_alert):
def create_es_infotainment(packer, frame, es_infotainment_msg, visual_alert, bus=CanBus.main):
# Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts
values = {s: es_infotainment_msg[s] for s in [
"CHECKSUM",
@@ -253,7 +254,7 @@ def create_es_infotainment(packer, frame, es_infotainment_msg, visual_alert):
if visual_alert == VisualAlert.fcw:
values["LKAS_State_Infotainment"] = 2
return packer.make_can_msg("ES_Infotainment", CanBus.main, values)
return packer.make_can_msg("ES_Infotainment", bus, values)
def create_es_highbeamassist(packer):
opendbc_repo/opendbc/car/subaru/tests/test_subaru.py
+27 -1
View File
@@ -2,11 +2,13 @@ from types import SimpleNamespace
import pytest
from opendbc.car import Bus
from opendbc.car.subaru.carcontroller import CarController
from opendbc.car.subaru.carstate import CarState
from opendbc.car.subaru.fingerprints import FW_VERSIONS
from opendbc.car.fw_versions import match_fw_to_car
from opendbc.car.subaru.interface import CarInterface
from opendbc.car.subaru.values import CAR, SubaruFlags, SubaruSafetyFlags
from opendbc.car.subaru.values import CAR, CanBus, SubaruFlags, SubaruSafetyFlags
from opendbc.car.structs import CarParams
@@ -112,6 +114,30 @@ def test_torque_platform_does_not_enable_angle_safety():
assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.LKAS_ANGLE)
def test_outback_2023_uses_d_platform_bus_layout():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_OUTBACK_2023)
parsers = CarState.get_can_parsers(CP)
assert CP.flags & SubaruFlags.D_PLATFORM
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM
assert CanBus.main_for_cp(CP) == CanBus.alt
assert CanBus.angle_for_cp(CP) == CanBus.camera
assert parsers[Bus.pt].bus == CanBus.alt
assert parsers[Bus.cam].bus == CanBus.camera
assert parsers[Bus.alt].bus == CanBus.alt
def test_other_angle_platforms_keep_existing_bus_layout():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025)
parsers = CarState.get_can_parsers(CP)
assert not (CP.flags & SubaruFlags.D_PLATFORM)
assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM)
assert parsers[Bus.pt].bus == CanBus.main
assert parsers[Bus.cam].bus == CanBus.camera
assert parsers[Bus.alt].bus == CanBus.alt
def test_angle_controller_tracks_driver_override():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025)
controller = CarController({}, CP)
opendbc_repo/opendbc/car/subaru/values.py
+15 -1
View File
@@ -74,6 +74,7 @@ class SubaruSafetyFlags(IntFlag):
PREGLOBAL_REVERSED_DRIVER_TORQUE = 4
STOP_AND_GO = 8
LKAS_ANGLE = 16
D_PLATFORM = 32
class SubaruFlags(IntFlag):
@@ -90,6 +91,7 @@ class SubaruFlags(IntFlag):
PREGLOBAL = 16
HYBRID = 32
LKAS_ANGLE = 64
D_PLATFORM = 128
GLOBAL_ES_ADDR = 0x787
@@ -101,6 +103,18 @@ class CanBus:
alt = 1
camera = 2
@staticmethod
def main_for_cp(CP):
return CanBus.alt if CP.flags & SubaruFlags.D_PLATFORM else CanBus.main
@staticmethod
def alt_for_cp(CP):
return CanBus.alt
@staticmethod
def angle_for_cp(CP):
return CanBus.camera if CP.flags & SubaruFlags.D_PLATFORM else CanBus.main
class Footnote(Enum):
GLOBAL = CarFootnote(
@@ -221,7 +235,7 @@ class CAR(Platforms):
SUBARU_OUTBACK_2023 = SubaruGen2PlatformConfig(
[SubaruCarDocs("Subaru Outback 2023-24", "All", car_parts=CarParts.common([CarHarness.subaru_d]))],
SUBARU_OUTBACK.specs,
flags=SubaruFlags.LKAS_ANGLE,
flags=SubaruFlags.LKAS_ANGLE | SubaruFlags.D_PLATFORM,
)
SUBARU_ASCENT_2023 = SubaruGen2PlatformConfig(
[SubaruCarDocs("Subaru Ascent 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d]))],
opendbc_repo/opendbc/safety/modes/subaru.h
+36 -4
View File
@@ -55,6 +55,12 @@
#define SUBARU_COMMON_TX_MSGS(alt_bus) \
{MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = false}, \
#define SUBARU_D_PLATFORM_ANGLE_TX_MSGS() \
{MSG_SUBARU_ES_LKAS_ANGLE, SUBARU_CAM_BUS, 8, .check_relay = true}, \
{MSG_SUBARU_ES_DashStatus, SUBARU_CAM_BUS, 8, .check_relay = true}, \
{MSG_SUBARU_ES_LKAS_State, SUBARU_CAM_BUS, 8, .check_relay = true}, \
{MSG_SUBARU_ES_Infotainment, SUBARU_CAM_BUS, 8, .check_relay = true}, \
#define SUBARU_COMMON_LONG_TX_MSGS(alt_bus) \
{MSG_SUBARU_ES_Distance, alt_bus, 8, .check_relay = true}, \
{MSG_SUBARU_ES_Brake, alt_bus, 8, .check_relay = true}, \
@@ -85,10 +91,21 @@
{.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_ES_Status, status_bus, 8, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define SUBARU_D_PLATFORM_ANGLE_RX_CHECKS() \
{.msg = {{MSG_SUBARU_Throttle, SUBARU_ALT_BUS, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_CAM_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Steering_2, SUBARU_CAM_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Wheel_Speeds, SUBARU_ALT_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Brake_Status, SUBARU_ALT_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_ES_Brake, SUBARU_ALT_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_ES_Status, SUBARU_ALT_BUS, 8, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_ES_DashStatus, SUBARU_CAM_BUS, 8, 10U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
static bool subaru_gen2 = false;
static bool subaru_longitudinal = false;
static bool subaru_stop_and_go = false;
static bool subaru_lkas_angle = false;
static bool subaru_d_platform = false;
static uint32_t subaru_get_checksum(const CANPacket_t *msg) {
return (uint8_t)msg->data[0];
@@ -110,15 +127,17 @@ static uint32_t subaru_compute_checksum(const CANPacket_t *msg) {
static void subaru_rx_hook(const CANPacket_t *msg) {
const unsigned int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
const unsigned int status_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_CAM_BUS;
const unsigned int steering_bus = subaru_d_platform ? SUBARU_CAM_BUS : SUBARU_MAIN_BUS;
const unsigned int main_bus = subaru_d_platform ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
if ((msg->addr == MSG_SUBARU_Steering_Torque) && (msg->bus == SUBARU_MAIN_BUS)) {
if ((msg->addr == MSG_SUBARU_Steering_Torque) && (msg->bus == steering_bus)) {
int torque_driver_new;
torque_driver_new = ((GET_BYTES(msg, 0, 4) >> 16) & 0x7FFU);
torque_driver_new = -1 * to_signed(torque_driver_new, 11);
update_sample(&torque_driver, torque_driver_new);
}
if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_Steering_2) && (msg->bus == SUBARU_MAIN_BUS)) {
if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_Steering_2) && (msg->bus == steering_bus)) {
int angle_meas_new = GET_BYTES(msg, 3, 3) & 0x1FFFFU;
angle_meas_new = -1 * to_signed(angle_meas_new, 17);
update_sample(&angle_meas, angle_meas_new);
@@ -152,7 +171,7 @@ static void subaru_rx_hook(const CANPacket_t *msg) {
brake_pressed = (msg->data[7] >> 6) & 1U;
}
if ((msg->addr == MSG_SUBARU_Throttle) && (msg->bus == SUBARU_MAIN_BUS)) {
if ((msg->addr == MSG_SUBARU_Throttle) && (msg->bus == main_bus)) {
gas_pressed = msg->data[4] != 0U;
}
}
@@ -286,6 +305,11 @@ static safety_config subaru_init(uint16_t param) {
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
};
static const CanMsg SUBARU_D_PLATFORM_ANGLE_TX_MSGS[] = {
SUBARU_D_PLATFORM_ANGLE_TX_MSGS()
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
};
static RxCheck subaru_rx_checks[] = {
SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS)
};
@@ -302,6 +326,10 @@ static safety_config subaru_init(uint16_t param) {
SUBARU_LKAS_ANGLE_RX_CHECKS(SUBARU_ALT_BUS, SUBARU_ALT_BUS)
};
static RxCheck subaru_d_platform_angle_rx_checks[] = {
SUBARU_D_PLATFORM_ANGLE_RX_CHECKS()
};
const uint16_t SUBARU_PARAM_GEN2 = 1;
subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
@@ -312,6 +340,9 @@ static safety_config subaru_init(uint16_t param) {
const uint16_t SUBARU_PARAM_LKAS_ANGLE = 16;
subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE);
const uint16_t SUBARU_PARAM_D_PLATFORM = 32;
subaru_d_platform = GET_FLAG(param, SUBARU_PARAM_D_PLATFORM);
#ifdef ALLOW_DEBUG
const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
@@ -319,7 +350,8 @@ static safety_config subaru_init(uint16_t param) {
safety_config ret;
if (subaru_lkas_angle) {
ret = subaru_gen2 ? BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_TX_MSGS) : \
ret = subaru_d_platform ? BUILD_SAFETY_CFG(subaru_d_platform_angle_rx_checks, SUBARU_D_PLATFORM_ANGLE_TX_MSGS) : \
subaru_gen2 ? BUILD_SAFETY_CFG(subaru_gen2_lkas_angle_rx_checks, SUBARU_GEN2_LKAS_ANGLE_TX_MSGS) : \
BUILD_SAFETY_CFG(subaru_lkas_angle_rx_checks, SUBARU_LKAS_ANGLE_TX_MSGS);
} else if (subaru_gen2) {
ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_LONG_TX_MSGS) : \
opendbc_repo/opendbc/safety/tests/common.py
+3
View File
@@ -1026,6 +1026,9 @@ class SafetyTest(SafetyTestBase):
if attr.startswith('TestHyundaiCanfdCCNC') and current_test.startswith('TestSubaruPreglobal'):
tx = list(filter(lambda m: m[0] not in [0x161], tx))
if current_test == 'TestSubaruDPlatformAngleSafety' and attr.startswith('TestRivian'):
tx = list(filter(lambda m: not (m[1] == 2 and m[0] in [0x321, 0x322, 0x323]), tx))
if attr.startswith('TestHyundaiLongitudinal') or attr in ('TestHyundaiSafetyFCEVLong',
'TestHyundaiLongitudinalAolLkasOnEngageSafety',
'TestHyundaiCanCanfdBlendedLongitudinalSafety',
opendbc_repo/opendbc/safety/tests/test_subaru.py
+33
View File
@@ -340,6 +340,39 @@ class TestSubaruGen2AngleStockLongitudinalSafety(TestSubaruStockLongitudinalSafe
TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS, SubaruMsg.ES_LKAS_ANGLE)
class TestSubaruDPlatformAngleSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase):
FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.D_PLATFORM
ALT_MAIN_BUS = SUBARU_ALT_BUS
TX_MSGS = [[SubaruMsg.ES_LKAS_ANGLE, SUBARU_CAM_BUS],
[SubaruMsg.ES_DashStatus, SUBARU_CAM_BUS],
[SubaruMsg.ES_LKAS_State, SUBARU_CAM_BUS],
[SubaruMsg.ES_Infotainment, SUBARU_CAM_BUS],
[SubaruMsg.ES_Distance, SUBARU_ALT_BUS]]
RELAY_MALFUNCTION_ADDRS = {SUBARU_CAM_BUS: (SubaruMsg.ES_LKAS_ANGLE, SubaruMsg.ES_DashStatus,
SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment)}
D_PLATFORM_STATUS_ADDRS = (SubaruMsg.ES_LKAS_ANGLE, SubaruMsg.ES_DashStatus,
SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment)
FWD_BLACKLISTED_ADDRS = {
SUBARU_MAIN_BUS: D_PLATFORM_STATUS_ADDRS,
}
def _torque_driver_msg(self, torque):
return self.packer.make_can_msg_safety("Steering_Torque", SUBARU_CAM_BUS, {"Steer_Torque_Sensor": torque})
def _user_gas_msg(self, gas):
return self.packer.make_can_msg_safety("Throttle", SUBARU_ALT_BUS, {"Throttle_Pedal": gas})
def _angle_cmd_msg(self, angle, enabled, increment_timer=True):
if increment_timer:
self.safety.set_timer(self.angle_cmd_cnt * int(1e6 / self.LATERAL_FREQUENCY))
self.angle_cmd_cnt += 1
values = {"LKAS_Output": angle, "LKAS_Request": enabled, "SET_3": 3}
return self.packer.make_can_msg_safety("ES_LKAS_ANGLE", SUBARU_CAM_BUS, values)
def _angle_meas_msg(self, angle):
return self.packer.make_can_msg_safety("Steering_2", SUBARU_CAM_BUS, {"Steering_Angle": angle})
class TestSubaruGen2LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSubaruGen2TorqueSafetyBase):
FLAGS = SubaruSafetyFlags.LONG | SubaruSafetyFlags.GEN2
TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS) + long_tx_msgs(SUBARU_ALT_BUS) + gen2_long_additional_tx_msgs()
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+28 -6
View File
@@ -799,6 +799,11 @@ PRIUS_CENTER_TAPER_LAT = 0.16
PRIUS_CENTER_TAPER_LAT_WIDTH = 0.035
PRIUS_CENTER_TAPER_SPEED = 18.0
PRIUS_CENTER_TAPER_SPEED_WIDTH = 2.2
PRIUS_CENTER_FRICTION_THRESHOLD_GAIN = 0.08
PRIUS_CENTER_FRICTION_THRESHOLD_LAT = 0.30
PRIUS_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.07
PRIUS_CENTER_FRICTION_THRESHOLD_SPEED = 18.0
PRIUS_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.2
RAV4_PRIME_PHASE_SCALE = 0.12
RAV4_PRIME_TURN_IN_FF_BOOST_LEFT = 0.055
@@ -853,8 +858,10 @@ SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX_SPEED = 27.0
SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX_SPEED_WIDTH = 3.0
LEXUS_IS_PHASE_SCALE = 0.10
LEXUS_IS_UNWIND_FF_REDUCTION_LEFT = 0.06
LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT = 0.12
LEXUS_IS_TURN_IN_FF_BOOST_LEFT = 0.04
LEXUS_IS_TURN_IN_FF_BOOST_RIGHT = 0.04
LEXUS_IS_UNWIND_FF_REDUCTION_LEFT = 0.10
LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT = 0.16
LEXUS_IS_UNWIND_LAT_ONSET = 0.18
LEXUS_IS_UNWIND_LAT_WIDTH = 0.07
LEXUS_IS_UNWIND_SPEED_ONSET = 9.0
@@ -876,14 +883,15 @@ RAM_1500_TRANSITION_LAT_FADE_END = 1.85
# reverses the requested lateral acceleration roughly once per second at
# 29-30 m/s. Fade only rapid, high-speed turn-building torque so the EPS has
# less stored torque to unwind while leaving steady curves and counter-torque.
KONA_NON_SCC_TRANSITION_TAPER_MAX = 0.34
KONA_NON_SCC_TRANSITION_TURN_IN_TAPER_MAX = 0.24
KONA_NON_SCC_TRANSITION_UNWIND_TAPER_MAX = 0.42
KONA_NON_SCC_TRANSITION_SPEED_ONSET = 22.0
KONA_NON_SCC_TRANSITION_SPEED_FULL = 29.0
KONA_NON_SCC_TRANSITION_JERK_ONSET = 0.35
KONA_NON_SCC_TRANSITION_JERK_FULL = 1.20
KONA_NON_SCC_TRANSITION_LAT_FADE_START = 0.45
KONA_NON_SCC_TRANSITION_LAT_FADE_END = 1.60
KONA_NON_SCC_CENTER_TAPER_MAX = 0.20
KONA_NON_SCC_CENTER_TAPER_MAX = 0.14
KONA_NON_SCC_CENTER_TAPER_LAT = 0.28
KONA_NON_SCC_CENTER_TAPER_SPEED_ONSET = 12.0
KONA_NON_SCC_CENTER_TAPER_SPEED_FULL = 24.0
@@ -1117,6 +1125,13 @@ def get_prius_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.
_flm_vehicle_knob("toyota_prius.unwind_threshold_increase_right", PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT),
) *
transition_envelope * unwind_weight)
center_speed_weight = _prius_sigmoid((v_ego - PRIUS_CENTER_FRICTION_THRESHOLD_SPEED) /
PRIUS_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH)
center_lat_weight = _prius_sigmoid((PRIUS_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) /
PRIUS_CENTER_FRICTION_THRESHOLD_LAT_WIDTH)
threshold_scale += (_flm_vehicle_knob("toyota_prius.center_friction_threshold_gain",
PRIUS_CENTER_FRICTION_THRESHOLD_GAIN) *
center_speed_weight * center_lat_weight)
return base_threshold * min(max(threshold_scale, 0.86), 1.16)
@@ -1269,11 +1284,14 @@ def get_lexus_is_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: fl
return 1.0
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / LEXUS_IS_PHASE_SCALE)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
lat_weight = _sigmoid((abs(desired_lateral_accel) - LEXUS_IS_UNWIND_LAT_ONSET) / LEXUS_IS_UNWIND_LAT_WIDTH)
speed_weight = _sigmoid((v_ego - LEXUS_IS_UNWIND_SPEED_ONSET) / LEXUS_IS_UNWIND_SPEED_WIDTH)
turn_in_boost = LEXUS_IS_TURN_IN_FF_BOOST_LEFT if desired_lateral_accel >= 0.0 else LEXUS_IS_TURN_IN_FF_BOOST_RIGHT
reduction = LEXUS_IS_UNWIND_FF_REDUCTION_LEFT if desired_lateral_accel >= 0.0 else LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT
return 1.0 - (reduction * unwind_weight * lat_weight * speed_weight)
return (1.0 + (turn_in_boost * turn_in_weight * lat_weight * speed_weight) -
(reduction * unwind_weight * lat_weight * speed_weight))
def get_subaru_impreza_pid_output_scale(angle_error_deg: float) -> float:
@@ -1298,7 +1316,10 @@ def get_kona_non_scc_highway_transition_output_scale(desired_lateral_accel: floa
[KONA_NON_SCC_TRANSITION_JERK_ONSET, KONA_NON_SCC_TRANSITION_JERK_FULL], [0.0, 1.0]))
lat_weight = 1.0 - float(np.interp(abs(desired_lateral_accel),
[KONA_NON_SCC_TRANSITION_LAT_FADE_START, KONA_NON_SCC_TRANSITION_LAT_FADE_END], [0.0, 1.0]))
return 1.0 - (KONA_NON_SCC_TRANSITION_TAPER_MAX * speed_weight * jerk_weight * lat_weight)
taper_max = (KONA_NON_SCC_TRANSITION_UNWIND_TAPER_MAX
if desired_lateral_accel * desired_lateral_jerk < 0.0
else KONA_NON_SCC_TRANSITION_TURN_IN_TAPER_MAX)
return 1.0 - (taper_max * speed_weight * jerk_weight * lat_weight)
def get_kona_non_scc_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
@@ -3229,6 +3250,7 @@ FLM_SUPPORTED_VEHICLE_KNOBS = {
"toyota_prius.turn_in_threshold_reduction_right": {"profile": "toyota_prius", "min": 0.0, "max": 0.50, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT},
"toyota_prius.unwind_threshold_increase_left": {"profile": "toyota_prius", "min": 0.0, "max": 0.90, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT},
"toyota_prius.unwind_threshold_increase_right": {"profile": "toyota_prius", "min": 0.0, "max": 0.90, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT},
"toyota_prius.center_friction_threshold_gain": {"profile": "toyota_prius", "min": 0.0, "max": 0.20, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True, "defaultValue": PRIUS_CENTER_FRICTION_THRESHOLD_GAIN},
}
selfdrive/controls/lib/longcontrol.py
+3
View File
@@ -292,6 +292,9 @@ class LongControl:
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)
self.vehicle_tuning.trim_volt_cruise_integrator(
self.pid, a_target, error, CS.vEgo, should_stop, has_lead,
)
self._trim_positive_overshoot_integrator(a_target, error, CS)
self.vehicle_tuning.trim_gm_truck_positive_hold_integrator(
self.pid, self.last_output_accel, a_target, error, CS,
selfdrive/controls/lib/longcontrol_vehicle_tunes.py
+15
View File
@@ -34,6 +34,10 @@ TOYOTA_SIENNA_COMFORT_FILTER_MIN_TTC = 4.5
TOYOTA_SIENNA_COMFORT_FILTER_MAX_CLOSING_SPEED = 4.0
TOYOTA_SIENNA_COMFORT_FILTER_MAX_LEAD_BRAKE = 2.5
TOYOTA_SIENNA_COMFORT_FILTER_BRAKE_BYPASS = -2.5
VOLT_CRUISE_INTEGRATOR_MIN_SPEED = 8.0
VOLT_CRUISE_INTEGRATOR_TARGET_MAX = 0.12
VOLT_CRUISE_INTEGRATOR_ERROR_MAX = 0.12
VOLT_CRUISE_INTEGRATOR_LEAK = 0.995
def get_bolt_acc_pedal_friction_bias(output_accel, a_target, v_ego):
@@ -260,6 +264,17 @@ class LongControlVehicleTuning:
bleed = interp(v_ego, [0.0, 4.0, 12.0, 25.0], [0.82, 0.86, 0.90, 0.94])
pid.i *= bleed
def trim_volt_cruise_integrator(self, pid, a_target, error, v_ego, should_stop, has_lead):
"""Release stale negative I during settled open-road speed holding."""
if not self.is_volt or should_stop or has_lead:
return
if v_ego < VOLT_CRUISE_INTEGRATOR_MIN_SPEED:
return
if abs(a_target) > VOLT_CRUISE_INTEGRATOR_TARGET_MAX or abs(error) > VOLT_CRUISE_INTEGRATOR_ERROR_MAX:
return
if pid.i < 0.0:
pid.i *= VOLT_CRUISE_INTEGRATOR_LEAK
def trim_gm_truck_positive_hold_integrator(self, pid, last_output_accel, a_target, error, CS):
if not self.is_gm_stock_truck or pid.i <= 0.0:
return
selfdrive/controls/tests/test_conditional_experimental_mode.py
+26 -1
View File
@@ -51,10 +51,11 @@ def make_cem(*, model_length: float, model_stopped: bool = False, tracking_lead:
return ConditionalExperimentalMode(planner)
def make_sm(traffic_mode_enabled: bool = False):
def make_sm(traffic_mode_enabled: bool = False, car_fingerprint: str = ""):
return {
"carState": SimpleNamespace(standstill=False, leftBlinker=False, rightBlinker=False, steeringAngleDeg=0.0),
"starpilotCarState": SimpleNamespace(trafficModeEnabled=traffic_mode_enabled),
"carParams": SimpleNamespace(carFingerprint=car_fingerprint),
}
@@ -107,6 +108,30 @@ def test_predicted_stop_within_threshold_triggers_stop_light():
assert cem.stop_light_detected
def test_elantra_stop_light_filter_uses_earlier_approach_timing():
v_ego = 36 * CV.MPH_TO_MS
model_length = v_ego * 4.0
default_cem = make_cem(model_length=model_length)
elantra_cem = make_cem(model_length=model_length)
default_sm = make_sm()
elantra_sm = make_sm(car_fingerprint="HYUNDAI_ELANTRA_2021")
default_steps = None
elantra_steps = None
for step in range(1, 30):
default_cem.stop_sign_and_light(v_ego, default_sm, model_time=7.0)
elantra_cem.stop_sign_and_light(v_ego, elantra_sm, model_time=7.0)
if default_steps is None and default_cem.stop_light_detected:
default_steps = step
if elantra_steps is None and elantra_cem.stop_light_detected:
elantra_steps = step
assert elantra_steps is not None
assert default_steps is not None
assert elantra_steps < default_steps
def test_chattering_lead_does_not_trigger_stop_light():
v_ego = 22 * CV.MPH_TO_MS
model_length = v_ego * 4.0
selfdrive/controls/tests/test_latcontrol.py
+11 -3
View File
@@ -635,6 +635,12 @@ class TestLatControl:
def test_prius_friction_curves(self):
base_threshold = get_gm_base_friction_threshold(12.0)
low_speed_center_threshold = get_prius_friction_threshold(8.0, 0.0, 0.0)
high_speed_center_threshold = get_prius_friction_threshold(30.0, 0.0, 0.0)
high_speed_curve_threshold = get_prius_friction_threshold(30.0, 0.8, 0.0)
assert high_speed_center_threshold > get_gm_base_friction_threshold(30.0)
assert high_speed_center_threshold > low_speed_center_threshold
assert high_speed_curve_threshold < high_speed_center_threshold
left_turn_in_threshold = get_prius_friction_threshold(6.0, 0.7, 0.8)
right_turn_in_threshold = get_prius_friction_threshold(6.0, -0.7, -0.8)
left_unwind_threshold = get_prius_friction_threshold(6.0, 0.7, -0.8)
@@ -832,13 +838,13 @@ class TestLatControl:
center_transition = get_kona_non_scc_highway_transition_output_scale(0.4, 1.25, 30.0)
medium_transition = get_kona_non_scc_highway_transition_output_scale(1.1, -1.25, 30.0)
assert center_transition == pytest.approx(0.66)
assert center_transition == pytest.approx(0.76)
assert center_transition < medium_transition < 1.0
assert get_kona_non_scc_highway_transition_output_scale(1.65, 2.5, 30.0) == pytest.approx(1.0)
def test_kona_non_scc_center_taper_curve(self):
assert get_kona_non_scc_center_taper_scale(0.0, 10.0) == pytest.approx(1.0)
assert get_kona_non_scc_center_taper_scale(0.0, 25.0) == pytest.approx(0.80)
assert get_kona_non_scc_center_taper_scale(0.0, 25.0) == pytest.approx(0.86)
assert get_kona_non_scc_center_taper_scale(0.28, 25.0) == pytest.approx(1.0)
assert get_kona_non_scc_center_taper_scale(0.10, 25.0) < get_kona_non_scc_center_taper_scale(0.10, 15.0)
@@ -1018,11 +1024,13 @@ class TestLatControl:
def test_lexus_is_ff_scale_curve(self):
steady_left = get_lexus_is_ff_scale(0.6, 0.0, 22.0)
turn_in_left = get_lexus_is_ff_scale(0.6, 0.5, 22.0)
turn_in_right = get_lexus_is_ff_scale(-0.6, -0.5, 22.0)
unwind_left = get_lexus_is_ff_scale(0.6, -0.5, 22.0)
unwind_right = get_lexus_is_ff_scale(-0.6, 0.5, 22.0)
low_speed_unwind_right = get_lexus_is_ff_scale(-0.6, 0.5, 5.0)
assert steady_left == 1.0
assert turn_in_left == 1.0
assert turn_in_left > steady_left
assert turn_in_right > steady_left
assert unwind_right < unwind_left < steady_left
assert unwind_right < low_speed_unwind_right < 1.0
selfdrive/controls/tests/test_longcontrol.py
+62
View File
@@ -715,6 +715,68 @@ def test_non_interceptor_volt_testing_ground_handoff_freezes_integrator(monkeypa
assert lc.vehicle_tuning.integrator_hold_frames > 0
def test_volt_cruise_integrator_releases_stale_negative_bias():
CP = car.CarParams.new_message()
CP.brand = "gm"
CP.carFingerprint = "CHEVROLET_VOLT_ASCM"
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.0]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.5]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.pid
lc.pid.i = -0.20
CS = car.CarState.new_message(vEgo=18.0, aEgo=0.0, brakePressed=False, gasPressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=0.0,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(),
has_lead=False,
)
assert lc.pid.i > -0.20
assert output_accel > -0.20
@pytest.mark.parametrize("kwargs", [
{"has_lead": True},
{"should_stop": True},
{"a_target": -0.25},
{"aEgo": 0.25},
{"vEgo": 4.0},
])
def test_volt_cruise_integrator_does_not_release_outside_settled_open_road(kwargs):
CP = car.CarParams.new_message()
CP.brand = "gm"
CP.carFingerprint = "CHEVROLET_VOLT_ASCM"
CP.longitudinalTuning.kpBP = [0.0]
CP.longitudinalTuning.kpV = [0.0]
CP.longitudinalTuning.kiBP = [0.0]
CP.longitudinalTuning.kiV = [0.5]
lc = LongControl(CP)
pid = SimpleNamespace(i=-0.20)
v_ego = kwargs.get("vEgo", 18.0)
a_ego = kwargs.get("aEgo", 0.0)
a_target = kwargs.get("a_target", 0.0)
lc.vehicle_tuning.trim_volt_cruise_integrator(
pid,
a_target=a_target,
error=a_target - a_ego,
v_ego=v_ego,
should_stop=kwargs.get("should_stop", False),
has_lead=kwargs.get("has_lead", False),
)
assert pid.i == pytest.approx(-0.20)
def test_negative_target_unwinds_positive_accel_command_after_sign_flip():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
selfdrive/controls/tests/test_starpilot_planner.py
+6 -1
View File
@@ -3,7 +3,7 @@ from pathlib import Path
from types import SimpleNamespace
from openpilot.common.realtime import DT_MDL
from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner
from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner, get_force_stop_jerk_scale
import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_module
@@ -33,6 +33,11 @@ def make_toggles(**overrides):
return SimpleNamespace(**defaults)
def test_force_stop_jerk_scale_is_platform_specific():
assert get_force_stop_jerk_scale(SimpleNamespace(carFingerprint="HYUNDAI_ELANTRA_2021")) == 0.60
assert get_force_stop_jerk_scale(SimpleNamespace(carFingerprint="OTHER_CAR")) == 0.32
def make_sm(planner, *, frame: int, v_ego: float, left_blinker: bool, right_blinker: bool = False, standstill: bool = False):
return FakeSM(frame, {
"radarState": SimpleNamespace(
starpilot/controls/lib/conditional_experimental_mode.py
+13
View File
@@ -64,6 +64,10 @@ class ConditionalExperimentalMode:
TURN_STOP_LIGHT_VETO_MAX_SPEED = 15 * CV.MPH_TO_MS
TURN_STOP_LIGHT_VETO_STEERING_ANGLE = 45.0
STOP_LIGHT_FILTER_TIME_OVERRIDES = {
"HYUNDAI_ELANTRA_2021": 0.25,
}
# ===== END TUNING PARAMETERS =====
# Current active values
@@ -436,6 +440,15 @@ class ConditionalExperimentalMode:
filter_time_curves = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_curves])
filter_time_leads = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_leads])
filter_time_lights = interp(speed_mph, bp, [self.LOW_SPEED_LIGHT_FILTER_TIME, self.LOW_SPEED_LIGHT_FILTER_TIME, tuned_filter_time_lights])
try:
car_params = sm["carParams"]
except (KeyError, IndexError, TypeError, AttributeError):
car_params = None
car_fingerprint = str(getattr(car_params, "carFingerprint", ""))
filter_time_lights = min(
filter_time_lights,
self.STOP_LIGHT_FILTER_TIME_OVERRIDES.get(car_fingerprint, filter_time_lights),
)
lead_clear_filter_time = interp(speed_mph, bp, [self.LEAD_CLEAR_FILTER_TIME_LOW, self.LEAD_CLEAR_FILTER_TIME_LOW, self.LEAD_CLEAR_FILTER_TIME_HIGH])
light_boost = interp(speed_mph, bp, [low_boost, low_boost, tuned_boost])
cap_factor = interp(speed_mph, bp, [low_cap_factor, low_cap_factor, tuned_cap_factor])
starpilot/controls/starpilot_planner.py
+16 -1
View File
@@ -31,6 +31,14 @@ from openpilot.starpilot.controls.lib.weather_checker import WeatherChecker
RADARLESS_TRACK_HOLD_TIME = 0.45
FORCE_STOP_JERK_SCALE = 0.32 # accel-change cost multiplier while forcing_stop (125 -> ~40)
FORCE_STOP_JERK_SCALE_OVERRIDES = {
"HYUNDAI_ELANTRA_2021": 0.60,
}
def get_force_stop_jerk_scale(car_params):
fingerprint = str(getattr(car_params, "carFingerprint", ""))
return FORCE_STOP_JERK_SCALE_OVERRIDES.get(fingerprint, FORCE_STOP_JERK_SCALE)
def _sanitize_json_value(value):
@@ -287,7 +295,14 @@ class StarPilotPlanner:
# While committed to a Force Stop, cut the MPC's accel-change penalty so terminal
# braking can ramp faster. 0.32 lands near 40, what long_mpc uses in blended mode.
jerk_scale = FORCE_STOP_JERK_SCALE if self.starpilot_vcruise.forcing_stop else 1.0
if self.starpilot_vcruise.forcing_stop:
try:
car_params = sm["carParams"]
except (KeyError, IndexError, TypeError, AttributeError):
car_params = None
jerk_scale = get_force_stop_jerk_scale(car_params)
else:
jerk_scale = 1.0
starpilotPlan.accelerationJerk = float(A_CHANGE_COST * self.starpilot_following.acceleration_jerk * jerk_scale)
starpilotPlan.dangerFactor = float(self.starpilot_following.danger_factor)
starpilotPlan.dangerJerk = float(DANGER_ZONE_COST * self.starpilot_following.danger_jerk)
starpilot/navigation/test_destination_store.py
+15
View File
@@ -46,3 +46,18 @@ def test_recent_destinations_dedupe_and_cap():
assert len(updated) == 10
assert [entry["place_name"] for entry in updated].count("Home") == 1
assert updated[-1]["place_name"] == "Old 9"
def test_recent_destinations_retain_coordinates_for_saved_name():
updated = update_recent_destinations("[]", {
"name": "Renamed favorite",
"latitude": 41.881832,
"longitude": -87.623177,
})
assert updated[0] == {
"place_name": "Renamed favorite",
"name": "Renamed favorite",
"latitude": 41.881832,
"longitude": -87.623177,
}
starpilot/starpilot_process.py
+1 -1
View File
@@ -252,7 +252,7 @@ def starpilot_thread():
config_realtime_process(5, Priority.CTRL_LOW)
pm = messaging.PubMaster(["starpilotPlan"])
sm = messaging.SubMaster(["carControl", "carState", "controlsState", "deviceState", "driverMonitoringState",
sm = messaging.SubMaster(["carControl", "carParams", "carState", "controlsState", "deviceState", "driverMonitoringState",
"gpsLocation", "gpsLocationExternal", "liveParameters", "managerState", "modelV2",
"onroadEvents", "pandaStates", "radarState", "selfdriveState", "starpilotCarState",
"starpilotRadarState", "starpilotSelfdriveState", "starpilotModelV2", "starpilotOnroadEvents", "mapdOut"],
starpilot/system/the_galaxy/assets/components/navigation/navigation_destination.js
+20 -6
View File
@@ -407,7 +407,19 @@ export function NavDestination() {
try {
const prev = JSON.parse(data.previousDestinations);
state.previousDestinations = prev.map(d => ({ name: d.place_name }));
state.previousDestinations = prev.map(d => {
const name = cleanSuggestionText(d?.place_name || d?.name || "");
if (!name) return null;
const latitude = Number(d?.latitude);
const longitude = Number(d?.longitude);
return {
...d,
name,
place_name: name,
...(Number.isFinite(latitude) && Number.isFinite(longitude) ? { latitude, longitude } : {})
};
}).filter(Boolean);
state.suggestions = JSON.stringify(state.previousDestinations);
} catch { }
try {
@@ -623,12 +635,14 @@ export function NavDestination() {
async function selectSuggestion(sugg) {
const label = sugg.full_address || sugg.name || sugg.address || "Unnamed Location";
let coords;
if (sugg.routeId) {
const savedLatitude = Number(sugg.latitude);
const savedLongitude = Number(sugg.longitude);
if (Number.isFinite(savedLatitude) && Number.isFinite(savedLongitude)) {
initiateNavigation({
name: sugg.name,
longitude: sugg.longitude,
latitude: sugg.latitude,
routeId: sugg.routeId
name: sugg.name || label,
longitude: savedLongitude,
latitude: savedLatitude,
routeId: sugg.routeId || null
});
return;
}