mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-05 16:26:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

HE'S BACK

Browse Source
This commit is contained in:
firestar5683
2026-08-01 14:36:56 -05:00
parent 205e4b3a43
commit 8436e41731
119 changed files with 1753 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params_keys.h
+1
View File
@@ -229,6 +229,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"ConditionalExperimental", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
{"CurvatureData", {PERSISTENT | DONT_LOG, JSON, "{}", "{}"}},
{"CurveSpeedController", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
{"CurveSpeedControllerNoLead", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
{"CustomAlerts", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
{"CustomAccelProfile", {PERSISTENT, BOOL, "0", "0", 3}},
{"CustomAccelProfileInitialized", {PERSISTENT, BOOL, "0", "0", 3}},
docs/CARS.md
+4
View File
@@ -231,13 +231,17 @@ A supported vehicle is one that just works when you install a comma device. All
|SEAT|Ateca 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=SEAT Ateca 2016-23">Buy Here</a></sub></details>|||
|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=SEAT Leon 2014-20">Buy Here</a></sub></details>|||
|Subaru|Ascent 2019-21|All[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Ascent 2019-21">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|Subaru|Ascent 2023|All[<sup>6</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru D connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Ascent 2023">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Crosstrek 2018-19">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Crosstrek 2020-23">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|Subaru|Crosstrek 2025|All[<sup>6</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru D connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Crosstrek 2025">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|Subaru|Forester 2019-21|All[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Forester 2019-21">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|Subaru|Forester 2022-24|All[<sup>6</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru C connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Forester 2022-24">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|Subaru|Impreza 2017-19|EyeSight Driver Assistance[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Impreza 2017-19">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|Subaru|Impreza 2020-22|EyeSight Driver Assistance[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Impreza 2020-22">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|Subaru|Legacy 2020-22|All[<sup>6</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru B connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Legacy 2020-22">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|Subaru|Outback 2020-22|All[<sup>6</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru B connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Outback 2020-22">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|Subaru|Outback 2023|All[<sup>6</sup>](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru D connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Outback 2023">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|Subaru|XV 2018-19|EyeSight Driver Assistance[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru XV 2018-19">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|Subaru|XV 2020-21|EyeSight Driver Assistance[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru XV 2020-21">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|Škoda|Fabia 2022-23[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Fabia 2022-23">Buy Here</a></sub></details>[<sup>15</sup>](#footnotes)|||
launch_chffrplus.sh
-2
View File
@@ -51,8 +51,6 @@ function agnos_init {
# StarPilot variables
sudo chmod 0777 /cache
sudo rm -f /data/misc/display/color_cal/color_cal /data/misc/display/color_cal/source.sha256
# Check if AGNOS update is required
AGNOS_CURRENT_VERSION="$(< /VERSION)"
AGNOS_UPDATE_REQUIRED=1
opendbc_repo/opendbc/car/gm/carcontroller.py
+9
View File
@@ -232,6 +232,14 @@ def shape_truck_positive_accel(accel: float, v_ego: float, enabled: bool,
return accel
def shape_truck_pitch_accel(pitch_accel: float, v_ego: float, enabled: bool) -> float:
if not enabled:
return pitch_accel
scale = float(np.interp(v_ego, [8.0, 15.0, 25.0, 35.0], [0.60, 0.45, 0.30, 0.25]))
return pitch_accel * scale
def shape_truck_friction_brake(apply_brake: int, accel_cmd: float, stopping: bool, active: bool) -> tuple[int, bool]:
if apply_brake <= 0:
return 0, False
@@ -1016,6 +1024,7 @@ class CarController(CarControllerBase):
getattr(self.CP, "transmissionType", None) == TransmissionType.automatic and
not self.CP.enableGasInterceptorDEPRECATED
)
accel_due_to_pitch = shape_truck_pitch_accel(accel_due_to_pitch, CS.out.vEgo, truck_long_smoothing)
accel_input = actuators.accel + accel_due_to_pitch
if truck_long_smoothing:
accel_input = shape_truck_positive_accel(
opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py
+10
View File
@@ -55,6 +55,7 @@ from opendbc.car.gm.carcontroller import (
get_stock_cc_active_for_cancel,
shape_bolt_acc_pedal_low_speed_friction,
shape_truck_friction_brake,
shape_truck_pitch_accel,
shape_truck_positive_accel,
should_use_fixed_stopping_brake,
should_activate_auto_hold,
@@ -861,6 +862,15 @@ def test_shape_truck_positive_accel_does_not_relax_without_speed_error():
assert no_error == base
def test_shape_truck_pitch_accel_attenuates_highway_grade_feedforward():
assert shape_truck_pitch_accel(-0.30, 30.0, True) == pytest.approx(-0.0825)
assert shape_truck_pitch_accel(0.30, 30.0, True) == pytest.approx(0.0825)
def test_shape_truck_pitch_accel_is_inactive_without_truck_tuning():
assert shape_truck_pitch_accel(-0.30, 30.0, False) == pytest.approx(-0.30)
def test_shape_truck_friction_brake_suppresses_boundary_chatter():
assert shape_truck_friction_brake(14, -0.3, False, False) == (0, False)
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+9 -1
View File
@@ -686,7 +686,15 @@ class CarController(CarControllerBase):
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
hud_control)
if can_canfd_blended:
if can_canfd_blended and self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING:
can_sends.extend(hyundaicanfd.create_steering_messages(
self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_torque, 0.0,
))
if self.frame % 5 == 0:
can_sends.append(hyundaicanfd.create_suppress_lfa(
self.packer, self.CAN, CS.lfa_block_msg, False,
))
elif can_canfd_blended:
can_sends.extend(hyundaican.create_lkas11_can_canfd_blended(self.packer, self.frame, self.CP, apply_torque, apply_steer_req,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
opendbc_repo/opendbc/car/hyundai/carstate.py
+37 -2
View File
@@ -123,6 +123,7 @@ class CarState(CarStateBase):
self.msg_162 = {}
self.msg_1b5 = {}
self.msg_364 = {}
self.lfa_block_msg = {}
self.stock_lkas_msg = {}
self.stock_lfa_msg = {}
self.stock_lfahda_cluster_msg = {}
@@ -332,7 +333,10 @@ class CarState(CarStateBase):
ret.cruiseState.speed = cp_cruise.vl[scc_msg]["VSetDis"] * speed_conv
if self.CP.flags & HyundaiFlags.CAN_CANFD_BLENDED:
self.msg_364 = copy.copy(cp_cam.vl["ALERTS_364"])
if self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING:
self.lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x2a4"])
else:
self.msg_364 = copy.copy(cp_cam.vl["ALERTS_364"])
# TODO: Find brake pressure
ret.brake = 0
@@ -391,7 +395,10 @@ class CarState(CarStateBase):
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
# save the entire LKAS11 and CLU11
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
if self.CP.flags & HyundaiFlags.CAN_CANFD_BLENDED and self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING:
self.lkas11 = {}
else:
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
self.clu11 = copy.copy(cp.vl["CLU11"])
self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE
prev_cruise_buttons = self.cruise_buttons[-1]
@@ -634,6 +641,34 @@ class CarState(CarStateBase):
if CP.flags & HyundaiFlags.CANFD:
return self.get_can_parsers_canfd(CP)
if CP.flags & HyundaiFlags.CAN_CANFD_BLENDED and CP.flags & HyundaiFlags.CANFD_LKA_STEERING:
msgs = [
("MDPS12", 100),
("TCS11", 100),
("TCS13", 50),
("TCS15", 10),
("CLU11", 50),
("CLU15", 5),
("ESP12", 100),
("CGW1", 10),
("CGW2", 5),
("WHL_SPD11", 50),
("SAS11", 100),
("SCC12", 50),
("EMS12", 100),
("EMS16", 100),
("LVR12", 100),
("BCM_PO_11", 0),
("CLU13", 0),
]
if CP.enableBsm:
msgs.append(("LCA11", 20))
return {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], msgs, CanBus(CP).ECAN),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [("CAM_0x2a4", 20)], CanBus(CP).CAM),
}
msgs = [
("BCM_PO_11", 0),
("CLU13", 0),
opendbc_repo/opendbc/car/hyundai/interface.py
+10 -2
View File
@@ -99,6 +99,10 @@ class CarInterface(CarInterfaceBase):
# "LFA steering" if camera directly sends LFA to the MDPS
cam_can = CanBus(None, fingerprint).CAM
lka_steering = 0x50 in fingerprint[cam_can] or 0x110 in fingerprint[cam_can]
if ret.flags & HyundaiFlags.CAN_CANFD_BLENDED:
lka_steering = Ecu.adas in [fw.ecu for fw in car_fw] or 0x50 in fingerprint[cam_can]
if lka_steering:
ret.flags |= HyundaiFlags.CANFD_LKA_STEERING.value
CAN = CanBus(None, fingerprint, lka_steering)
if ret.flags & HyundaiFlags.CANFD:
@@ -173,6 +177,8 @@ class CarInterface(CarInterfaceBase):
else:
# Shared configuration for non CAN-FD cars
ret.alphaLongitudinalAvailable = candidate not in UNSUPPORTED_LONGITUDINAL_CAR or candidate in LEGACY_LONGITUDINAL_CAR
if ret.flags & HyundaiFlags.CAN_CANFD_BLENDED and ret.flags & HyundaiFlags.CANFD_LKA_STEERING:
ret.alphaLongitudinalAvailable = False
ret.enableBsm = 0x58b in fingerprint[CAN.ECAN]
# Send LFA message on cars with HDA
@@ -201,6 +207,8 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON.value
if ret.flags & HyundaiFlags.CAN_CANFD_BLENDED:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CAN_CANFD_BLENDED.value
if ret.flags & HyundaiFlags.CANFD_LKA_STEERING:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_LKA_STEERING.value
if hyundai_cancel_button_enables_cruise(candidate):
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANCEL_BTN_ENABLE.value
@@ -263,8 +271,8 @@ class CarInterface(CarInterfaceBase):
if candidate == CAR.HYUNDAI_ELANTRA_2021:
ret.longitudinalActuatorDelay = 0.22
ret.stopAccel = -1.5
ret.stoppingDecelRate = 0.5
ret.stopAccel = -0.85
ret.stoppingDecelRate = 0.35
if candidate == CAR.HYUNDAI_ELANTRA_HEV_2024:
ret.longitudinalActuatorDelay = 0.22
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+94 -4
View File
@@ -22,7 +22,7 @@ from opendbc.car.hyundai import hyundaican, hyundaicanfd
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.radar_interface import MRREVO14F_RADAR_START_ADDR, MRR30_RADAR_START_ADDR, MRR35_RADAR_START_ADDR, \
RADAR_START_ADDR, get_radar_track_config
from opendbc.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, \
from opendbc.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, DATELESS_FUZZY_CARS, \
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, \
@@ -56,6 +56,7 @@ NO_DATES_PLATFORMS = {
CAR.KIA_OPTIMA_G4_FL,
CAR.KIA_SORENTO,
CAR.HYUNDAI_KONA,
CAR.HYUNDAI_KONA_NON_SCC,
CAR.HYUNDAI_KONA_EV,
CAR.HYUNDAI_KONA_EV_2022,
CAR.HYUNDAI_KONA_HEV,
@@ -369,6 +370,55 @@ class TestHyundaiFingerprint:
assert palisade_2023.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_CANFD_BLENDED
assert palisade_2023.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANCEL_BTN_ENABLE
def test_palisade_telluride_hda2_uses_mixed_can_layout(self):
fingerprint = gen_empty_fingerprint()
fingerprint[2][0x50] = 16
car_fw = [CarParams.CarFw(ecu=Ecu.adas, fwVersion=b"", address=0x730, brand="hyundai")]
CP = CarInterface.get_params(CAR.HYUNDAI_PALISADE_2023, fingerprint, car_fw, True, False, False, None)
can_bus = CanBus(CP)
parsers = CarState(CP, None).get_can_parsers(CP)
assert CP.flags & HyundaiFlags.CAN_CANFD_BLENDED
assert CP.flags & HyundaiFlags.CANFD_LKA_STEERING
assert not CP.alphaLongitudinalAvailable
assert not CP.openpilotLongitudinalControl
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_CANFD_BLENDED
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANFD_LKA_STEERING
assert can_bus.ACAN == 0
assert can_bus.ECAN == 1
assert parsers[Bus.pt].bus == 1
assert parsers[Bus.cam].bus == 2
assert CarControllerParams(CP).STEER_MAX == 384
def test_palisade_telluride_hda2_sends_lkas_and_camera_suppression(self):
fingerprint = gen_empty_fingerprint()
fingerprint[2][0x50] = 16
car_fw = [CarParams.CarFw(ecu=Ecu.adas, fwVersion=b"", address=0x730, brand="hyundai")]
CP = CarInterface.get_params(CAR.HYUNDAI_PALISADE_2023, fingerprint, car_fw, False, False, False, None)
controller = CarController(DBC[CP.carFingerprint], CP)
controller.frame = 0
hud_control = SimpleNamespace(
visualAlert=CarControl.HUDControl.VisualAlert.none,
leftLaneVisible=True,
rightLaneVisible=True,
leftLaneDepart=False,
rightLaneDepart=False,
)
lfa_block_msg = {f"BYTE{i}": 0 for i in range(3, 24) if i != 7}
lfa_block_msg["COUNTER"] = 0
CS = SimpleNamespace(lfa_block_msg=lfa_block_msg, redneck_send_button=Buttons.NONE)
CC = SimpleNamespace(enabled=True, cruiseControl=SimpleNamespace(cancel=False, resume=False))
actuators = SimpleNamespace(longControlState=LongCtrlState.off)
msgs = controller.create_can_msgs(True, 100, False, 0.0, 0.0, False, hud_control, actuators, CS, CC, 2)
msg_addrs_buses = {(addr, bus) for addr, _, bus in msgs}
assert (0x50, 0) in msg_addrs_buses
assert (0x2A4, 0) in msg_addrs_buses
assert not ({0x340, 0x364} & {addr for addr, _, _ in msgs})
@pytest.mark.parametrize("candidate", (CAR.HYUNDAI_ELANTRA_2024, CAR.HYUNDAI_ELANTRA_HEV_2024))
def test_hyundai_can_refresh_platforms_use_refresh_dbc_and_safety_param(self, candidate):
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], False, False, False, None)
@@ -397,6 +447,30 @@ class TestHyundaiFingerprint:
palisade_2023 = CarInterface.get_params(CAR.HYUNDAI_PALISADE_2023, gen_empty_fingerprint(), [], True, False, False, None)
assert palisade_2023.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON
def test_sonata_hybrid_aol_main_lkas_sync_is_scoped(self):
toggles = SimpleNamespace(always_on_lateral_lkas=True, main_cruise_aol_toggle=True)
sonata_hybrid_cp = CarInterface.get_params(CAR.HYUNDAI_SONATA_HYBRID, gen_empty_fingerprint(), [], False, False, False, None)
sonata_hybrid_fpcp = CarInterface.get_starpilot_params(
CAR.HYUNDAI_SONATA_HYBRID, gen_empty_fingerprint(), [], sonata_hybrid_cp, toggles,
)
assert sonata_hybrid_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC
sonata_cp = CarInterface.get_params(CAR.HYUNDAI_SONATA, gen_empty_fingerprint(), [], False, False, False, None)
sonata_fpcp = CarInterface.get_starpilot_params(CAR.HYUNDAI_SONATA, gen_empty_fingerprint(), [], sonata_cp, toggles)
assert not (sonata_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC)
disabled_toggles = SimpleNamespace(always_on_lateral_lkas=True, main_cruise_aol_toggle=False)
disabled_fpcp = CarInterface.get_starpilot_params(
CAR.HYUNDAI_SONATA_HYBRID, gen_empty_fingerprint(), [], sonata_hybrid_cp, disabled_toggles,
)
assert not (disabled_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC)
minimal_fpcp = CarInterface.get_starpilot_params(
CAR.HYUNDAI_SONATA_HYBRID, gen_empty_fingerprint(), [], sonata_hybrid_cp, SimpleNamespace(),
)
assert not (minimal_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC)
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
@@ -694,8 +768,8 @@ class TestHyundaiFingerprint:
CP = CarInterface.get_params(CAR.HYUNDAI_ELANTRA_2021, gen_empty_fingerprint(), [], True, False, False, toggles)
assert CP.longitudinalActuatorDelay == pytest.approx(0.22)
assert CP.stopAccel == pytest.approx(-1.5)
assert CP.stoppingDecelRate == pytest.approx(0.5)
assert CP.stopAccel == pytest.approx(-0.85)
assert CP.stoppingDecelRate == pytest.approx(0.35)
def test_elantra_hev_2024_longitudinal_delay_matches_observed_response(self):
toggles = get_test_toggles()
@@ -778,6 +852,22 @@ class TestHyundaiFingerprint:
assert exact
assert CAR.HYUNDAI_KONA_NON_SCC in matches
def test_kona_non_scc_fw_matches_with_unstable_transmission_padding(self):
route_fw = {
(Ecu.eps, 0x7d4): b'\xf1\x00OS MDPS C 1.00 1.05 56310/J9500 4OSDC105',
(Ecu.fwdCamera, 0x7c4): b'\xf1\x00OS9 LKAS AT AUS RHD 1.00 1.00 95740-J9200 g30',
(Ecu.fwdRadar, 0x7d0): b'\xf1\x00OS__ FCA --CUP 1.00 1.00 95655-J9100 ',
(Ecu.transmission, 0x7e1): b'\xf1\x006U2V0_C2\x00\x006U2V1051\x00\x00DOS4T16AS2\x0e\xdc_\xa7',
}
car_fw = [
CarParams.CarFw(ecu=ecu, fwVersion=version, address=address, subAddress=0, brand="hyundai")
for (ecu, address), version in route_fw.items()
]
exact, matches = match_fw_to_car(car_fw, "", log=False)
assert not exact
assert matches == {CAR.HYUNDAI_KONA_NON_SCC}
def test_kia_forte_2019_non_scc_does_not_require_fca11_or_scc12(self):
toggles = get_test_toggles()
CP = CarInterface.get_params(CAR.KIA_FORTE_2019_NON_SCC, gen_empty_fingerprint(), [], True, False, False, toggles)
@@ -2707,7 +2797,7 @@ class TestHyundaiFingerprint:
CAR.GENESIS_G70_2020,
}
excluded_platforms |= CANFD_CAR - EV_CAR - CANFD_FUZZY_WHITELIST # shared platform codes
excluded_platforms |= NO_DATES_PLATFORMS # date codes are required to match
excluded_platforms |= NO_DATES_PLATFORMS - DATELESS_FUZZY_CARS
platforms_with_shared_codes = set()
for platform, fw_by_addr in FW_VERSIONS.items():
opendbc_repo/opendbc/car/hyundai/values.py
+16 -6
View File
@@ -77,11 +77,14 @@ class CarControllerParams:
self.STEER_DELTA_DOWN = 3
elif CP.flags & HyundaiFlags.CAN_CANFD_BLENDED:
self.STEER_MAX = 404
self.STEER_DRIVER_ALLOWANCE = 50
self.STEER_THRESHOLD = 150
self.STEER_DELTA_UP = 2
self.STEER_DELTA_DOWN = 3
if CP.flags & HyundaiFlags.CANFD_LKA_STEERING:
self.STEER_MAX = 384
else:
self.STEER_MAX = 404
self.STEER_DRIVER_ALLOWANCE = 50
self.STEER_THRESHOLD = 150
self.STEER_DELTA_UP = 2
self.STEER_DELTA_DOWN = 3
# Default for most HKG
else:
@@ -108,6 +111,7 @@ class HyundaiSafetyFlags(IntFlag):
class HyundaiStarPilotSafetyFlags(IntFlag):
AOL_MAIN_LKAS_SYNC = 32
HAS_LDA_BUTTON = 1024
AOL_LKAS_ON_ENGAGE = 2048
@@ -476,8 +480,12 @@ class CAR(Platforms):
[
HyundaiCarDocs("Hyundai Palisade (without HDA II) 2023-25", "Highway Driving Assist",
car_parts=CarParts.common([CarHarness.hyundai_a])),
HyundaiCarDocs("Hyundai Palisade (with HDA II) 2023-24", "Highway Driving Assist II",
car_parts=CarParts.common([CarHarness.hyundai_r])),
HyundaiCarDocs("Kia Telluride (without HDA II) 2023-25", "Highway Driving Assist",
car_parts=CarParts.common([CarHarness.hyundai_l])),
HyundaiCarDocs("Kia Telluride (with HDA II) 2023-24", "Highway Driving Assist II",
car_parts=CarParts.common([CarHarness.hyundai_p])),
],
HYUNDAI_PALISADE.specs,
flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.CAN_CANFD_BLENDED | HyundaiFlags.RADAR_SCC,
@@ -1037,7 +1045,7 @@ def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str
if not any(found_platform_code in expected_platform_codes for found_platform_code in found_platform_codes):
break
if ecu[0] in DATE_FW_ECUS:
if ecu[0] in DATE_FW_ECUS and candidate not in DATELESS_FUZZY_CARS:
# If ECU can have a FW date, require it to exist
# (this excludes candidates in the database without dates)
if not len(expected_dates) or not len(found_dates):
@@ -1085,6 +1093,8 @@ PLATFORM_CODE_ECUS = [Ecu.fwdRadar, Ecu.fwdCamera, Ecu.eps]
# TODO: there are date codes in the ABS firmware versions in hex
DATE_FW_ECUS = [Ecu.fwdCamera]
DATELESS_FUZZY_CARS = {CAR.HYUNDAI_KONA_NON_SCC}
# Note: an ECU on CAN FD cars may sometimes send 0x30080aaaaaaaaaaa (flow control continue) while we
# are attempting to query ECUs. This currently does not seem to affect fingerprinting from the camera
FW_QUERY_CONFIG = FwQueryConfig(
opendbc_repo/opendbc/car/interfaces.py
+4
View File
@@ -254,6 +254,10 @@ class CarInterfaceBase(ABC):
# LKASButtonControl == 9 means BUTTON_FUNCTIONS["AOL_TOGGLE"] in starpilot_variables.
if params.get_bool("AlwaysOnLateral") and params.get_int("LKASButtonControl") == 9:
fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value
if candidate == HYUNDAI.HYUNDAI_SONATA_HYBRID and getattr(starpilot_toggles, "always_on_lateral_lkas", False) and \
getattr(starpilot_toggles, "main_cruise_aol_toggle", False):
fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC.value
elif platform in TOYOTA:
fp_ret.canUsePedal = not CP.autoResumeSng
fp_ret.canUseSDSU = candidate not in UNSUPPORTED_DSU_CAR and candidate not in TSS2_CAR
opendbc_repo/opendbc/car/subaru/carcontroller.py
+69 -26
View File
@@ -1,10 +1,11 @@
import numpy as np
from opendbc.can import CANPacker
from opendbc.car import Bus, DT_CTRL, make_tester_present_msg
from opendbc.car.lateral import apply_driver_steer_torque_limits, common_fault_avoidance
from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance
from opendbc.car.interfaces import CarControllerBase
from opendbc.car.subaru import subarucan
from opendbc.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags
from opendbc.car.vehicle_model import VehicleModel
# FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and
# involves the total steering angle change rather than rate, but these limits work well for now
@@ -15,10 +16,17 @@ _SNG_ACC_MIN_DIST = 3
_SNG_ACC_MAX_DIST = 4.5
def get_safety_CP():
from opendbc.car.subaru.interface import CarInterface
return CarInterface.get_non_essential_params("SUBARU_ASCENT")
class CarController(CarControllerBase):
def __init__(self, dbc_names, CP):
super().__init__(dbc_names, CP)
self.apply_torque_last = 0
self.apply_steer_last = 0
self.driver_override = False
self.cruise_button_prev = 0
self.steer_rate_counter = 0
@@ -26,10 +34,62 @@ class CarController(CarControllerBase):
self.p = CarControllerParams(CP)
self.packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
if CP.flags & SubaruFlags.LKAS_ANGLE:
self.VM = VehicleModel(get_safety_CP())
self.prev_close_distance = 0
self.epb_resume_frames_remaining = -1
self.last_standstill_frame = 0
def lateral_angle(self, CC, CS):
abs_torque = abs(CS.out.steeringTorque)
if abs_torque > self.p.STEER_OVERRIDE_TORQUE_HIGH:
self.driver_override = True
elif abs_torque < self.p.STEER_OVERRIDE_TORQUE_LOW:
self.driver_override = False
lat_active = CC.latActive and not self.driver_override
apply_steer = apply_steer_angle_limits_vm(
CC.actuators.steeringAngleDeg,
self.apply_steer_last,
CS.out.vEgoRaw,
CS.out.steeringAngleDeg,
lat_active,
self.p,
self.VM,
)
if not lat_active:
apply_steer = CS.out.steeringAngleDeg
self.apply_steer_last = apply_steer
return subarucan.create_steering_control_angle(self.packer, apply_steer, lat_active)
def lateral_torque(self, CC, CS):
apply_torque = int(round(CC.actuators.torque * self.p.STEER_MAX))
apply_torque = apply_driver_steer_torque_limits(apply_torque, self.apply_torque_last, CS.out.steeringTorque, self.p)
if not CC.latActive:
apply_torque = 0
self.apply_torque_last = apply_torque
if self.CP.flags & SubaruFlags.PREGLOBAL:
return subarucan.create_preglobal_steering_control(
self.packer, self.frame // self.p.STEER_STEP, apply_torque, CC.latActive,
)
apply_steer_req = CC.latActive
if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED:
self.steer_rate_counter, apply_steer_req = common_fault_avoidance(
abs(CS.out.steeringRateDeg) > MAX_STEER_RATE,
apply_steer_req,
self.steer_rate_counter,
MAX_STEER_RATE_FRAMES,
)
return subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req)
def update(self, CC, CS, now_nanos, starpilot_toggles):
actuators = CC.actuators
hud_control = CC.hudControl
@@ -39,30 +99,10 @@ class CarController(CarControllerBase):
# *** steering ***
if (self.frame % self.p.STEER_STEP) == 0:
apply_torque = int(round(actuators.torque * self.p.STEER_MAX))
# limits due to driver torque
new_torque = int(round(apply_torque))
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.p)
if not CC.latActive:
apply_torque = 0
if self.CP.flags & SubaruFlags.PREGLOBAL:
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_torque, CC.latActive))
if self.CP.flags & SubaruFlags.LKAS_ANGLE:
can_sends.append(self.lateral_angle(CC, CS))
else:
apply_steer_req = CC.latActive
if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED:
# Steering rate fault prevention
self.steer_rate_counter, apply_steer_req = \
common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req,
self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
can_sends.append(subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req))
self.apply_torque_last = apply_torque
can_sends.append(self.lateral_torque(CC, CS))
# *** stop and go ***
subaru_sng_manual_parking_brake = getattr(starpilot_toggles, "subaru_sng_manual_parking_brake", False)
@@ -162,8 +202,11 @@ class CarController(CarControllerBase):
can_sends.append(subarucan.create_es_static_2(self.packer))
new_actuators = actuators.as_builder()
new_actuators.torque = self.apply_torque_last / self.p.STEER_MAX
new_actuators.torqueOutputCan = self.apply_torque_last
if self.CP.flags & SubaruFlags.LKAS_ANGLE:
new_actuators.steeringAngleDeg = self.apply_steer_last
else:
new_actuators.torque = self.apply_torque_last / self.p.STEER_MAX
new_actuators.torqueOutputCan = self.apply_torque_last
self.frame += 1
return new_actuators, can_sends
opendbc_repo/opendbc/car/subaru/carstate.py
+13 -6
View File
@@ -61,11 +61,16 @@ class CarState(CarStateBase):
can_gear = int(cp_transmission.vl["Transmission"]["Gear"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
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
else:
ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
steering_updated = len(cp.vl_all["Steering_Torque"]["Steering_Angle"]) > 0
if not (self.CP.flags & SubaruFlags.PREGLOBAL):
# 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, cp.vl["Steering_Torque"]["COUNTER"])
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"]
@@ -74,7 +79,11 @@ class CarState(CarStateBase):
ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold
cp_cruise = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp
if self.CP.flags & SubaruFlags.HYBRID:
cp_es_brake = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp_cam
if self.CP.flags & SubaruFlags.LKAS_ANGLE:
ret.cruiseState.enabled = cp_es_brake.vl["ES_Status"]['Cruise_Activated'] != 0
ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0
elif self.CP.flags & SubaruFlags.HYBRID:
ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0
ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0
else:
@@ -104,9 +113,7 @@ class CarState(CarStateBase):
(cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2)
self.es_lkas_state_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
cp_es_brake = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp_cam
self.es_brake_msg = copy.copy(cp_es_brake.vl["ES_Brake"])
cp_es_status = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp_cam
# TODO: Hybrid cars don't have ES_Distance, need a replacement
if not (self.CP.flags & SubaruFlags.HYBRID):
@@ -114,7 +121,7 @@ class CarState(CarStateBase):
ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \
(cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0)
self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"])
self.es_status_msg = copy.copy(cp_es_brake.vl["ES_Status"])
self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"])
if not (self.CP.flags & SubaruFlags.HYBRID):
opendbc_repo/opendbc/car/subaru/fingerprints.py
+14
View File
@@ -244,6 +244,20 @@ FW_VERSIONS = {
b'\xf4!`0\x07',
],
},
CAR.SUBARU_CROSSTREK_2025: {
(Ecu.abs, 0x7b0, None): [
b'\xa2 $\x15\x05',
b'\xa2 $\x17\x06',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x1d!\x08\x00F\x14!\x08\x00=',
b'\x1b!\x08\x00D\x11!\x08\x01;',
],
(Ecu.engine, 0x7a2, None): [
b'\x04"cP\x07',
b'\xe8!cp\x07',
],
},
CAR.SUBARU_FORESTER: {
(Ecu.abs, 0x7b0, None): [
b'\xa3 \x18\x14\x00',
opendbc_repo/opendbc/car/subaru/interface.py
+8 -5
View File
@@ -18,7 +18,7 @@ class CarInterface(CarInterfaceBase):
# - replacement for ES_Distance so we can cancel the cruise control
# - to find the Cruise_Activated bit from the car
# - proper panda safety setup (use the correct cruise_activated bit, throttle from Throttle_Hybrid, etc)
ret.dashcamOnly = bool(ret.flags & (SubaruFlags.PREGLOBAL | SubaruFlags.LKAS_ANGLE | SubaruFlags.HYBRID))
ret.dashcamOnly = bool(ret.flags & (SubaruFlags.PREGLOBAL | SubaruFlags.HYBRID))
ret.autoResumeSng = not (ret.flags & SubaruFlags.GLOBAL_GEN2 or ret.flags & SubaruFlags.HYBRID)
# Detect infotainment message sent from the camera
@@ -33,16 +33,19 @@ class CarInterface(CarInterfaceBase):
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
ret.steerLimitTimer = 0.4
ret.steerActuatorDelay = 0.1
if ret.flags & SubaruFlags.LKAS_ANGLE:
ret.steerControlType = structs.CarParams.SteerControlType.angle
else:
if not (ret.flags & SubaruFlags.LKAS_ANGLE):
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if candidate in (CAR.SUBARU_ASCENT, CAR.SUBARU_ASCENT_2023):
if ret.flags & SubaruFlags.LKAS_ANGLE:
ret.steerControlType = structs.CarParams.SteerControlType.angle
elif candidate in (CAR.SUBARU_ASCENT, CAR.SUBARU_ASCENT_2023):
ret.steerActuatorDelay = 0.3 # end-to-end angle controller
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kf = 0.00003
opendbc_repo/opendbc/car/subaru/subarucan.py
+2 -2
View File
@@ -13,9 +13,9 @@ 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_torque, steer_req):
def create_steering_control_angle(packer, apply_angle, steer_req):
values = {
"LKAS_Output": apply_torque,
"LKAS_Output": apply_angle,
"LKAS_Request": steer_req,
"SET_3": 3
}
opendbc_repo/opendbc/car/subaru/tests/test_subaru.py
+45 -1
View File
@@ -1,8 +1,12 @@
from types import SimpleNamespace
import pytest
from opendbc.car.subaru.carcontroller import CarController
from opendbc.car.subaru.fingerprints import FW_VERSIONS
from opendbc.car.subaru.values import SubaruFlags
from opendbc.car.subaru.interface import CarInterface
from opendbc.car.subaru.values import CAR, SubaruFlags, SubaruSafetyFlags
from opendbc.car.structs import CarParams
def make_sng_controller(flags=0, prev_close_distance=4.0):
@@ -61,3 +65,43 @@ class TestSubaruFingerprint:
fw_size = len(fws[0])
for fw in fws:
assert len(fw) == fw_size, f"{platform} {ecu}: {len(fw)} {fw_size}"
ANGLE_PLATFORMS = (
CAR.SUBARU_FORESTER_2022,
CAR.SUBARU_OUTBACK_2023,
CAR.SUBARU_ASCENT_2023,
CAR.SUBARU_CROSSTREK_2025,
)
@pytest.mark.parametrize("platform", ANGLE_PLATFORMS)
def test_angle_platform_params(platform):
CP = CarInterface.get_non_essential_params(platform)
assert CP.flags & SubaruFlags.LKAS_ANGLE
assert CP.steerControlType == CarParams.SteerControlType.angle
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.LKAS_ANGLE
assert not CP.dashcamOnly
assert not CP.alphaLongitudinalAvailable
def test_torque_platform_does_not_enable_angle_safety():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_IMPREZA_2020)
assert not (CP.flags & SubaruFlags.LKAS_ANGLE)
assert CP.steerControlType == CarParams.SteerControlType.torque
assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.LKAS_ANGLE)
def test_angle_controller_tracks_driver_override():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025)
controller = CarController({}, CP)
CC = SimpleNamespace(latActive=True, actuators=SimpleNamespace(steeringAngleDeg=15.0))
CS = SimpleNamespace(out=SimpleNamespace(vEgoRaw=15.0, steeringAngleDeg=2.0, steeringTorque=250.0))
msg = controller.lateral_angle(CC, CS)
assert controller.driver_override
assert controller.apply_steer_last == CS.out.steeringAngleDeg
assert msg[0] == 0x124
opendbc_repo/opendbc/car/subaru/values.py
+20 -1
View File
@@ -1,15 +1,25 @@
from dataclasses import dataclass, field
from enum import Enum, IntFlag
from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
from opendbc.car.structs import CarParams
from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL
Ecu = CarParams.Ecu
class CarControllerParams:
ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
650,
([], []),
([], []),
MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * 0.06),
MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * 0.06),
MAX_ANGLE_RATE=1,
)
def __init__(self, CP):
self.STEER_STEP = 2 # how often we update the steer cmd
self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max
@@ -18,6 +28,9 @@ class CarControllerParams:
self.STEER_DRIVER_MULTIPLIER = 50 # weight driver torque heavily
self.STEER_DRIVER_FACTOR = 1 # from dbc
self.STEER_OVERRIDE_TORQUE_HIGH = 200
self.STEER_OVERRIDE_TORQUE_LOW = 150
if CP.flags & SubaruFlags.GLOBAL_GEN2:
# TODO: lower rate limits, this reaches min/max in 0.5s which negatively affects tuning
self.STEER_MAX = 1500
@@ -60,6 +73,7 @@ class SubaruSafetyFlags(IntFlag):
LONG = 2
PREGLOBAL_REVERSED_DRIVER_TORQUE = 4
STOP_AND_GO = 8
LKAS_ANGLE = 16
class SubaruFlags(IntFlag):
@@ -214,6 +228,11 @@ class CAR(Platforms):
SUBARU_ASCENT.specs,
flags=SubaruFlags.LKAS_ANGLE,
)
SUBARU_CROSSTREK_2025 = SubaruGen2PlatformConfig(
[SubaruCarDocs("Subaru Crosstrek 2025", "All", car_parts=CarParts.common([CarHarness.subaru_d]))],
CarSpecs(mass=1529, wheelbase=2.67, steerRatio=17),
flags=SubaruFlags.LKAS_ANGLE,
)
SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
opendbc_repo/opendbc/car/tesla/teslacan.py
+1 -6
View File
@@ -16,12 +16,7 @@ class TeslaCAN:
return self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values)
def create_longitudinal_command(self, acc_state, accel, counter, v_ego, active):
from opendbc.car.interfaces import V_CRUISE_MAX
set_speed = max(v_ego * CV.MS_TO_KPH, 0)
if active:
# TODO: this causes jerking after gas override when above set speed
set_speed = 0 if accel < 0 else V_CRUISE_MAX
set_speed = min(max(v_ego + accel, 0) * CV.MS_TO_KPH, 400)
values = {
"DAS_setSpeed": set_speed,
opendbc_repo/opendbc/car/tesla/tests/test_teslacan.py
+25
View File
@@ -0,0 +1,25 @@
import pytest
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.tesla.teslacan import TeslaCAN
class RecordingPacker:
def make_can_msg(self, name, bus, values):
return name, bus, values
@pytest.mark.parametrize("active", [False, True])
@pytest.mark.parametrize(
("v_ego", "accel", "expected_set_speed"),
[
(20.0, 1.0, 21.0 * CV.MS_TO_KPH),
(20.0, -2.0, 18.0 * CV.MS_TO_KPH),
(1.0, -2.0, 0.0),
(120.0, 2.0, 400.0),
],
)
def test_longitudinal_set_speed_tracks_accel_continuously(active, v_ego, accel, expected_set_speed):
_, _, values = TeslaCAN(RecordingPacker()).create_longitudinal_command(4, accel, 0, v_ego, active)
assert values["DAS_setSpeed"] == pytest.approx(expected_set_speed)
opendbc_repo/opendbc/car/tests/routes.py
+1
View File
@@ -343,6 +343,7 @@ routes = [
CarTestRoute("1bbe6bf2d62f58a8/2022-07-14--17-11-43", SUBARU.SUBARU_OUTBACK, segment=10),
CarTestRoute("c56e69bbc74b8fad/2022-08-18--09-43-51", SUBARU.SUBARU_LEGACY, segment=3),
CarTestRoute("f4e3a0c511a076f4/2022-08-04--16-16-48", SUBARU.SUBARU_CROSSTREK_HYBRID, segment=2),
CarTestRoute("f73c01590368ee5b/00000017--117e1dd96d", SUBARU.SUBARU_CROSSTREK_2025),
CarTestRoute("7fd1e4f3a33c1673/2022-12-04--15-09-53", SUBARU.SUBARU_FORESTER_2022, segment=4),
CarTestRoute("f3b34c0d2632aa83/2023-07-23--20-43-25", SUBARU.SUBARU_OUTBACK_2023, segment=7),
CarTestRoute("99437cef6d5ff2ee/2023-03-13--21-21-38", SUBARU.SUBARU_ASCENT_2023, segment=7),
opendbc_repo/opendbc/car/torque_data/override.toml
+1
View File
@@ -14,6 +14,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
"SUBARU_FORESTER_2022" = [nan, 3.0, nan]
"SUBARU_OUTBACK_2023" = [nan, 3.0, nan]
"SUBARU_ASCENT_2023" = [nan, 3.0, nan]
"SUBARU_CROSSTREK_2025" = [nan, 3.0, nan]
# Toyota LTA also has torque
"TOYOTA_RAV4_TSS2_2023" = [nan, 3.0, nan]
opendbc_repo/opendbc/car/toyota/fingerprints.py
+10
View File
@@ -1309,13 +1309,22 @@ FW_VERSIONS = {
b'\x01896630841000\x00\x00\x00\x00',
b'\x01896630857101\x00\x00\x00\x00',
b'\x01896630864000\x00\x00\x00\x00',
b'\x01896630869000\x00\x00\x00\x00',
],
(Ecu.abs, 0x7b0, None): [
b'\x01F15260815100\x00\x00\x00\x00',
b'\x01F15260815300\x00\x00\x00\x00',
b'\x01F15260823000\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'\x018965B4509100\x00\x00\x00\x00',
b'\x018965B4514000\x00\x00\x00\x00',
],
(Ecu.hybrid, 0x7d2, None): [
b'\x02899830812000\x00\x00\x00\x00899850813000\x00\x00\x00\x00',
],
(Ecu.srs, 0x780, None): [
b'\x028917F0815200\x00\x00\x00\x008917H0801200\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301500\x00\x00\x00\x00',
@@ -1324,6 +1333,7 @@ FW_VERSIONS = {
b'\x028646F0802200\x00\x00\x00\x008646G4202100\x00\x00\x00\x00',
b'\x028646F0802300\x00\x00\x00\x008646G4202100\x00\x00\x00\x00',
b'\x028646F0802400\x00\x00\x00\x008646G4202100\x00\x00\x00\x00',
b'\x028646F0802500\x00\x00\x00\x008646G4202100\x00\x00\x00\x00',
],
},
CAR.LEXUS_CTH: {
opendbc_repo/opendbc/car/toyota/interface.py
+3
View File
@@ -197,6 +197,9 @@ class CarInterface(CarInterfaceBase):
if candidate == CAR.TOYOTA_HIGHLANDER and ret.openpilotLongitudinalControl and not ret.flags & ToyotaFlags.HYBRID.value:
ret.longitudinalActuatorDelay = 0.4
if candidate == CAR.TOYOTA_SIENNA and ret.openpilotLongitudinalControl:
ret.longitudinalActuatorDelay = 0.5
if ret.enableGasInterceptorDEPRECATED:
# Pedal/SDSU Toyotas feel best with a softer final stop clamp.
ret.longitudinalActuatorDelay = max(ret.longitudinalActuatorDelay, 0.2)
opendbc_repo/opendbc/car/toyota/tests/test_toyota.py
+48 -1
View File
@@ -6,7 +6,7 @@ from hypothesis import given, settings, strategies as st
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
from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car
from opendbc.car.toyota import toyotacan
from opendbc.car.toyota.carcontroller import CarController, get_camry_hybrid_feedforward, get_long_tune, get_prius_feedforward, \
get_prius_positive_feedforward_scale, \
@@ -127,6 +127,32 @@ class TestToyotaInterfaces:
assert not long_params.flags & ToyotaFlags.HYBRID.value
assert long_params.longitudinalActuatorDelay == pytest.approx(0.4)
def test_sienna_openpilot_long_uses_measured_actuator_delay(self):
stock_params = CarInterface.get_params(
CAR.TOYOTA_SIENNA,
{bus: {} for bus in range(8)},
[],
alpha_long=False,
is_release=False,
docs=False,
starpilot_toggles=SimpleNamespace(),
)
long_params = CarInterface.get_params(
CAR.TOYOTA_SIENNA,
{bus: ({0x2FF: 8} if bus == 0 else {}) for bus in range(8)},
[],
alpha_long=False,
is_release=False,
docs=False,
starpilot_toggles=SimpleNamespace(),
)
assert not stock_params.openpilotLongitudinalControl
assert stock_params.longitudinalActuatorDelay == pytest.approx(0.15)
assert long_params.openpilotLongitudinalControl
assert not long_params.flags & ToyotaFlags.HYBRID.value
assert long_params.longitudinalActuatorDelay == pytest.approx(0.5)
@pytest.mark.parametrize("camera_message", [0x343, 0x4CB])
def test_dsu_bypass_enables_longitudinal(self, camera_message):
fingerprint = {bus: {} for bus in range(8)}
@@ -331,6 +357,27 @@ class TestToyotaInterfaces:
class TestToyotaFingerprint:
def test_sienna_2025_route_fw_exact_match(self):
route_fw = {
(Ecu.engine, 0x700, None): b'\x01896630869000\x00\x00\x00\x00',
(Ecu.abs, 0x7b0, None): b'\x01F15260823000\x00\x00\x00\x00',
(Ecu.eps, 0x7a1, None): b'\x018965B4514000\x00\x00\x00\x00',
(Ecu.hybrid, 0x7d2, None): b'\x02899830812000\x00\x00\x00\x00899850813000\x00\x00\x00\x00',
(Ecu.srs, 0x780, None): b'\x028917F0815200\x00\x00\x00\x008917H0801200\x00\x00\x00\x00',
(Ecu.fwdRadar, 0x750, 0xf): b'\x018821F3301500\x00\x00\x00\x00',
(Ecu.fwdCamera, 0x750, 0x6d): b'\x028646F0802500\x00\x00\x00\x008646G4202100\x00\x00\x00\x00',
}
car_fw = [
CarParams.CarFw(ecu=ecu, address=address, subAddress=0 if sub_address is None else sub_address,
fwVersion=version, brand="toyota")
for (ecu, address, sub_address), version in route_fw.items()
]
exact, matches = match_fw_to_car(car_fw, "5TDESKFC4SS158497", allow_fuzzy=False, log=False)
assert exact
assert matches == {CAR.TOYOTA_SIENNA_4TH_GEN}
def test_non_essential_ecus(self, subtests):
# Ensures only the cars that have multiple engine ECUs are in the engine non-essential ECU list
for car_model, ecus in FW_VERSIONS.items():
opendbc_repo/opendbc/car/toyota/values.py
+1 -1
View File
@@ -324,7 +324,7 @@ class CAR(Platforms):
flags=ToyotaFlags.NO_STOP_TIMER,
)
TOYOTA_SIENNA_4TH_GEN = ToyotaSecOCPlatformConfig(
[ToyotaCommunityCarDocs("Toyota Sienna 2021-23", min_enable_speed=MIN_ACC_SPEED)],
[ToyotaCommunityCarDocs("Toyota Sienna 2021-25", min_enable_speed=MIN_ACC_SPEED)],
CarSpecs(mass=4625. * CV.LB_TO_KG, wheelbase=3.06, steerRatio=17.8, tireStiffnessFactor=0.444),
)
opendbc_repo/opendbc/safety/modes/hyundai.h
+68 -9
View File
@@ -89,6 +89,18 @@ static const CanMsg HYUNDAI_LONG_REFRESH_TX_MSGS[] = {
};
static bool hyundai_legacy = false;
static bool hyundai_can_canfd_blended_hda2 = false;
static bool hyundai_acc_main_on_rx_prev = false;
#define HYUNDAI_CAN_CANFD_BLENDED_HDA2_RX_CHECKS() \
{.msg = {{0x260, 1, 8, 100U, .max_counter = 3U, .ignore_quality_flag = true}, \
{0x371, 1, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }}}, \
{.msg = {{0x386, 1, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x394, 1, 8, 50U, .max_counter = 7U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x251, 1, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x4F1, 1, 4, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
HYUNDAI_SCC11_ADDR_CHECK(1) \
HYUNDAI_SCC12_ADDR_CHECK(1, true)
static uint8_t hyundai_get_counter(const CANPacket_t *msg) {
@@ -164,10 +176,11 @@ static uint32_t hyundai_compute_checksum(const CANPacket_t *msg) {
}
static void hyundai_rx_hook(const CANPacket_t *msg) {
const uint8_t pt_bus = hyundai_can_canfd_blended_hda2 ? 1U : 0U;
const uint8_t scc_bus = hyundai_camera_scc ? 2U : pt_bus;
// SCC12 is on bus 2 for camera-based SCC cars, bus 0 on all others
if (msg->addr == 0x421U) {
if (((msg->bus == 0U) && !hyundai_camera_scc) || ((msg->bus == 2U) && hyundai_camera_scc)) {
if (msg->bus == scc_bus) {
// 2 bits: 13-14
uint8_t cruise_byte = hyundai_can_canfd_blended ? (msg->data[3] >> 4) : (GET_BYTES(msg, 0, 4) >> 13);
int cruise_engaged = cruise_byte & 0x3U;
@@ -176,9 +189,14 @@ static void hyundai_rx_hook(const CANPacket_t *msg) {
}
if (msg->addr == 0x420U) {
if (((msg->bus == 0U) && !hyundai_camera_scc) || ((msg->bus == 2U) && hyundai_camera_scc)) {
if (msg->bus == scc_bus) {
if (!hyundai_longitudinal) {
acc_main_on = GET_BIT(msg, hyundai_can_canfd_blended ? 27U : 0U);
const bool acc_main_on_rx = GET_BIT(msg, hyundai_can_canfd_blended ? 27U : 0U);
if (hyundai_aol_main_lkas_sync && (acc_main_on_rx != hyundai_acc_main_on_rx_prev)) {
lkas_on = false;
}
acc_main_on = acc_main_on_rx;
hyundai_acc_main_on_rx_prev = acc_main_on_rx;
}
}
}
@@ -188,7 +206,7 @@ static void hyundai_rx_hook(const CANPacket_t *msg) {
hyundai_common_cruise_state_check((cruise_set_speed > 0U) && (cruise_set_speed < 255U));
}
if (msg->bus == 0U) {
if (msg->bus == pt_bus) {
if (msg->addr == 0x251U) {
int torque_driver_new = (GET_BYTES(msg, 0, 2) & 0x7ffU) - 1024U;
// update array of samples
@@ -304,7 +322,7 @@ static bool hyundai_tx_hook(const CANPacket_t *msg) {
}
// LKA STEER: safety check
if (msg->addr == 0x340U) {
if ((msg->addr == 0x340U) && !hyundai_can_canfd_blended_hda2) {
int desired_torque = ((GET_BYTES(msg, 0, 4) >> 16) & 0x7ffU) - 1024U;
bool steer_req = GET_BIT(msg, 27U);
@@ -317,6 +335,15 @@ static bool hyundai_tx_hook(const CANPacket_t *msg) {
}
}
if ((msg->addr == 0x50U) && hyundai_can_canfd_blended_hda2) {
int desired_torque = ((((int)msg->data[6] & 0xFU) << 7) | (msg->data[5] >> 1)) - 1024;
bool steer_req = GET_BIT(msg, 52U);
if (steer_torque_cmd_checks(desired_torque, steer_req, HYUNDAI_STEERING_LIMITS)) {
tx = false;
}
}
// UDS: Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
if (msg->addr == 0x7D0U) {
if ((GET_BYTES(msg, 0, 4) != 0x00803E02U) || (GET_BYTES(msg, 4, 4) != 0x0U)) {
@@ -363,6 +390,12 @@ static safety_config hyundai_init(uint16_t param) {
{0x364, 0, 8, .check_relay = true},
};
static const CanMsg HYUNDAI_CAN_CANFD_BLENDED_HDA2_TX_MSGS[] = {
{0x50, 0, 16, .check_relay = true},
{0x4F1, 1, 4, .check_relay = false},
{0x2A4, 0, 24, .check_relay = true},
};
static const CanMsg HYUNDAI_CAN_CANFD_BLENDED_LONG_TX_MSGS[] = {
{0x340, 0, 8, .check_relay = true},
{0x4F1, 0, 4, .check_relay = false},
@@ -380,6 +413,9 @@ static safety_config hyundai_init(uint16_t param) {
hyundai_common_init(param);
hyundai_legacy = false;
hyundai_can_canfd_blended_hda2 = hyundai_can_canfd_blended && hyundai_canfd_lka_steering;
hyundai_aol_main_lkas_sync = GET_FLAG(param, 32U);
hyundai_acc_main_on_rx_prev = false;
if (hyundai_can_canfd_blended) {
gen_crc_lookup_table_16(0x1021, hyundai_canfd_crc_lut);
@@ -421,7 +457,13 @@ static safety_config hyundai_init(uint16_t param) {
SET_RX_CHECKS(hyundai_long_rx_checks, ret);
}
}
if (hyundai_camera_scc) {
if (hyundai_can_canfd_blended_hda2) {
static RxCheck hyundai_can_canfd_blended_hda2_long_rx_checks[] = {
HYUNDAI_CAN_CANFD_BLENDED_HDA2_RX_CHECKS()
};
SET_RX_CHECKS(hyundai_can_canfd_blended_hda2_long_rx_checks, ret);
SET_TX_MSGS(HYUNDAI_CAN_CANFD_BLENDED_HDA2_TX_MSGS, ret);
} else if (hyundai_camera_scc) {
if (hyundai_can_refresh_msgs) {
SET_TX_MSGS(HYUNDAI_CAMERA_SCC_LONG_REFRESH_TX_MSGS, ret);
} else {
@@ -475,10 +517,27 @@ static safety_config hyundai_init(uint16_t param) {
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
SET_TX_MSGS(HYUNDAI_CAN_CANFD_BLENDED_TX_MSGS, ret);
if (hyundai_has_lda_button) {
static RxCheck hyundai_can_canfd_blended_hda2_rx_checks[] = {
HYUNDAI_CAN_CANFD_BLENDED_HDA2_RX_CHECKS()
};
static RxCheck hyundai_can_canfd_blended_hda2_rx_checks_lda[] = {
HYUNDAI_CAN_CANFD_BLENDED_HDA2_RX_CHECKS()
HYUNDAI_LDA_BUTTON_ADDR_CHECK
};
if (hyundai_can_canfd_blended_hda2) {
SET_TX_MSGS(HYUNDAI_CAN_CANFD_BLENDED_HDA2_TX_MSGS, ret);
if (hyundai_has_lda_button) {
SET_RX_CHECKS(hyundai_can_canfd_blended_hda2_rx_checks_lda, ret);
} else {
SET_RX_CHECKS(hyundai_can_canfd_blended_hda2_rx_checks, ret);
}
} else if (hyundai_has_lda_button) {
SET_TX_MSGS(HYUNDAI_CAN_CANFD_BLENDED_TX_MSGS, ret);
SET_RX_CHECKS(hyundai_can_canfd_blended_rx_checks_lda, ret);
} else {
SET_TX_MSGS(HYUNDAI_CAN_CANFD_BLENDED_TX_MSGS, ret);
SET_RX_CHECKS(hyundai_can_canfd_blended_rx_checks, ret);
}
} else {
opendbc_repo/opendbc/safety/modes/hyundai_common.h
+7 -1
View File
@@ -60,6 +60,9 @@ bool hyundai_cancel_button_enable = false;
extern bool hyundai_can_refresh_msgs;
bool hyundai_can_refresh_msgs = false;
extern bool hyundai_aol_main_lkas_sync;
bool hyundai_aol_main_lkas_sync = false;
static uint8_t hyundai_last_button_interaction; // button messages since the user pressed an enable button
static bool acc_main_on_prev;
static bool acc_main_on_tx;
@@ -95,6 +98,7 @@ void hyundai_common_init(uint16_t param) {
hyundai_non_scc = GET_FLAG(param, HYUNDAI_PARAM_NON_SCC);
hyundai_cancel_button_enable = GET_FLAG(param, HYUNDAI_PARAM_CANCEL_BTN_ENABLE);
hyundai_can_refresh_msgs = GET_FLAG(param, HYUNDAI_PARAM_CAN_REFRESH_MSGS);
hyundai_aol_main_lkas_sync = false;
hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES;
acc_main_on_prev = false;
@@ -160,7 +164,9 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai
}
if (main_button && !main_button_prev) {
acc_main_on = !acc_main_on;
if (!hyundai_aol_main_lkas_sync) {
acc_main_on = !acc_main_on;
}
}
main_button_prev = main_button;
}
opendbc_repo/opendbc/safety/modes/subaru.h
+67 -6
View File
@@ -22,11 +22,13 @@
#define MSG_SUBARU_Brake_Status 0x13cU
#define MSG_SUBARU_CruiseControl 0x240U
#define MSG_SUBARU_Throttle 0x40U
#define MSG_SUBARU_Steering_2 0x11aU
#define MSG_SUBARU_Steering_Torque 0x119U
#define MSG_SUBARU_Wheel_Speeds 0x13aU
#define MSG_SUBARU_Brake_Pedal 0x139U
#define MSG_SUBARU_ES_LKAS 0x122U
#define MSG_SUBARU_ES_LKAS_ANGLE 0x124U
#define MSG_SUBARU_ES_Brake 0x220U
#define MSG_SUBARU_ES_Distance 0x221U
#define MSG_SUBARU_ES_Status 0x222U
@@ -75,9 +77,18 @@
{.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_CruiseControl, alt_bus, 8, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
#define SUBARU_LKAS_ANGLE_RX_CHECKS(alt_bus, status_bus) \
{.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Steering_2, SUBARU_MAIN_BUS, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_SUBARU_Wheel_Speeds, alt_bus, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.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 }}}, \
static bool subaru_gen2 = false;
static bool subaru_longitudinal = false;
static bool subaru_stop_and_go = false;
static bool subaru_lkas_angle = false;
static uint32_t subaru_get_checksum(const CANPacket_t *msg) {
return (uint8_t)msg->data[0];
@@ -98,21 +109,27 @@ 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;
if ((msg->addr == MSG_SUBARU_Steering_Torque) && (msg->bus == SUBARU_MAIN_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);
}
int angle_meas_new = (GET_BYTES(msg, 4, 2) & 0xFFFFU);
// convert Steering_Torque -> Steering_Angle to centidegrees, to match the ES_LKAS_ANGLE angle request units
angle_meas_new = ROUND(to_signed(angle_meas_new, 16) * -2.17);
if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_Steering_2) && (msg->bus == SUBARU_MAIN_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);
}
// enter controls on rising edge of ACC, exit controls on ACC off
if ((msg->addr == MSG_SUBARU_CruiseControl) && (msg->bus == alt_main_bus)) {
if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_ES_Status) && (msg->bus == status_bus)) {
bool cruise_engaged = GET_BIT(msg, 29U);
pcm_cruise_check(cruise_engaged);
}
if (!subaru_lkas_angle && (msg->addr == MSG_SUBARU_CruiseControl) && (msg->bus == alt_main_bus)) {
bool cruise_engaged = (msg->data[5] >> 1) & 1U;
pcm_cruise_check(cruise_engaged);
@@ -144,6 +161,18 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
const TorqueSteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(3071, 50, 70);
const TorqueSteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1500, 35, 50);
const AngleSteeringLimits SUBARU_ANGLE_STEERING_LIMITS = {
.max_angle = 650 * 100,
.angle_deg_to_can = 100.,
.frequency = 50U,
};
const AngleSteeringParams SUBARU_ANGLE_STEERING_PARAMS = {
.slip_factor = -0.000580374471400815,
.steer_ratio = 13.5,
.wheelbase = 2.890000104904175,
};
const LongitudinalLimits SUBARU_LONG_LIMITS = {
.min_gas = 808, // appears to be engine braking
.max_gas = 3400, // approx 2 m/s^2 when maxing cruise_rpm and cruise_throttle
@@ -168,6 +197,14 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits);
}
if (msg->addr == MSG_SUBARU_ES_LKAS_ANGLE) {
int desired_angle = GET_BYTES(msg, 5, 3) & 0x1FFFFU;
desired_angle = -1 * to_signed(desired_angle, 17);
bool lkas_request = GET_BIT(msg, 12U);
violation |= steer_angle_cmd_checks_vm(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS, SUBARU_ANGLE_STEERING_PARAMS);
}
// check es_brake brake_pressure limits
if (msg->addr == MSG_SUBARU_ES_Brake) {
int es_brake_pressure = GET_BYTES(msg, 2, 2);
@@ -239,6 +276,16 @@ static safety_config subaru_init(uint16_t param) {
SUBARU_STOP_AND_GO_ADDITIONAL_TX_MSGS()
};
static const CanMsg SUBARU_LKAS_ANGLE_TX_MSGS[] = {
SUBARU_BASE_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
};
static const CanMsg SUBARU_GEN2_LKAS_ANGLE_TX_MSGS[] = {
SUBARU_BASE_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
};
static RxCheck subaru_rx_checks[] = {
SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS)
};
@@ -247,6 +294,14 @@ static safety_config subaru_init(uint16_t param) {
SUBARU_COMMON_RX_CHECKS(SUBARU_ALT_BUS)
};
static RxCheck subaru_lkas_angle_rx_checks[] = {
SUBARU_LKAS_ANGLE_RX_CHECKS(SUBARU_MAIN_BUS, SUBARU_CAM_BUS)
};
static RxCheck subaru_gen2_lkas_angle_rx_checks[] = {
SUBARU_LKAS_ANGLE_RX_CHECKS(SUBARU_ALT_BUS, SUBARU_ALT_BUS)
};
const uint16_t SUBARU_PARAM_GEN2 = 1;
subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
@@ -254,13 +309,19 @@ static safety_config subaru_init(uint16_t param) {
const uint16_t SUBARU_PARAM_STOP_AND_GO = 8;
subaru_stop_and_go = GET_FLAG(param, SUBARU_PARAM_STOP_AND_GO);
const uint16_t SUBARU_PARAM_LKAS_ANGLE = 16;
subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE);
#ifdef ALLOW_DEBUG
const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
#endif
safety_config ret;
if (subaru_gen2) {
if (subaru_lkas_angle) {
ret = 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) : \
BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_TX_MSGS);
} else {
opendbc_repo/opendbc/safety/tests/libsafety/libsafety_py.py
+2
View File
@@ -43,6 +43,8 @@ bool get_brake_pressed_prev(void);
bool get_regen_braking_prev(void);
bool get_steering_disengage_prev(void);
bool get_acc_main_on(void);
bool get_aol_allowed(void);
bool get_lkas_on(void);
uint32_t get_acc_main_on_mismatches(void);
float get_vehicle_speed_min(void);
float get_vehicle_speed_max(void);
opendbc_repo/opendbc/safety/tests/libsafety/safety.c
+8
View File
@@ -103,6 +103,14 @@ bool get_acc_main_on(void){
return acc_main_on;
}
bool get_aol_allowed(void){
return aol_allowed;
}
bool get_lkas_on(void){
return lkas_on;
}
float get_vehicle_speed_min(void){
return vehicle_speed.min / VEHICLE_SPEED_FACTOR;
}
opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py
+7 -3
View File
@@ -1,5 +1,6 @@
from opendbc.car.ford.values import FordSafetyFlags
from opendbc.car.hyundai.values import HyundaiSafetyFlags
from opendbc.car.subaru.values import SubaruSafetyFlags
from opendbc.car.toyota.values import ToyotaSafetyFlags
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
@@ -32,7 +33,7 @@ def is_steering_msg(mode, param, addr):
elif mode == CarParams.SafetyModel.chrysler:
ret = addr == 0x292
elif mode == CarParams.SafetyModel.subaru:
ret = addr == 0x122
ret = addr == (0x124 if param & SubaruSafetyFlags.LKAS_ANGLE else 0x122)
elif mode == CarParams.SafetyModel.ford:
ret = addr == (0x3ca if param & FordSafetyFlags.LKA_STEERING else
0x3d6 if param & FordSafetyFlags.CANFD else
@@ -76,8 +77,11 @@ def get_steer_value(mode, param, msg):
elif mode == CarParams.SafetyModel.chrysler:
torque = (((msg.data[0] & 0x7) << 8) | msg.data[1]) - 1024
elif mode == CarParams.SafetyModel.subaru:
torque = ((msg.data[3] & 0x1F) << 8) | msg.data[2]
torque = -to_signed(torque, 13)
if param & SubaruSafetyFlags.LKAS_ANGLE:
angle = -to_signed((msg.data[5] | (msg.data[6] << 8) | (msg.data[7] << 16)) & 0x1FFFF, 17)
else:
torque = ((msg.data[3] & 0x1F) << 8) | msg.data[2]
torque = -to_signed(torque, 13)
elif mode == CarParams.SafetyModel.ford:
if param & FordSafetyFlags.LKA_STEERING:
action = msg.data[0] >> 5
opendbc_repo/opendbc/safety/tests/test_hyundai.py
+101
View File
@@ -4,6 +4,7 @@ import unittest
from opendbc.car.hyundai.values import HyundaiSafetyFlags, HyundaiStarPilotSafetyFlags
from opendbc.car.structs import CarParams
from opendbc.safety import ALTERNATIVE_EXPERIENCE
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
@@ -251,6 +252,46 @@ class TestHyundaiCanCanfdBlendedSafety(TestHyundaiSafety):
return libsafety_py.make_CANPacket(0x420, 0, bytes(dat))
class TestHyundaiCanCanfdBlendedHda2Safety(unittest.TestCase):
TX_MSGS = [[0x50, 0], [0x4F1, 1], [0x2A4, 0]]
def setUp(self):
self.packer = CANPackerSafety("hyundai_palisade_2023_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(
CarParams.SafetyModel.hyundai,
HyundaiSafetyFlags.CAN_CANFD_BLENDED | HyundaiSafetyFlags.CANFD_LKA_STEERING,
)
self.safety.init_tests()
def _lkas_msg(self, torque=0, steer_req=False):
return self.packer.make_can_msg_panda("LKAS", 0, {
"TORQUE_REQUEST": torque,
"STEER_REQ": int(steer_req),
})
def test_hda2_tx_messages_are_scoped_to_combined_safety_flags(self):
self.assertTrue(self.safety.safety_tx_hook(self._lkas_msg()))
self.assertTrue(self.safety.safety_tx_hook(self.packer.make_can_msg_panda("CAM_0x2a4", 0, {})))
self.assertFalse(self.safety.safety_tx_hook(common.make_msg(0, 0x340, 8)))
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.CAN_CANFD_BLENDED)
self.safety.init_tests()
self.assertFalse(self.safety.safety_tx_hook(self._lkas_msg()))
self.assertFalse(self.safety.safety_tx_hook(self.packer.make_can_msg_panda("CAM_0x2a4", 0, {})))
def test_hda2_steering_torque_is_checked(self):
self.safety.set_controls_allowed(True)
self.assertTrue(self.safety.safety_tx_hook(self._lkas_msg(0, True)))
self.assertFalse(self.safety.safety_tx_hook(self._lkas_msg(500, True)))
def test_hda2_camera_forwarding_blocks_replaced_frames(self):
self.assertEqual(2, self.safety.safety_fwd_hook(0, 0x123))
self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x123))
self.assertEqual(-1, self.safety.safety_fwd_hook(2, 0x50))
self.assertEqual(-1, self.safety.safety_fwd_hook(2, 0x2A4))
class TestHyundaiSafetyFCEV(TestHyundaiSafety):
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
@@ -490,5 +531,65 @@ class TestHyundaiAolLkasOnEngageStockSafety(HyundaiAolLkasOnEngageStockBase, Tes
self.safety.init_tests()
class TestHyundaiAolMainLkasSyncSafety(TestHyundaiSafety):
def setUp(self):
self.packer = CANPackerSafety("hyundai_kia_generic")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(
CarParams.SafetyModel.hyundai,
HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON | HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC,
)
self.safety.init_tests()
@staticmethod
def _lkas_button_msg(pressed):
dat = bytearray(8)
dat[0] = int(pressed) << 4
return libsafety_py.make_CANPacket(0x391, 0, bytes(dat))
def test_confirmed_main_state_rephases_lkas_button(self):
self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
self.safety.set_controls_allowed(False)
self._rx(self._lkas_button_msg(True))
self._rx(self._lkas_button_msg(False))
self.assertTrue(self.safety.get_lkas_on())
self.assertTrue(self.safety.get_aol_allowed())
self._set_prev_torque(0)
self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
self._rx(self._button_msg(Buttons.NONE, main_button=True))
self._rx(self._button_msg(Buttons.NONE, main_button=False))
self.assertFalse(self.safety.get_acc_main_on())
self.assertTrue(self.safety.get_lkas_on())
self.assertTrue(self.safety.get_aol_allowed())
self._rx(self._acc_state_msg(True))
self.assertFalse(self.safety.get_lkas_on())
self.assertTrue(self.safety.get_aol_allowed())
self._set_prev_torque(0)
self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
self._rx(self._button_msg(Buttons.NONE, main_button=True))
self._rx(self._button_msg(Buttons.NONE, main_button=False))
self.assertTrue(self.safety.get_acc_main_on())
self.assertTrue(self.safety.get_aol_allowed())
self._rx(self._acc_state_msg(False))
self.assertFalse(self.safety.get_controls_allowed())
self.assertFalse(self.safety.get_acc_main_on())
self.assertFalse(self.safety.get_lkas_on())
self.assertFalse(self.safety.get_aol_allowed())
self._set_prev_torque(0)
self.assertFalse(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
self._rx(self._lkas_button_msg(True))
self._rx(self._lkas_button_msg(False))
self.assertTrue(self.safety.get_lkas_on())
self.assertTrue(self.safety.get_aol_allowed())
self._set_prev_torque(0)
self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
if __name__ == "__main__":
unittest.main()
opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py
+24
View File
@@ -441,6 +441,30 @@ class TestHyundaiCanfdLFASteeringAltButtons(TestHyundaiCanfdLFASteeringAltButton
pass
class TestHyundaiCanfdAltButtonFlagIsolation(unittest.TestCase):
TX_MSGS = []
def setUp(self):
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.CANFD_ALT_BUTTONS)
self.safety.init_tests()
@staticmethod
def _button_msg(*, main=False, lka=False):
dat = bytearray(16)
dat[4] = (int(main) << 2) | (int(lka) << 7)
return libsafety_py.make_CANPacket(0x1AA, 0, bytes(dat))
def test_alt_buttons_do_not_enable_classic_main_lkas_sync(self):
self.safety.safety_rx_hook(self._button_msg(lka=True))
self.safety.safety_rx_hook(self._button_msg())
self.assertTrue(self.safety.get_lkas_on())
self.safety.safety_rx_hook(self._button_msg(main=True))
self.safety.safety_rx_hook(self._button_msg())
self.assertTrue(self.safety.get_lkas_on())
class TestHyundaiCanfdCCNCSupportFrames(common.SafetyTestBase):
TX_MSGS = [[0x161, 0], [0x162, 0], [0x7C4, 2], [0xEA, 2]]
opendbc_repo/opendbc/safety/tests/test_subaru.py
+131 -2
View File
@@ -2,11 +2,16 @@
import enum
import unittest
from opendbc.car.subaru.values import SubaruSafetyFlags
import numpy as np
from opendbc.car.lateral import get_max_angle_vm
from opendbc.car.subaru.carcontroller import get_safety_CP
from opendbc.car.subaru.values import CarControllerParams, SubaruSafetyFlags
from opendbc.car.structs import CarParams
from opendbc.car.vehicle_model import VehicleModel
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.tests.common import CANPackerSafety, away_round, round_speed
from functools import partial
@@ -14,6 +19,7 @@ class SubaruMsg(enum.IntEnum):
Brake_Status = 0x13c
CruiseControl = 0x240
Throttle = 0x40
Steering_2 = 0x11a
Steering_Torque = 0x119
Wheel_Speeds = 0x13a
Brake_Pedal = 0x139
@@ -178,6 +184,109 @@ class TestSubaruTorqueSafetyBase(TestSubaruSafetyBase, common.DriverTorqueSteeri
return self.packer.make_can_msg_safety("ES_LKAS", SUBARU_MAIN_BUS, values)
class TestSubaruAngleSafetyBase(TestSubaruSafetyBase, common.AngleSteeringSafetyTest):
ALT_MAIN_BUS = SUBARU_ALT_BUS
TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS, SubaruMsg.ES_LKAS_ANGLE)
RELAY_MALFUNCTION_ADDRS = {SUBARU_MAIN_BUS: (SubaruMsg.ES_LKAS_ANGLE, SubaruMsg.ES_DashStatus,
SubaruMsg.ES_LKAS_State, SubaruMsg.ES_Infotainment)}
FWD_BLACKLISTED_ADDRS = fwd_blacklisted_addr(SubaruMsg.ES_LKAS_ANGLE)
FLAGS = SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.GEN2
STEER_ANGLE_MAX = 650
DEG_TO_CAN = 100
ANGLE_RATE_BP = None
ANGLE_RATE_UP = None
ANGLE_RATE_DOWN = None
LATERAL_FREQUENCY = 50
def setUp(self):
self.VM = VehicleModel(get_safety_CP())
self.angle_cmd_cnt = 0
super().setUp()
def _get_steer_cmd_angle_max(self, speed):
return get_max_angle_vm(max(speed, 1), self.VM, CarControllerParams)
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_MAIN_BUS, values)
def _angle_meas_msg(self, angle):
return self.packer.make_can_msg_safety("Steering_2", SUBARU_MAIN_BUS, {"Steering_Angle": angle})
def _speed_msg(self, speed):
values = {s: speed * 3.6 for s in ["FR", "FL", "RR", "RL"]}
return self.packer.make_can_msg_safety("Wheel_Speeds", self.ALT_MAIN_BUS, values)
def _pcm_status_msg(self, enable):
bus = SUBARU_ALT_BUS if self.FLAGS & SubaruSafetyFlags.GEN2 else SUBARU_CAM_BUS
return self.packer.make_can_msg_safety("ES_Status", bus, {"Cruise_Activated": enable})
def _toggle_aol(self, toggle_on):
return None
def test_angle_cmd_when_enabled(self):
pass
def _setup_speed(self, speed):
self.safety.init_tests()
self.safety.set_controls_allowed(True)
self._reset_speed_measurement(speed + 1)
def _find_max_allowed_angle_can(self, sign):
lo, hi = 0, int(self.STEER_ANGLE_MAX * self.DEG_TO_CAN) + 10
while lo < hi:
mid = (lo + hi + 1) // 2
self.safety.set_desired_angle_last(mid * sign)
if self._tx(self._angle_cmd_msg(mid / self.DEG_TO_CAN * sign, True)):
lo = mid
else:
hi = mid - 1
return lo
def _find_max_allowed_delta_can(self, sign):
lo, hi = 0, int(self.STEER_ANGLE_MAX * self.DEG_TO_CAN) + 10
while lo < hi:
mid = (lo + hi + 1) // 2
self.safety.set_desired_angle_last(0)
if self._tx(self._angle_cmd_msg(mid / self.DEG_TO_CAN * sign, True)):
lo = mid
else:
hi = mid - 1
return lo
def test_lateral_accel_limit(self):
for speed in np.linspace(1, 40, 40):
speed = round_speed(away_round(speed * 3.6 / 0.057) * 0.057 / 3.6)
for sign in (-1, 1):
self._setup_speed(speed)
max_can = self._find_max_allowed_angle_can(sign)
self.safety.set_desired_angle_last(max_can * sign)
self.assertTrue(self._tx(self._angle_cmd_msg(max_can / self.DEG_TO_CAN * sign, True)))
if max_can < self.STEER_ANGLE_MAX * self.DEG_TO_CAN:
over = max_can + 1
self.safety.set_desired_angle_last(over * sign)
self.assertFalse(self._tx(self._angle_cmd_msg(over / self.DEG_TO_CAN * sign, True)))
def test_lateral_jerk_limit(self):
for speed in np.linspace(1, 40, 40):
speed = round_speed(away_round(speed * 3.6 / 0.057) * 0.057 / 3.6)
for sign in (-1, 1):
self._setup_speed(speed)
self.assertTrue(self._tx(self._angle_cmd_msg(0, True)))
max_delta = self._find_max_allowed_delta_can(sign)
self.safety.set_desired_angle_last(0)
self.assertTrue(self._tx(self._angle_cmd_msg(max_delta / self.DEG_TO_CAN * sign, True)))
over = max_delta + 1
self.safety.set_desired_angle_last(0)
self.assertFalse(self._tx(self._angle_cmd_msg(over / self.DEG_TO_CAN * sign, True)))
class TestSubaruGen1TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):
FLAGS = 0
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS)
@@ -187,6 +296,14 @@ class TestSubaruGen1StopAndGoSafety(TestSubaruStockLongitudinalSafetyBase, TestS
FLAGS = SubaruSafetyFlags.STOP_AND_GO
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS) + [[SubaruMsg.Throttle, SUBARU_CAM_BUS],
[SubaruMsg.Brake_Pedal, SUBARU_CAM_BUS]]
RELAY_MALFUNCTION_ADDRS = {
**TestSubaruSafetyBase.RELAY_MALFUNCTION_ADDRS,
SUBARU_CAM_BUS: (SubaruMsg.Throttle, SubaruMsg.Brake_Pedal),
}
FWD_BLACKLISTED_ADDRS = {
**fwd_blacklisted_addr(),
SUBARU_MAIN_BUS: (SubaruMsg.Throttle, SubaruMsg.Brake_Pedal),
}
class TestSubaruGen2TorqueSafetyBase(TestSubaruTorqueSafetyBase):
@@ -211,6 +328,18 @@ class TestSubaruGen1LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSub
SubaruMsg.ES_Distance)}
class TestSubaruGen1AngleStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase):
ALT_MAIN_BUS = SUBARU_MAIN_BUS
FLAGS = SubaruSafetyFlags.LKAS_ANGLE
TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS, SubaruMsg.ES_LKAS_ANGLE)
class TestSubaruGen2AngleStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase):
ALT_MAIN_BUS = SUBARU_ALT_BUS
FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE
TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS, SubaruMsg.ES_LKAS_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()
panda/board/obj/body_h7.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/body_h7/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/body_h7/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/gitversion.h
+1 -1
View File
@@ -1,2 +1,2 @@
extern const uint8_t gitversion[19];
const uint8_t gitversion[19] = "DEV-0351e7d8-DEBUG";
const uint8_t gitversion[19] = "DEV-205e4b3a-DEBUG";
panda/board/obj/panda.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_can_ignition_only.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_can_ignition_only/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_can_ignition_only/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_can_ignition_only.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_can_ignition_only/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_can_ignition_only/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_hkg_remote.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_hkg_remote/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_hkg_remote/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_remote.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_remote/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_remote/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_remote_can_ignition_only/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_remote_can_ignition_only/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_hkg_remote.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_hkg_remote/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_hkg_remote/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_jungle_h7.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_jungle_h7/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_jungle_h7/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_remote.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_remote/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_remote/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_remote_can_ignition_only.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_remote_can_ignition_only/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_remote_can_ignition_only/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/version
+1 -1
View File
@@ -1 +1 @@
DEV-0351e7d8-DEBUG
DEV-205e4b3a-DEBUG
selfdrive/car/card.py
+11 -1
View File
@@ -157,7 +157,11 @@ class Car:
secoc_key = self.params.get("SecOCKey")
if secoc_key is not None:
saved_secoc_key = bytes.fromhex(secoc_key.strip())
try:
saved_secoc_key = bytes.fromhex(secoc_key.strip())
except (TypeError, ValueError):
saved_secoc_key = b""
if len(saved_secoc_key) == 16:
self.CP.secOcKeyAvailable = True
self.CI.CS.secoc_key = saved_secoc_key
@@ -166,6 +170,12 @@ class Car:
else:
cloudlog.warning("Saved SecOC key is invalid")
if self.CP.secOcRequired and not self.CP.secOcKeyAvailable:
self.CP.passive = True
safety_config = structs.CarParams.SafetyConfig()
safety_config.safetyModel = structs.CarParams.SafetyModel.noOutput
self.CP.safetyConfigs = [safety_config]
# Write previous route's CarParams
prev_cp = self.params.get("CarParamsPersistent")
if prev_cp is not None:
selfdrive/car/redneck_cruise.py
+13 -3
View File
@@ -21,6 +21,8 @@ LEAD_EXTRA_COAST_BUFFER_FACTOR = 0.6
LEAD_EXTRA_COAST_BUFFER_MAX_MS = 3.0 * CV.MPH_TO_MS
LEAD_EXTRA_COAST_HEADWAY_MIN_S = 1.5
LEAD_EXTRA_COAST_HEADWAY_MAX_S = 3.0
LEAD_CLOSING_REL_SPEED_MIN_MS = 0.5 * CV.MPH_TO_MS
LEAD_PROACTIVE_COAST_HEADWAY_MAX_S = 4.0
LEAD_DEPARTURE_REL_SPEED_MIN_MS = 1.0 * CV.MPH_TO_MS
LEAD_DEPARTURE_HEADWAY_MIN_S = 1.8
LEAD_DEPARTURE_HEADWAY_MAX_S = 4.5
@@ -51,7 +53,8 @@ def select_redneck_target_speed(v_cruise_kph: float, speed_cluster_ms: float,
target_speed_ms = float(starpilot_target_speed_ms)
if allow_plan_decrease and len(plan_speeds_ms) > 0:
if lead_present and target_speed_ms > speed_cluster_ms and plan_speeds_ms[0] > speed_cluster_ms:
lead_closing = lead_present and lead_rel_speed_ms < -LEAD_CLOSING_REL_SPEED_MIN_MS
if lead_present and not lead_closing and target_speed_ms > speed_cluster_ms and plan_speeds_ms[0] > speed_cluster_ms:
recovery_lookahead_points = min(len(plan_speeds_ms), LEAD_RECOVERY_LOOKAHEAD_POINTS)
recovery_target_speed_ms = max(speed_cluster_ms, min(plan_speeds_ms[:recovery_lookahead_points]))
departure_boost_ms = get_lead_departure_boost_ms(
@@ -65,17 +68,24 @@ def select_redneck_target_speed(v_cruise_kph: float, speed_cluster_ms: float,
return min(target_speed_ms, recovery_target_speed_ms)
decrease_target_speed_ms = min(plan_speeds_ms[:lookahead_points])
if lead_present and target_speed_ms > speed_cluster_ms and \
lead_headway_s = lead_distance_m / speed_cluster_ms if lead_distance_m > 0.0 and speed_cluster_ms > 0.1 else float("inf")
proactive_coast = lead_closing and lead_headway_s <= LEAD_PROACTIVE_COAST_HEADWAY_MAX_S
if not proactive_coast and lead_present and target_speed_ms > speed_cluster_ms and \
decrease_target_speed_ms >= speed_cluster_ms - LEAD_RECOVERY_HOLD_BUFFER_MS:
return speed_cluster_ms
if lead_present and decrease_target_speed_ms < speed_cluster_ms:
if lead_present and (decrease_target_speed_ms < speed_cluster_ms or proactive_coast):
decrease_target_speed_ms = max(0.0, decrease_target_speed_ms - get_lead_coast_buffer_ms(
speed_cluster_ms,
lead_distance_m,
lead_rel_speed_ms,
))
if proactive_coast and target_speed_ms > speed_cluster_ms and \
decrease_target_speed_ms >= speed_cluster_ms - LEAD_RECOVERY_HOLD_BUFFER_MS:
return speed_cluster_ms
if decrease_target_speed_ms < target_speed_ms:
return decrease_target_speed_ms
selfdrive/car/tests/test_redneck_cruise.py
+48
View File
@@ -228,6 +228,54 @@ class TestRedneckCruise(unittest.TestCase):
)
self.assertLess(target_speed, 53.0 * CV.MPH_TO_MS)
def test_target_speed_does_not_recover_while_closing_on_lead(self):
target_speed = select_redneck_target_speed(
120.0,
88.0 * CV.KPH_TO_MS,
0.0,
[89.0 * CV.KPH_TO_MS, 89.0 * CV.KPH_TO_MS, 88.0 * CV.KPH_TO_MS,
87.0 * CV.KPH_TO_MS, 85.0 * CV.KPH_TO_MS, 80.0 * CV.KPH_TO_MS],
6,
allow_plan_decrease=True,
lead_present=True,
lead_distance_m=46.8,
lead_rel_speed_ms=-2.2,
)
self.assertLess(target_speed, 80.0 * CV.KPH_TO_MS)
def test_target_speed_coasts_before_closing_lead_plan_crosses_set_speed(self):
target_speed = select_redneck_target_speed(
120.0,
100.0 * CV.KPH_TO_MS,
0.0,
[106.0 * CV.KPH_TO_MS, 105.0 * CV.KPH_TO_MS, 104.0 * CV.KPH_TO_MS,
103.0 * CV.KPH_TO_MS, 102.0 * CV.KPH_TO_MS],
5,
allow_plan_decrease=True,
lead_present=True,
lead_distance_m=55.8,
lead_rel_speed_ms=-1.1,
)
self.assertLess(target_speed, 100.0 * CV.KPH_TO_MS)
def test_target_speed_holds_for_distant_closing_lead(self):
target_speed = select_redneck_target_speed(
120.0,
100.0 * CV.KPH_TO_MS,
0.0,
[106.0 * CV.KPH_TO_MS, 105.0 * CV.KPH_TO_MS, 104.0 * CV.KPH_TO_MS,
103.0 * CV.KPH_TO_MS, 102.0 * CV.KPH_TO_MS],
5,
allow_plan_decrease=True,
lead_present=True,
lead_distance_m=150.0,
lead_rel_speed_ms=-1.1,
)
self.assertAlmostEqual(100.0 * CV.KPH_TO_MS, target_speed)
def test_target_speed_uses_near_term_recovery_for_lead_speedup(self):
target_speed = select_redneck_target_speed(
120.0,
selfdrive/controls/lib/desire_helper.py
+7 -8
View File
@@ -51,7 +51,6 @@ TURN_DESIRES = {
class DesireHelper:
def __init__(self):
self.params = Params()
self.params_memory = Params(memory=True)
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
@@ -67,15 +66,10 @@ class DesireHelper:
self.lane_change_wait_timer = 0.0
self.nav_desires_allowed = False
self._nav_param_counter = -1
self._nav_instruction_state_raw: object = None
self._nav_instruction_state: dict[str, object] = {}
def _update_nav_params(self):
self._nav_param_counter += 1
if self._nav_param_counter % 60 == 0:
self.nav_desires_allowed = self.params.get_bool("NavDesiresAllowed")
raw = self.params_memory.get("NavInstructionState") or {}
if raw == self._nav_instruction_state_raw:
return
@@ -200,6 +194,7 @@ class DesireHelper:
def _navigation_desire(self, carstate, lateral_active, starpilotPlan, starpilot_toggles, nudgeless_enabled):
self._update_nav_params()
self.nav_desires_allowed = bool(getattr(starpilot_toggles, "nav_desires_allowed", self.nav_desires_allowed))
if not self.nav_desires_allowed or not lateral_active or not bool(self._nav_instruction_state.get("valid", False)):
return log.Desire.none
@@ -223,10 +218,14 @@ class DesireHelper:
if self._nav_torque_applied(carstate, lane_change_direction) or nudgeless_allowed:
return log.Desire.keepRight
elif modifier in ("left", "sharpLeft"):
if not carstate.rightBlinker and not carstate.leftBlindspot and carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill and self._nav_turn_is_imminent(carstate, maneuver_distance):
turn_allowed = not carstate.rightBlinker and not carstate.leftBlindspot
turn_allowed &= carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill
if turn_allowed and self._nav_turn_is_imminent(carstate, maneuver_distance):
return log.Desire.turnLeft
elif modifier in ("right", "sharpRight"):
if not carstate.leftBlinker and not carstate.rightBlindspot and carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill and self._nav_turn_is_imminent(carstate, maneuver_distance):
turn_allowed = not carstate.leftBlinker and not carstate.rightBlindspot
turn_allowed &= carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill
if turn_allowed and self._nav_turn_is_imminent(carstate, maneuver_distance):
return log.Desire.turnRight
return log.Desire.none
selfdrive/controls/lib/latcontrol_torque.py
+10 -2
View File
@@ -128,6 +128,9 @@ class LatControlTorque(LatControl):
self.torque_params.latAccelFactor *= SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT
if self.is_kia_forte:
self.torque_params.latAccelFactor *= KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT
if self.is_ram_1500:
self.torque_params.latAccelFactor *= RAM_1500_BASE_LAT_ACCEL_FACTOR_MULT
self.update_limits()
if self.is_civic_bosch_modified:
self.torque_params.latAccelFactor *= CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT
if civic_bosch_modified_a_lateral_testing_ground_active():
@@ -157,6 +160,8 @@ class LatControlTorque(LatControl):
latAccelFactor *= SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT
if self.is_kia_forte:
latAccelFactor *= KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT
if self.is_ram_1500:
latAccelFactor *= RAM_1500_BASE_LAT_ACCEL_FACTOR_MULT
if self.is_civic_bosch_modified:
latAccelFactor *= CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT
if civic_bosch_modified_a_lateral_testing_ground_active():
@@ -403,7 +408,9 @@ class LatControlTorque(LatControl):
CS.vEgo < self.low_speed_reset_threshold or unwind_detected)
output_lataccel = self.pid.update(pid_log.error, error_rate=-measurement_rate, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator)
output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
if self.is_bolt_2017:
if bolt_2022_2023_tuned_path_active:
output_torque *= get_bolt_2022_2023_center_output_scale(setpoint, CS.vEgo)
elif self.is_bolt_2017:
output_torque *= get_bolt_2017_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif bolt_2018_2021_tuned_path_active:
output_torque *= get_bolt_2018_2021_dynamic_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo)
@@ -420,7 +427,7 @@ class LatControlTorque(LatControl):
if ioniq_6_active:
output_torque *= get_ioniq_6_highway_output_taper_scale(setpoint, CS.vEgo)
output_torque *= get_ioniq_6_highway_transition_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif self.is_ram_1500:
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 rav4_prime_active:
output_torque *= get_rav4_prime_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo)
@@ -434,6 +441,7 @@ class LatControlTorque(LatControl):
output_torque *= kia_ev6_low_speed_center_taper
elif kia_carnival_active:
output_torque *= kia_carnival_center_taper
output_torque *= get_kia_carnival_highway_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif tucson_4th_gen_active:
output_torque *= tucson_4th_gen_center_taper
elif self.is_silverado:
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+72 -5
View File
@@ -153,6 +153,8 @@ RAM_1500_CARS = (
CHRYSLER_CAR.RAM_1500_5TH_GEN,
)
RAM_1500_BASE_LAT_ACCEL_FACTOR_MULT = 1.20
BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3
BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045
BOLT_2017_STEER_RATIO_ONSET_SPEED = 20.0 * CV.MPH_TO_MS
@@ -221,6 +223,13 @@ BOLT_2022_2023_CENTER_TAPER_LAT = 0.18
BOLT_2022_2023_CENTER_TAPER_LAT_WIDTH = 0.03
BOLT_2022_2023_CENTER_TAPER_SPEED = 25.0
BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH = 2.5
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_MAX = 0.07
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT = 0.14
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.04
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED = 4.0
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.5
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 14.0
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX_WIDTH = 2.0
BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16
BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.12
BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT = 0.26
@@ -373,6 +382,18 @@ KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX = 14.5
KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX_WIDTH = 2.0
KIA_CARNIVAL_FRICTION_THRESHOLD_GAIN = 0.24
KIA_CARNIVAL_FRICTION_CENTER_FADE_MAX = 0.34
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_MAX = 0.14
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_LAT = 0.24
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_LAT_WIDTH = 0.06
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED = 28.0
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED_WIDTH = 2.0
KIA_CARNIVAL_HIGHWAY_FRICTION_THRESHOLD_GAIN = 0.14
KIA_CARNIVAL_HIGHWAY_FRICTION_CENTER_FADE_MAX = 0.20
KIA_CARNIVAL_HIGHWAY_TRANSITION_TAPER_MAX = 0.28
KIA_CARNIVAL_HIGHWAY_TRANSITION_JERK = 0.45
KIA_CARNIVAL_HIGHWAY_TRANSITION_JERK_WIDTH = 0.15
KIA_CARNIVAL_HIGHWAY_TRANSITION_LAT_CUTOFF = 1.20
KIA_CARNIVAL_HIGHWAY_TRANSITION_LAT_WIDTH = 0.20
TUCSON_4TH_GEN_CENTER_TAPER_MAX = 0.44
TUCSON_4TH_GEN_CENTER_TAPER_LAT = 0.28
@@ -684,8 +705,8 @@ KIA_EV6_TURN_IN_BOOST_LEFT = 0.62
KIA_EV6_TURN_IN_BOOST_RIGHT = 0.60
KIA_EV6_UNWIND_TAPER_LEFT = 0.56
KIA_EV6_UNWIND_TAPER_RIGHT = 0.54
KIA_EV6_BASE_UNWIND_TAPER_LEFT = 0.06
KIA_EV6_BASE_UNWIND_TAPER_RIGHT = 0.05
KIA_EV6_BASE_UNWIND_TAPER_LEFT = 0.10
KIA_EV6_BASE_UNWIND_TAPER_RIGHT = 0.13
KIA_EV6_JWARM_BASE_TURN_IN_BOOST_LEFT = 0.12
KIA_EV6_JWARM_BASE_TURN_IN_BOOST_RIGHT = 0.14
KIA_EV6_JWARM_BASE_UNWIND_TAPER_LEFT = 0.15
@@ -1448,6 +1469,24 @@ def get_bolt_2022_2023_ff_scale(desired_lateral_accel: float, desired_lateral_je
return 1.0 + (extra_scale * center_taper * turn_in_boost * max(unwind_taper, 0.0))
def get_bolt_2022_2023_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
highway_speed_weight = _bolt_2022_2023_sigmoid((v_ego - BOLT_2022_2023_CENTER_TAPER_SPEED) /
BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH)
highway_center_weight = _bolt_2022_2023_sigmoid((BOLT_2022_2023_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
BOLT_2022_2023_CENTER_TAPER_LAT_WIDTH)
low_speed_onset = _bolt_2022_2023_sigmoid((v_ego - BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED) /
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH)
low_speed_cutoff = _bolt_2022_2023_sigmoid((BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) /
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX_WIDTH)
low_speed_center_weight = _bolt_2022_2023_sigmoid((BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT_WIDTH)
highway_reduction = (_flm_vehicle_knob("gm_bolt_2022_2023.center_taper_max", BOLT_2022_2023_CENTER_TAPER_MAX) *
highway_speed_weight * highway_center_weight)
low_speed_reduction = (BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_MAX * low_speed_onset * low_speed_cutoff *
low_speed_center_weight)
return 1.0 - min(highway_reduction + low_speed_reduction, 0.95)
def get_bolt_2022_2023_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_gm_base_friction_threshold(v_ego)
transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
@@ -1801,20 +1840,48 @@ def _kia_carnival_center_weights(desired_lateral_accel: float, v_ego: float) ->
return speed_weight, center_weight
def _kia_carnival_highway_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]:
speed_weight = _sigmoid((v_ego - KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED) /
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED_WIDTH)
center_weight = _sigmoid((KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_LAT_WIDTH)
return speed_weight, center_weight
def get_kia_carnival_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight, center_weight = _kia_carnival_center_weights(desired_lateral_accel, v_ego)
return 1.0 - (KIA_CARNIVAL_CENTER_TAPER_MAX * speed_weight * center_weight)
highway_speed_weight, highway_center_weight = _kia_carnival_highway_center_weights(desired_lateral_accel, v_ego)
reduction = KIA_CARNIVAL_CENTER_TAPER_MAX * speed_weight * center_weight
reduction += KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_MAX * highway_speed_weight * highway_center_weight
return 1.0 - min(reduction, 0.95)
def get_kia_carnival_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
del desired_lateral_jerk
speed_weight, center_weight = _kia_carnival_center_weights(desired_lateral_accel, v_ego)
return get_hkg_canfd_base_friction_threshold(v_ego) * (1.0 + KIA_CARNIVAL_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight)
highway_speed_weight, highway_center_weight = _kia_carnival_highway_center_weights(desired_lateral_accel, v_ego)
gain = KIA_CARNIVAL_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight
gain += KIA_CARNIVAL_HIGHWAY_FRICTION_THRESHOLD_GAIN * highway_speed_weight * highway_center_weight
return get_hkg_canfd_base_friction_threshold(v_ego) * (1.0 + gain)
def get_kia_carnival_friction_center_fade_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight, center_weight = _kia_carnival_center_weights(desired_lateral_accel, v_ego)
return 1.0 - (KIA_CARNIVAL_FRICTION_CENTER_FADE_MAX * speed_weight * center_weight)
highway_speed_weight, highway_center_weight = _kia_carnival_highway_center_weights(desired_lateral_accel, v_ego)
reduction = KIA_CARNIVAL_FRICTION_CENTER_FADE_MAX * speed_weight * center_weight
reduction += KIA_CARNIVAL_HIGHWAY_FRICTION_CENTER_FADE_MAX * highway_speed_weight * highway_center_weight
return 1.0 - min(reduction, 0.95)
def get_kia_carnival_highway_transition_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float,
v_ego: float) -> float:
speed_weight = _sigmoid((v_ego - KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED) /
KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED_WIDTH)
jerk_weight = _sigmoid((abs(desired_lateral_jerk) - KIA_CARNIVAL_HIGHWAY_TRANSITION_JERK) /
KIA_CARNIVAL_HIGHWAY_TRANSITION_JERK_WIDTH)
lat_weight = _sigmoid((KIA_CARNIVAL_HIGHWAY_TRANSITION_LAT_CUTOFF - abs(desired_lateral_accel)) /
KIA_CARNIVAL_HIGHWAY_TRANSITION_LAT_WIDTH)
return 1.0 - (KIA_CARNIVAL_HIGHWAY_TRANSITION_TAPER_MAX * speed_weight * jerk_weight * lat_weight)
def _tucson_4th_gen_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]:
selfdrive/controls/lib/longcontrol.py
+10 -1
View File
@@ -234,6 +234,7 @@ class LongControl:
self.pid.neg_limit = accel_limits[0]
self.pid.pos_limit = accel_limits[1]
previous_long_control_state = self.long_control_state
allow_stopping_release = self._stop_release_ready(CS, a_target, should_stop, has_lead, starpilot_toggles)
self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
should_stop, CS.brakePressed,
@@ -290,7 +291,15 @@ class LongControl:
freeze_integrator=freeze_integrator)
raw_output_accel = self._cap_positive_output_on_negative_target(raw_output_accel, a_target, error, CS)
raw_output_accel = self.vehicle_tuning.apply_pedal_long_brake_bias(raw_output_accel, a_target, CS)
raw_output_accel = self.vehicle_tuning.apply_bolt_start_handoff_floor(
raw_output_accel,
self.last_output_accel,
a_target,
CS.vEgo,
previous_long_control_state == LongCtrlState.starting,
should_stop,
has_lead,
)
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:
selfdrive/controls/lib/longcontrol_vehicle_tunes.py
+31
View File
@@ -10,6 +10,11 @@ interp = np.interp
BOLT_ACC_PEDAL_REGEN_LIMIT_BP = [0.0, 1.5, 4.0, 8.0, 15.0, 30.0]
BOLT_ACC_PEDAL_REGEN_LIMIT_V = [-0.93, -1.28, -1.98, -2.58, -2.86, -2.95]
BOLT_ACC_PEDAL_START_HANDOFF_TIME = 0.75
BOLT_ACC_PEDAL_START_HANDOFF_MAX_SPEED = 1.25
BOLT_ACC_PEDAL_START_HANDOFF_MIN_TARGET = 0.15
BOLT_ACC_PEDAL_START_HANDOFF_FLOOR_BP = [0.0, 0.5, BOLT_ACC_PEDAL_START_HANDOFF_MAX_SPEED]
BOLT_ACC_PEDAL_START_HANDOFF_FLOOR_V = [0.22, 0.18, 0.10]
NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35
NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40
GM_TRUCK_TARGET_FILTER_MIN_SPEED = 12.0
@@ -94,6 +99,32 @@ class LongControlVehicleTuning:
self.integrator_hold_frames = 0
self.gm_truck_filtered_a_target = 0.0
self.gm_truck_target_filter_initialized = False
self.bolt_start_handoff_frames = 0
def apply_bolt_start_handoff_floor(self, output_accel, last_output_accel, a_target, v_ego,
starting_handoff, should_stop, has_lead):
if not self.is_bolt_acc_pedal_friction_car:
return output_accel
if starting_handoff:
self.bolt_start_handoff_frames = int(round(BOLT_ACC_PEDAL_START_HANDOFF_TIME / DT_CTRL))
safe_to_hold = (
self.bolt_start_handoff_frames > 0 and
has_lead and
not should_stop and
a_target > BOLT_ACC_PEDAL_START_HANDOFF_MIN_TARGET and
v_ego < BOLT_ACC_PEDAL_START_HANDOFF_MAX_SPEED
)
if not safe_to_hold:
self.bolt_start_handoff_frames = 0
return output_accel
self.bolt_start_handoff_frames -= 1
speed_floor = float(interp(v_ego, BOLT_ACC_PEDAL_START_HANDOFF_FLOOR_BP,
BOLT_ACC_PEDAL_START_HANDOFF_FLOOR_V))
target_floor = min(speed_floor, max(0.0, 0.4 * float(a_target)))
return max(float(output_accel), min(float(last_output_accel), target_floor))
def shape_gm_truck_accel_target(self, a_target, v_ego, should_stop):
if not self.is_gm_stock_truck:
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+10 -1
View File
@@ -383,6 +383,7 @@ class LongitudinalMpc:
self.current_filter_time = LEAD_FILTER_TIME_LOW
self.lead_a_filter = FirstOrderFilter(0.0, self.current_filter_time, self.dt)
self.lead_v_filter = FirstOrderFilter(0.0, self.current_filter_time, self.dt)
self.duplicate_lead_x_filters = [FirstOrderFilter(0.0, 0.0, self.dt, initialized=False) for _ in range(2)]
self.duplicate_lead_a_filters = [FirstOrderFilter(0.0, 0.0, self.dt, initialized=False) for _ in range(2)]
self.duplicate_lead_v_filters = [FirstOrderFilter(0.0, 0.0, self.dt, initialized=False) for _ in range(2)]
# Slew-limited filter factor to avoid abrupt 0.50↔1.00 jumps
@@ -426,7 +427,7 @@ class LongitudinalMpc:
self.time_linearization = 0.0
self.time_integrator = 0.0
self.x0 = np.zeros(X_DIM)
for lead_filter in (*self.duplicate_lead_a_filters, *self.duplicate_lead_v_filters):
for lead_filter in (*self.duplicate_lead_x_filters, *self.duplicate_lead_a_filters, *self.duplicate_lead_v_filters):
lead_filter.x = 0.0
lead_filter.initialized = False
self.set_weights()
@@ -604,10 +605,17 @@ class LongitudinalMpc:
filter_time = max(filter_time, DUPLICATE_VISION_LEAD_FILTER_TIME)
filter_time *= self.filter_time_factor
x_filter = self.duplicate_lead_x_filters[lead_index]
a_filter = self.duplicate_lead_a_filters[lead_index]
v_filter = self.duplicate_lead_v_filters[lead_index]
x_filter.update_alpha(filter_time)
a_filter.update_alpha(filter_time)
v_filter.update_alpha(filter_time)
if x_filter.initialized and x_lead <= x_filter.x:
x_filter.x = x_lead
else:
x_filter.update(x_lead)
x_lead = x_filter.x
a_lead = a_filter.update(a_lead)
v_lead = v_filter.update(v_lead)
else:
@@ -616,6 +624,7 @@ class LongitudinalMpc:
self.lead_v_filter.update(v_lead)
a_lead = self.lead_a_filter.x
v_lead = self.lead_v_filter.x
self.duplicate_lead_x_filters[lead_index].initialized = False
self.duplicate_lead_a_filters[lead_index].initialized = False
self.duplicate_lead_v_filters[lead_index].initialized = False
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego)
selfdrive/controls/tests/test_latcontrol.py
+64 -4
View File
@@ -27,6 +27,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
clear_flm_runtime_overrides,
get_flm_runtime_overrides,
get_hkg_canfd_base_friction_threshold,
RAM_1500_BASE_LAT_ACCEL_FACTOR_MULT,
get_ram_1500_transition_output_scale,
get_subaru_impreza_pid_output_scale,
normalize_flm_overrides,
@@ -44,6 +45,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
get_bolt_2017_steer_ratio_scale,
get_bolt_2017_torque_scale,
get_bolt_2022_2023_ff_scale,
get_bolt_2022_2023_center_output_scale,
get_bolt_2022_2023_friction_scale,
get_bolt_2022_2023_friction_threshold,
get_trailer_lateral_ff_scale,
@@ -86,6 +88,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
get_kia_carnival_center_taper_scale,
get_kia_carnival_friction_center_fade_scale,
get_kia_carnival_friction_threshold,
get_kia_carnival_highway_transition_output_scale,
get_kia_stinger_2022_center_taper_scale,
get_kia_stinger_2022_friction_threshold,
get_tucson_4th_gen_center_taper_scale,
@@ -214,6 +217,21 @@ class TestLatControl:
assert get_bolt_2022_2023_ff_scale(0.6, -0.7, 6.0) < get_bolt_2022_2023_ff_scale(0.6, -0.7, 20.0)
assert get_bolt_2022_2023_ff_scale(0.14, 0.0, 30.0) < get_bolt_2022_2023_ff_scale(0.14, 0.0, 20.0)
def test_bolt_2022_2023_center_output_taper(self):
low_speed_center = get_bolt_2022_2023_center_output_scale(0.04, 10.0)
low_speed_turn = get_bolt_2022_2023_center_output_scale(0.40, 10.0)
middle_speed_center = get_bolt_2022_2023_center_output_scale(0.04, 20.0)
highway_center = get_bolt_2022_2023_center_output_scale(0.04, 31.0)
highway_turn = get_bolt_2022_2023_center_output_scale(0.40, 31.0)
creep_center = get_bolt_2022_2023_center_output_scale(0.04, 1.0)
assert 0.92 < low_speed_center < 0.95
assert low_speed_turn > 0.99
assert middle_speed_center > 0.98
assert 0.88 < highway_center < 0.91
assert highway_turn > 0.99
assert creep_center > 0.99
def test_bolt_2022_2023_friction_threshold_curve(self):
base = get_gm_base_friction_threshold(6.0)
left_turn_in = get_bolt_2022_2023_friction_threshold(6.0, 0.7, 0.8)
@@ -431,20 +449,42 @@ class TestLatControl:
neighborhood_taper = get_kia_carnival_center_taper_scale(0.04, 5.0)
neighborhood_turn_taper = get_kia_carnival_center_taper_scale(0.35, 5.0)
highway_taper = get_kia_carnival_center_taper_scale(0.04, 25.0)
high_speed_center_taper = get_kia_carnival_center_taper_scale(0.04, 32.4)
high_speed_turn_taper = get_kia_carnival_center_taper_scale(0.80, 32.4)
assert center_taper < turn_taper <= 1.0
assert center_taper < low_speed_taper <= 1.0
assert center_taper < highway_taper <= 1.0
assert center_taper < 0.84
assert neighborhood_taper < 0.94
assert neighborhood_turn_taper > 0.99
assert 0.85 < high_speed_center_taper < 0.90
assert high_speed_turn_taper > 0.99
center_threshold = get_kia_carnival_friction_threshold(8.5, 0.04)
turn_threshold = get_kia_carnival_friction_threshold(8.5, 0.35)
high_speed_center_threshold = get_kia_carnival_friction_threshold(32.4, 0.04)
high_speed_turn_threshold = get_kia_carnival_friction_threshold(32.4, 0.80)
assert center_threshold > turn_threshold >= get_hkg_canfd_base_friction_threshold(8.5)
assert high_speed_center_threshold > high_speed_turn_threshold >= get_hkg_canfd_base_friction_threshold(32.4)
center_fade = get_kia_carnival_friction_center_fade_scale(0.04, 8.5)
turn_fade = get_kia_carnival_friction_center_fade_scale(0.35, 8.5)
high_speed_center_fade = get_kia_carnival_friction_center_fade_scale(0.04, 32.4)
high_speed_turn_fade = get_kia_carnival_friction_center_fade_scale(0.80, 32.4)
assert center_fade < 0.75 < turn_fade <= 1.0
assert 0.79 < high_speed_center_fade < 0.85
assert high_speed_turn_fade > 0.99
def test_kia_carnival_highway_transition_taper(self):
smooth_curve = get_kia_carnival_highway_transition_output_scale(0.60, 0.10, 32.4)
abrupt_curve = get_kia_carnival_highway_transition_output_scale(0.60, 1.20, 32.4)
low_speed_abrupt = get_kia_carnival_highway_transition_output_scale(0.60, 1.20, 20.0)
large_curve_abrupt = get_kia_carnival_highway_transition_output_scale(1.60, 1.20, 32.4)
assert 0.75 < abrupt_curve < 0.77
assert smooth_curve > 0.96
assert low_speed_abrupt > 0.99
assert large_curve_abrupt > 0.96
def test_genesis_g90_ff_scale_curve(self):
assert get_genesis_g90_ff_scale(0.0, 0.0, 20.0) == 1.0
@@ -654,9 +694,29 @@ class TestLatControl:
)
assert controller.is_ram_1500
assert controller.torque_params.latAccelFactor == pytest.approx(2.0 * RAM_1500_BASE_LAT_ACCEL_FACTOR_MULT)
assert lac_log.active
assert tapered_output == pytest.approx(base_output * 0.5)
def test_ram_1500_transition_taper_preserves_corrective_torque(self, monkeypatch):
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(CHRYSLER.RAM_1500_5TH_GEN)
CS.steeringAngleDeg = -12.0
base_output, _, _ = controller.update(
True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles,
)
monkeypatch.setattr(latcontrol_torque, "get_ram_1500_transition_output_scale", lambda *_args: 0.5)
tapered_controller, tapered_VM, tapered_CS, tapered_params, tapered_toggles = self._build_torque_controller(
CHRYSLER.RAM_1500_5TH_GEN,
)
tapered_CS.steeringAngleDeg = -12.0
tapered_output, _, _ = tapered_controller.update(
True, tapered_CS, tapered_VM, tapered_params, False, 0.0025, False, 0.2, None, None, tapered_toggles,
)
assert base_output > 0.0
assert tapered_output == pytest.approx(base_output)
def test_ioniq_5_center_taper_curve(self):
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
@@ -1258,8 +1318,8 @@ class TestLatControl:
assert turn_in_right > steady_right
assert unwind_left < steady_left
assert unwind_right < steady_right
assert unwind_left < 1.03
assert unwind_right < 1.07
assert unwind_left < 0.98
assert unwind_right < 0.98
def test_kia_ev6_jwarm_testing_ground_phase_correction(self, monkeypatch):
clear_flm_runtime_overrides()
@@ -1274,8 +1334,8 @@ class TestLatControl:
assert get_kia_ev6_ff_scale(0.45, 0.0, 10.0) == pytest.approx(normal_steady)
assert get_kia_ev6_ff_scale(0.45, 0.7, 10.0) > normal_turn_in_left + 0.08
assert get_kia_ev6_ff_scale(-0.45, -0.7, 10.0) > normal_turn_in_right + 0.10
assert get_kia_ev6_ff_scale(0.45, -0.7, 10.0) < normal_unwind_left - 0.07
assert get_kia_ev6_ff_scale(-0.45, 0.7, 10.0) < normal_unwind_right - 0.08
assert get_kia_ev6_ff_scale(0.45, -0.7, 10.0) < normal_unwind_left - 0.04
assert get_kia_ev6_ff_scale(-0.45, 0.7, 10.0) < normal_unwind_right - 0.02
def test_kia_ev6_jwarm_abrupt_low_speed_phase_correction_is_bounded(self):
calm_low_speed = get_kia_ev6_jwarm_phase_confidence(6.0, 0.25)
selfdrive/controls/tests/test_longcontrol.py
+90
View File
@@ -306,6 +306,96 @@ def test_starting_accel_keeps_start_accel_shove_below_profile_ceiling():
assert output_accel == pytest.approx(1.5)
def test_bolt_acc_pedal_starting_handoff_keeps_small_positive_command():
CP = make_longcontrol_cp(
brand="gm",
startingState=True,
vEgoStarting=0.35,
enableGasInterceptorDEPRECATED=True,
flags=GMFlags.PEDAL_LONG.value,
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
)
CP.longitudinalTuning.kpV = [0.8]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.starting
lc.last_output_accel = 0.55
CS = car.CarState.new_message(vEgo=0.4, aEgo=1.5, brakePressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=0.55,
should_stop=False,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(vEgoStarting=0.35),
has_lead=True,
)
assert lc.long_control_state == LongCtrlState.pid
assert output_accel == pytest.approx(0.188, abs=0.01)
@pytest.mark.parametrize(("a_target", "should_stop"), ((-0.2, False), (0.55, True)))
def test_bolt_acc_pedal_starting_handoff_never_overrides_stop_request(a_target, should_stop):
CP = make_longcontrol_cp(
brand="gm",
startingState=True,
vEgoStarting=0.35,
enableGasInterceptorDEPRECATED=True,
flags=GMFlags.PEDAL_LONG.value,
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
)
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.starting
lc.last_output_accel = 0.55
CS = car.CarState.new_message(vEgo=0.4, aEgo=0.0, brakePressed=False)
CS.cruiseState.standstill = False
output_accel = lc.update(
active=True,
CS=CS,
a_target=a_target,
should_stop=should_stop,
accel_limits=(-3.0, 2.0),
starpilot_toggles=make_toggles(vEgoStarting=0.35),
has_lead=True,
)
assert output_accel <= 0.0
def test_bolt_acc_pedal_starting_handoff_floor_clears_when_lead_brakes_again():
CP = make_longcontrol_cp(
brand="gm",
startingState=True,
vEgoStarting=0.35,
enableGasInterceptorDEPRECATED=True,
flags=GMFlags.PEDAL_LONG.value,
carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
)
CP.longitudinalTuning.kpV = [0.8]
lc = LongControl(CP)
lc.long_control_state = LongCtrlState.starting
lc.last_output_accel = 0.55
CS = car.CarState.new_message(vEgo=0.4, aEgo=1.5, brakePressed=False)
CS.cruiseState.standstill = False
toggles = make_toggles(vEgoStarting=0.35)
launch_output = lc.update(True, CS, 0.55, False, (-3.0, 2.0), toggles, has_lead=True)
assert launch_output > 0.0
CS.vEgo = 0.5
CS.aEgo = 0.0
brake_output = lc.update(True, CS, -0.5, True, (-3.0, 2.0), toggles, has_lead=True)
assert lc.long_control_state == LongCtrlState.stopping
assert brake_output < 0.0
assert lc.vehicle_tuning.bolt_start_handoff_frames == 0
def test_update_requires_sustained_moderate_positive_target_to_leave_stopping():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP.longitudinalTuning.kpBP = [0.0]
selfdrive/controls/tests/test_longitudinal_planner.py
+46
View File
@@ -54,6 +54,52 @@ def test_mpc_duplicate_lead_filters_do_not_cross_contaminate_tracks():
assert mpc.duplicate_lead_v_filters[1].x == pytest.approx(28.0)
def test_mpc_duplicate_vision_filter_smooths_distance_jumps_per_track():
mpc = LongitudinalMpc()
mpc.set_cur_state(27.0, 0.0)
mpc.current_filter_time = 1.2
lead_one = make_lead(status=True, d_rel=52.0, v_lead=25.0, model_prob=1.0)
lead_two = make_lead(status=True, d_rel=70.0, v_lead=25.0, model_prob=1.0)
first_one = mpc.process_lead(lead_one, lead_index=0, smooth_duplicate_vision=True)
first_two = mpc.process_lead(lead_two, lead_index=1, smooth_duplicate_vision=True)
assert first_one[0, 0] == pytest.approx(52.0)
assert first_two[0, 0] == pytest.approx(70.0)
lead_one.dRel = 60.0
filtered_one = mpc.process_lead(lead_one, lead_index=0, smooth_duplicate_vision=True)
assert 52.0 < filtered_one[0, 0] < 54.0
assert mpc.duplicate_lead_x_filters[1].x == pytest.approx(70.0)
def test_mpc_duplicate_vision_distance_filter_bypasses_urgent_path():
mpc = LongitudinalMpc()
mpc.set_cur_state(27.0, 0.0)
mpc.current_filter_time = 1.2
lead = make_lead(status=True, d_rel=60.0, v_lead=25.0, model_prob=1.0)
mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=True)
lead.dRel = 35.0
raw = mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=False)
assert raw[0, 0] == pytest.approx(35.0)
assert not mpc.duplicate_lead_x_filters[0].initialized
def test_mpc_duplicate_vision_distance_filter_never_delays_closer_lead():
mpc = LongitudinalMpc()
mpc.set_cur_state(27.0, 0.0)
mpc.current_filter_time = 1.2
lead = make_lead(status=True, d_rel=60.0, v_lead=25.0, model_prob=1.0)
mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=True)
lead.dRel = 42.0
closer = mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=True)
assert closer[0, 0] == pytest.approx(42.0)
def test_mpc_duplicate_vision_filter_damps_low_speed_velocity_noise():
mpc = LongitudinalMpc()
mpc.set_cur_state(18.0, 0.0)
selfdrive/controls/tests/test_navigation_desires.py
+16 -2
View File
@@ -32,6 +32,7 @@ def make_toggles(**overrides):
"one_lane_change": False,
"use_turn_desires": False,
"lane_changes_require_cruise": False,
"nav_desires_allowed": True,
}
defaults.update(overrides)
return SimpleNamespace(**defaults)
@@ -493,7 +494,6 @@ def test_turn_desire_released_after_stop_completes():
def test_nav_desires_disabled_leave_desire_unchanged():
helper = DesireHelper()
helper.nav_desires_allowed = False
helper._update_nav_params = lambda: None
helper._nav_instruction_state = {"valid": True, "maneuverModifier": "left"}
@@ -502,7 +502,21 @@ def test_nav_desires_disabled_leave_desire_unchanged():
True,
0.0,
make_plan(),
make_toggles(minimum_lane_change_speed=10.0),
make_toggles(minimum_lane_change_speed=10.0, nav_desires_allowed=False),
)
assert helper.desire == log.Desire.none
def test_disabling_nav_desires_clears_active_route_desire_immediately():
helper = DesireHelper()
helper._update_nav_params = lambda: None
helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightRight"}
car_state = make_car_state(vEgo=20.0)
plan = make_plan(laneWidthRight=4.2)
helper.update(car_state, True, 0.0, plan, make_toggles(nav_desires_allowed=True))
assert helper.desire == log.Desire.keepRight
helper.update(car_state, True, 0.0, plan, make_toggles(nav_desires_allowed=False))
assert helper.desire == log.Desire.none
selfdrive/controls/tests/test_starpilot_vcruise.py
+45
View File
@@ -35,6 +35,7 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False
params_memory=FakeParams({"NavInstructionState": nav_state or {}}),
lead_one=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0),
starpilot_cem=SimpleNamespace(stop_light_detected=red_light),
starpilot_following=SimpleNamespace(following_lead=False),
tracking_lead=False,
driving_in_curve=False,
model_length=60.0,
@@ -83,6 +84,7 @@ def make_toggles():
force_stops=True,
force_standstill=False,
curve_speed_controller=False,
csc_no_lead=False,
nav_longitudinal_allowed=False,
speed_limit_controller=False,
show_speed_limits=False,
@@ -151,6 +153,49 @@ def test_curve_speed_controller_releases_immediately_when_disabled():
assert not vcruise.csc_controlling_speed
def test_curve_speed_controller_can_be_limited_to_driving_without_a_lead():
planner, vcruise = make_vcruise()
sm = make_sm(standstill=False)
toggles = make_toggles()
toggles.curve_speed_controller = True
toggles.csc_no_lead = True
def set_curve_target(_v_ego):
vcruise.csc.target_set = True
vcruise.csc.target = 14.0
vcruise.csc.update_target = set_curve_target
planner.road_curvature_detected = True
result = update_vcruise(vcruise, sm, toggles, now=30.0, v_ego=20.0)
assert result == pytest.approx(14.0)
assert vcruise.csc_controlling_speed
planner.starpilot_following.following_lead = True
result = update_vcruise(vcruise, sm, toggles, now=30.1, v_ego=20.0)
assert result == pytest.approx(20.0)
assert not vcruise.csc_controlling_speed
def test_curve_speed_controller_stays_enabled_with_a_lead_by_default():
planner, vcruise = make_vcruise()
sm = make_sm(standstill=False)
toggles = make_toggles()
toggles.curve_speed_controller = True
planner.starpilot_following.following_lead = True
planner.road_curvature_detected = True
def set_curve_target(_v_ego):
vcruise.csc.target_set = True
vcruise.csc.target = 14.0
vcruise.csc.update_target = set_curve_target
result = update_vcruise(vcruise, sm, toggles, now=40.0, v_ego=20.0)
assert result == pytest.approx(14.0)
assert vcruise.csc_controlling_speed
def test_curve_speed_controller_ramps_toward_curve_speed_at_bounded_rate():
planner = SimpleNamespace(
params=FakeParams(),
selfdrive/selfdrived/selfdrived.py
+2 -2
View File
@@ -232,10 +232,10 @@ class SelfdriveD:
self.startup_event = None
if not car_recognized:
self.startup_event = EventName.startupNoCar
elif car_recognized and self.CP.passive:
self.startup_event = EventName.startupNoControl
elif self.CP.secOcRequired and not self.CP.secOcKeyAvailable:
self.startup_event = EventName.startupNoSecOcKey
elif car_recognized and self.CP.passive:
self.startup_event = EventName.startupNoControl
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
selfdrive/ui/lib/fingerprint_catalog.py
+1 -1
View File
@@ -39,7 +39,7 @@ FINGERPRINT_MAKE_TO_VALUES_DIR = {
"volkswagen": "volkswagen",
}
_FINGERPRINT_CARDOCS_RE = re.compile(r'\w*CarDocs\(\s*"([^"]+)"')
_FINGERPRINT_CARDOCS_RE = re.compile(r'\w*CarDocs\w*\(\s*"([^"]+)"')
_FINGERPRINT_PLATFORM_RE = re.compile(r'(\w+)\s*=\s*\w+\s*\(\s*\[([\s\S]*?)\]\s*,')
_FINGERPRINT_PLATFORM_NAME_RE = re.compile(r'^[A-Z0-9_]+$')
_FINGERPRINT_VALID_NAME_RE = re.compile(r'^[A-Za-z0-9 \u0160.(),&\-]+$')
selfdrive/ui/mici/layouts/onboarding.py
+11 -1
View File
@@ -124,7 +124,7 @@ class TrainingGuideDMTutorial(NavWidget):
ui_state.params.put_bool_nonblocking("IsDriverViewEnabled", True)
sm = ui_state.sm
if sm.recv_frame.get("driverMonitoringState", 0) == 0:
if sm.recv_frame.get("driverMonitoringState", 0) == 0 or sm.recv_frame.get("driverStateV2", 0) == 0:
return
dm_state = sm["driverMonitoringState"]
@@ -351,6 +351,7 @@ class OnboardingWindow(Widget):
self._terms.set_enabled(lambda: self.enabled) # for nav stack
self._training_guide = TrainingGuide(completed_callback=self._on_completed_training)
self._training_guide.set_enabled(lambda: self.enabled) # for nav stack
self._needs_initial_push = False
def _on_uninstall(self):
ui_state.params.put_bool("DoUninstall", True)
@@ -359,6 +360,7 @@ class OnboardingWindow(Widget):
super().show_event()
device.set_override_interactive_timeout(300)
device.set_offroad_brightness(100)
self._needs_initial_push = True
def hide_event(self):
super().hide_event()
@@ -376,12 +378,20 @@ class OnboardingWindow(Widget):
def _on_terms_accepted(self):
ui_state.params.put("HasAcceptedTerms", terms_version)
self._accepted_terms = True
gui_app.push_widget(self._training_guide)
def _on_completed_training(self):
ui_state.params.put("CompletedTrainingVersion", training_version)
self._training_done = True
self.close()
def _render(self, _):
rl.draw_rectangle_rec(self._rect, rl.BLACK)
if self._needs_initial_push:
self._needs_initial_push = False
if self._accepted_terms and not self._training_done:
gui_app.push_widget(self._training_guide)
self._terms.render(self._rect)
selfdrive/ui/mici/onroad/augmented_road_view.py
+4 -3
View File
@@ -840,13 +840,14 @@ class AugmentedRoadView(CameraView):
self.switch_stream(target)
return
wide_available = WIDE_CAM in self.available_streams
if camera_view == CAMERA_VIEW_DRIVER:
target = DRIVER_CAM
elif camera_view == CAMERA_VIEW_STANDARD:
target = ROAD_CAM
elif camera_view == CAMERA_VIEW_WIDE:
target = WIDE_CAM if WIDE_CAM in self.available_streams else ROAD_CAM
elif sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams:
target = WIDE_CAM if wide_available else ROAD_CAM
elif sm['selfdriveState'].experimentalMode and wide_available:
v_ego = sm['carState'].vEgo
if v_ego < WIDE_CAM_MAX_SPEED:
target = WIDE_CAM
@@ -854,7 +855,7 @@ class AugmentedRoadView(CameraView):
target = ROAD_CAM
else:
# Hysteresis zone - keep the current road camera selection.
target = WIDE_CAM if self.stream_type == WIDE_CAM else ROAD_CAM
target = WIDE_CAM if self.stream_type == WIDE_CAM and wide_available else ROAD_CAM
else:
target = ROAD_CAM
selfdrive/ui/mici/onroad/cameraview.py
+5 -1
View File
@@ -420,8 +420,12 @@ class CameraView(Widget):
# Switch to target
self.client = self._target_client
self._stream_type = self._target_stream_type
enhance_driver_val = getattr(self, "_enhance_driver_val", None)
if enhance_driver_val is not None:
enhance_driver_val[0] = 1 if self._stream_type == VisionStreamType.VISION_STREAM_DRIVER else 0
client_frame_id = getattr(self.client, "frame_id", -1) if hasattr(self, "client") and self.client is not None else -1
self._last_frame_id = int(getattr(self.frame, "frame_id", client_frame_id)) if self.frame is not None else -1
frame = getattr(self, "frame", None)
self._last_frame_id = int(getattr(frame, "frame_id", client_frame_id)) if frame is not None else -1
self._texture_needs_update = True
# Reset state
selfdrive/ui/mici/onroad/driver_camera_dialog.py
+2
View File
@@ -162,6 +162,8 @@ class BaseDriverCameraDialog(Widget):
driver_data = self.driver_state_renderer.get_driver_data()
if not dm_state.visionPolicyState.faceDetected:
return
if len(driver_data.facePosition) < 2 or len(driver_data.faceOrientationStd) < 2:
return
# Get face position and orientation
face_x, face_y = driver_data.facePosition
selfdrive/ui/mici/tests/test_camera_cleanup.py
+27
View File
@@ -98,6 +98,33 @@ def test_stream_switch_releases_graphics_before_old_client(module):
assert events == ["graphics", "client", "initialize"]
@pytest.mark.parametrize(("target_stream", "expected"), (
(mici_cameraview.VisionStreamType.VISION_STREAM_DRIVER, 1),
(mici_cameraview.VisionStreamType.VISION_STREAM_ROAD, 0),
(mici_cameraview.VisionStreamType.VISION_STREAM_WIDE_ROAD, 0),
))
def test_mici_stream_switch_updates_driver_enhancement(target_stream, expected):
class FakeClient:
frame_id = 42
view = mici_cameraview.CameraView.__new__(mici_cameraview.CameraView)
view.client = FakeClient()
view._target_client = FakeClient()
view._target_stream_type = target_stream
view._stream_type = mici_cameraview.VisionStreamType.VISION_STREAM_DRIVER
view._switching = True
view._texture_needs_update = False
view._enhance_driver_val = [-1]
view._closed = True
view._clear_textures = lambda: None
view._initialize_textures = lambda: None
view._complete_switch()
assert view._enhance_driver_val[0] == expected
assert view._last_frame_id == -1
@pytest.mark.parametrize("module", (mici_cameraview, big_cameraview))
def test_egl_cleanup_deletes_texture_before_images(monkeypatch, module):
events = []

Some files were not shown because too many files have changed in this diff Show More