diff --git a/common/params_keys.h b/common/params_keys.h index eda2c76ef..51916d5b2 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -229,6 +229,7 @@ inline static std::unordered_map 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}}, diff --git a/docs/CARS.md b/docs/CARS.md index 6e6748d80..b06ceefef 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -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[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 USB-C coupler
- 1 VW J533 connector
- 1 comma four
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
Buy Here
||| |SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 USB-C coupler
- 1 VW J533 connector
- 1 comma four
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
Buy Here
||| |Subaru|Ascent 2019-21|All[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 Subaru A connector
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
Tools- 1 Pry Tool
- 1 Socket Wrench 8mm or 5/16" (deep)
||| +|Subaru|Ascent 2023|All[6](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 Subaru D connector
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
Tools- 1 Pry Tool
- 1 Socket Wrench 8mm or 5/16" (deep)
||| |Subaru|Crosstrek 2018-19|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 Subaru A connector
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
Tools- 1 Pry Tool
- 1 Socket Wrench 8mm or 5/16" (deep)
||| |Subaru|Crosstrek 2020-23|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 Subaru A connector
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
Tools- 1 Pry Tool
- 1 Socket Wrench 8mm or 5/16" (deep)
||| +|Subaru|Crosstrek 2025|All[6](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 Subaru D connector
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
Tools- 1 Pry Tool
- 1 Socket Wrench 8mm or 5/16" (deep)
||| |Subaru|Forester 2019-21|All[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 Subaru A connector
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
Tools- 1 Pry Tool
- 1 Socket Wrench 8mm or 5/16" (deep)
||| +|Subaru|Forester 2022-24|All[6](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 Subaru C connector
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
Tools- 1 Pry Tool
- 1 Socket Wrench 8mm or 5/16" (deep)
||| |Subaru|Impreza 2017-19|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 Subaru A connector
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
Tools- 1 Pry Tool
- 1 Socket Wrench 8mm or 5/16" (deep)
||| |Subaru|Impreza 2020-22|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 Subaru A connector
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
Tools- 1 Pry Tool
- 1 Socket Wrench 8mm or 5/16" (deep)
||| |Subaru|Legacy 2020-22|All[6](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 Subaru B connector
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
Tools- 1 Pry Tool
- 1 Socket Wrench 8mm or 5/16" (deep)
||| |Subaru|Outback 2020-22|All[6](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 Subaru B connector
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
Tools- 1 Pry Tool
- 1 Socket Wrench 8mm or 5/16" (deep)
||| +|Subaru|Outback 2023|All[6](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 Subaru D connector
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
Tools- 1 Pry Tool
- 1 Socket Wrench 8mm or 5/16" (deep)
||| |Subaru|XV 2018-19|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 Subaru A connector
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
Tools- 1 Pry Tool
- 1 Socket Wrench 8mm or 5/16" (deep)
||| |Subaru|XV 2020-21|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 Subaru A connector
- 1 comma four
- 1 comma power v3
- 1 harness box
- 1 mount
Buy Here
Tools- 1 Pry Tool
- 1 Socket Wrench 8mm or 5/16" (deep)
||| |Škoda|Fabia 2022-23[13](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,14](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 OBD-C cable (2 ft)
- 1 USB-C coupler
- 1 VW J533 connector
- 1 comma four
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
Buy Here
[15](#footnotes)||| diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 5a4132538..7c78bea69 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -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 diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index b5106d712..1863a2aaf 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -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( diff --git a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py index 9ace3fcf3..b97fed0c2 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py @@ -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) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index e79227052..573d37107 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -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, diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 5263aa9b6..0e08c8647 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -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), diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index 5d79baaaf..7a2ae77fb 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -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 diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index d523b7b4b..1f679d1f7 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -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(): diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 480980956..19905fa12 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -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( diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index 6b7b54f89..5c700868c 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -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 diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index 41378ee74..63bf1a850 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -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 diff --git a/opendbc_repo/opendbc/car/subaru/carstate.py b/opendbc_repo/opendbc/car/subaru/carstate.py index 33b9d3eac..ae679cb32 100644 --- a/opendbc_repo/opendbc/car/subaru/carstate.py +++ b/opendbc_repo/opendbc/car/subaru/carstate.py @@ -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): diff --git a/opendbc_repo/opendbc/car/subaru/fingerprints.py b/opendbc_repo/opendbc/car/subaru/fingerprints.py index 64158446b..bd48010f0 100644 --- a/opendbc_repo/opendbc/car/subaru/fingerprints.py +++ b/opendbc_repo/opendbc/car/subaru/fingerprints.py @@ -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', diff --git a/opendbc_repo/opendbc/car/subaru/interface.py b/opendbc_repo/opendbc/car/subaru/interface.py index 0790dc43f..a0181ed09 100644 --- a/opendbc_repo/opendbc/car/subaru/interface.py +++ b/opendbc_repo/opendbc/car/subaru/interface.py @@ -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 diff --git a/opendbc_repo/opendbc/car/subaru/subarucan.py b/opendbc_repo/opendbc/car/subaru/subarucan.py index 5ae7822b9..87bf82794 100644 --- a/opendbc_repo/opendbc/car/subaru/subarucan.py +++ b/opendbc_repo/opendbc/car/subaru/subarucan.py @@ -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 } diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index 45e87f111..b827baa71 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py @@ -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 diff --git a/opendbc_repo/opendbc/car/subaru/values.py b/opendbc_repo/opendbc/car/subaru/values.py index 5f5ad692e..df3cfd175 100644 --- a/opendbc_repo/opendbc/car/subaru/values.py +++ b/opendbc_repo/opendbc/car/subaru/values.py @@ -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]) + \ diff --git a/opendbc_repo/opendbc/car/tesla/teslacan.py b/opendbc_repo/opendbc/car/tesla/teslacan.py index 4bfc67a4e..e21246799 100644 --- a/opendbc_repo/opendbc/car/tesla/teslacan.py +++ b/opendbc_repo/opendbc/car/tesla/teslacan.py @@ -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, diff --git a/opendbc_repo/opendbc/car/tesla/tests/test_teslacan.py b/opendbc_repo/opendbc/car/tesla/tests/test_teslacan.py new file mode 100644 index 000000000..190685873 --- /dev/null +++ b/opendbc_repo/opendbc/car/tesla/tests/test_teslacan.py @@ -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) diff --git a/opendbc_repo/opendbc/car/tests/routes.py b/opendbc_repo/opendbc/car/tests/routes.py index bf1c862ab..029ceb963 100644 --- a/opendbc_repo/opendbc/car/tests/routes.py +++ b/opendbc_repo/opendbc/car/tests/routes.py @@ -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), diff --git a/opendbc_repo/opendbc/car/torque_data/override.toml b/opendbc_repo/opendbc/car/torque_data/override.toml index 2d5049643..df375cdbf 100644 --- a/opendbc_repo/opendbc/car/torque_data/override.toml +++ b/opendbc_repo/opendbc/car/torque_data/override.toml @@ -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] diff --git a/opendbc_repo/opendbc/car/toyota/fingerprints.py b/opendbc_repo/opendbc/car/toyota/fingerprints.py index a0abb1f47..724eecb72 100644 --- a/opendbc_repo/opendbc/car/toyota/fingerprints.py +++ b/opendbc_repo/opendbc/car/toyota/fingerprints.py @@ -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: { diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 9cce8585d..3d707d646 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -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) diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index 55679c1df..77d19bdc8 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -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(): diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index edbd83780..9b549a78e 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -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), ) diff --git a/opendbc_repo/opendbc/safety/modes/hyundai.h b/opendbc_repo/opendbc/safety/modes/hyundai.h index 8781c7b9d..bf482af1a 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai.h @@ -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 { diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_common.h b/opendbc_repo/opendbc/safety/modes/hyundai_common.h index a5604290f..9fd10ec5f 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_common.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_common.h @@ -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; } diff --git a/opendbc_repo/opendbc/safety/modes/subaru.h b/opendbc_repo/opendbc/safety/modes/subaru.h index 9be84bb62..73d481401 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru.h +++ b/opendbc_repo/opendbc/safety/modes/subaru.h @@ -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 { diff --git a/opendbc_repo/opendbc/safety/tests/libsafety/libsafety_py.py b/opendbc_repo/opendbc/safety/tests/libsafety/libsafety_py.py index df66a7fcb..9255a3835 100644 --- a/opendbc_repo/opendbc/safety/tests/libsafety/libsafety_py.py +++ b/opendbc_repo/opendbc/safety/tests/libsafety/libsafety_py.py @@ -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); diff --git a/opendbc_repo/opendbc/safety/tests/libsafety/safety.c b/opendbc_repo/opendbc/safety/tests/libsafety/safety.c index 28c50e5f5..de186e208 100644 --- a/opendbc_repo/opendbc/safety/tests/libsafety/safety.c +++ b/opendbc_repo/opendbc/safety/tests/libsafety/safety.c @@ -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; } diff --git a/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py b/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py index ef7d97027..6887988ae 100644 --- a/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py +++ b/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py @@ -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 diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai.py b/opendbc_repo/opendbc/safety/tests/test_hyundai.py index 1b641a7b0..fd2e9644c 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai.py @@ -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() diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py index 4ad6fec44..247a17b50 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py @@ -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]] diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru.py b/opendbc_repo/opendbc/safety/tests/test_subaru.py index de8086d84..be4836fe6 100755 --- a/opendbc_repo/opendbc/safety/tests/test_subaru.py +++ b/opendbc_repo/opendbc/safety/tests/test_subaru.py @@ -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() diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 3cbac2c6e..06c831176 100644 Binary files a/panda/board/obj/body_h7.bin.signed and b/panda/board/obj/body_h7.bin.signed differ diff --git a/panda/board/obj/body_h7/main.bin b/panda/board/obj/body_h7/main.bin index 98f260264..73660d1cc 100755 Binary files a/panda/board/obj/body_h7/main.bin and b/panda/board/obj/body_h7/main.bin differ diff --git a/panda/board/obj/body_h7/main.elf b/panda/board/obj/body_h7/main.elf index 96a7c4738..96981585d 100755 Binary files a/panda/board/obj/body_h7/main.elf and b/panda/board/obj/body_h7/main.elf differ diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index 48339bb2c..bc7d5faaa 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -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"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 1c61c6b83..522efbea7 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda/main.bin b/panda/board/obj/panda/main.bin index 9029efc97..a1a04f921 100755 Binary files a/panda/board/obj/panda/main.bin and b/panda/board/obj/panda/main.bin differ diff --git a/panda/board/obj/panda/main.elf b/panda/board/obj/panda/main.elf index b5789478f..50914bdbc 100755 Binary files a/panda/board/obj/panda/main.elf and b/panda/board/obj/panda/main.elf differ diff --git a/panda/board/obj/panda_can_ignition_only.bin.signed b/panda/board/obj/panda_can_ignition_only.bin.signed index 4087a716e..4764290de 100644 Binary files a/panda/board/obj/panda_can_ignition_only.bin.signed and b/panda/board/obj/panda_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_can_ignition_only/main.bin b/panda/board/obj/panda_can_ignition_only/main.bin index 011ec55b5..b696f8721 100755 Binary files a/panda/board/obj/panda_can_ignition_only/main.bin and b/panda/board/obj/panda_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_can_ignition_only/main.elf b/panda/board/obj/panda_can_ignition_only/main.elf index 4dda0f71a..b44beef9a 100755 Binary files a/panda/board/obj/panda_can_ignition_only/main.elf and b/panda/board/obj/panda_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index bbfd4caf3..906a16546 100644 Binary files a/panda/board/obj/panda_h7.bin.signed and b/panda/board/obj/panda_h7.bin.signed differ diff --git a/panda/board/obj/panda_h7/main.bin b/panda/board/obj/panda_h7/main.bin index 89b201b93..3238ee736 100755 Binary files a/panda/board/obj/panda_h7/main.bin and b/panda/board/obj/panda_h7/main.bin differ diff --git a/panda/board/obj/panda_h7/main.elf b/panda/board/obj/panda_h7/main.elf index 73d7f0a4e..bf8373960 100755 Binary files a/panda/board/obj/panda_h7/main.elf and b/panda/board/obj/panda_h7/main.elf differ diff --git a/panda/board/obj/panda_h7_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_can_ignition_only.bin.signed index 942bb5da2..f4a93c282 100644 Binary files a/panda/board/obj/panda_h7_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_can_ignition_only/main.bin b/panda/board/obj/panda_h7_can_ignition_only/main.bin index cb51b0fa9..db0e71ce0 100755 Binary files a/panda/board/obj/panda_h7_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_can_ignition_only/main.elf b/panda/board/obj/panda_h7_can_ignition_only/main.elf index caab2f5ff..d943b8b75 100755 Binary files a/panda/board/obj/panda_h7_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_h7_hkg_remote.bin.signed b/panda/board/obj/panda_h7_hkg_remote.bin.signed index fdbe8449a..9a3a004fa 100644 Binary files a/panda/board/obj/panda_h7_hkg_remote.bin.signed and b/panda/board/obj/panda_h7_hkg_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_hkg_remote/main.bin b/panda/board/obj/panda_h7_hkg_remote/main.bin index 4e70f0e8d..a81a17a1c 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote/main.bin and b/panda/board/obj/panda_h7_hkg_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_hkg_remote/main.elf b/panda/board/obj/panda_h7_hkg_remote/main.elf index 37b00b399..6a0e43516 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote/main.elf and b/panda/board/obj/panda_h7_hkg_remote/main.elf differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed index e1556f6fb..aa0bc760b 100644 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin index 6c294bc56..c320c6397 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf index 22c7a7d0b..2f3fa4653 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_h7_remote.bin.signed b/panda/board/obj/panda_h7_remote.bin.signed index 9e91933dc..3ff2d7543 100644 Binary files a/panda/board/obj/panda_h7_remote.bin.signed and b/panda/board/obj/panda_h7_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote/main.bin b/panda/board/obj/panda_h7_remote/main.bin index fe8400f04..070c3860d 100755 Binary files a/panda/board/obj/panda_h7_remote/main.bin and b/panda/board/obj/panda_h7_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_remote/main.elf b/panda/board/obj/panda_h7_remote/main.elf index 0cb856f86..852c6c90c 100755 Binary files a/panda/board/obj/panda_h7_remote/main.elf and b/panda/board/obj/panda_h7_remote/main.elf differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed index 4a52262fd..a555bfb92 100644 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin b/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin index c51a2a9a6..cfb2f4749 100755 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf b/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf index c1cbfc940..9c0aa2910 100755 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_hkg_remote.bin.signed b/panda/board/obj/panda_hkg_remote.bin.signed index b68fcffd6..f06778a0a 100644 Binary files a/panda/board/obj/panda_hkg_remote.bin.signed and b/panda/board/obj/panda_hkg_remote.bin.signed differ diff --git a/panda/board/obj/panda_hkg_remote/main.bin b/panda/board/obj/panda_hkg_remote/main.bin index 1af3b51c3..aab83b3d5 100755 Binary files a/panda/board/obj/panda_hkg_remote/main.bin and b/panda/board/obj/panda_hkg_remote/main.bin differ diff --git a/panda/board/obj/panda_hkg_remote/main.elf b/panda/board/obj/panda_hkg_remote/main.elf index 1cf13a3eb..04c496b85 100755 Binary files a/panda/board/obj/panda_hkg_remote/main.elf and b/panda/board/obj/panda_hkg_remote/main.elf differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed index ccf2776d3..ab881ed92 100644 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin index 816ebdd78..2d92644ed 100755 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf index b03354438..223cb3e50 100755 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_jungle_h7.bin.signed b/panda/board/obj/panda_jungle_h7.bin.signed index 10e536b32..2896da697 100644 Binary files a/panda/board/obj/panda_jungle_h7.bin.signed and b/panda/board/obj/panda_jungle_h7.bin.signed differ diff --git a/panda/board/obj/panda_jungle_h7/main.bin b/panda/board/obj/panda_jungle_h7/main.bin index 1390dd2be..adf3619e1 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.bin and b/panda/board/obj/panda_jungle_h7/main.bin differ diff --git a/panda/board/obj/panda_jungle_h7/main.elf b/panda/board/obj/panda_jungle_h7/main.elf index a36122235..8a60fd0f8 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.elf and b/panda/board/obj/panda_jungle_h7/main.elf differ diff --git a/panda/board/obj/panda_remote.bin.signed b/panda/board/obj/panda_remote.bin.signed index 719ea2198..ec7e90201 100644 Binary files a/panda/board/obj/panda_remote.bin.signed and b/panda/board/obj/panda_remote.bin.signed differ diff --git a/panda/board/obj/panda_remote/main.bin b/panda/board/obj/panda_remote/main.bin index c14b35aa3..bcce9fb11 100755 Binary files a/panda/board/obj/panda_remote/main.bin and b/panda/board/obj/panda_remote/main.bin differ diff --git a/panda/board/obj/panda_remote/main.elf b/panda/board/obj/panda_remote/main.elf index 886fac91b..d02121276 100755 Binary files a/panda/board/obj/panda_remote/main.elf and b/panda/board/obj/panda_remote/main.elf differ diff --git a/panda/board/obj/panda_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_remote_can_ignition_only.bin.signed index 5816b5900..5dcd75854 100644 Binary files a/panda/board/obj/panda_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_remote_can_ignition_only/main.bin b/panda/board/obj/panda_remote_can_ignition_only/main.bin index 62042d5cc..120e03890 100755 Binary files a/panda/board/obj/panda_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_remote_can_ignition_only/main.elf b/panda/board/obj/panda_remote_can_ignition_only/main.elf index a69a179c0..3b32b2c4e 100755 Binary files a/panda/board/obj/panda_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/version b/panda/board/obj/version index 9b9bec59c..3626c193d 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-0351e7d8-DEBUG \ No newline at end of file +DEV-205e4b3a-DEBUG \ No newline at end of file diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 76d30ced2..2a6b59cc3 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -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: diff --git a/selfdrive/car/redneck_cruise.py b/selfdrive/car/redneck_cruise.py index 31be4ecb4..464e3ae51 100644 --- a/selfdrive/car/redneck_cruise.py +++ b/selfdrive/car/redneck_cruise.py @@ -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 diff --git a/selfdrive/car/tests/test_redneck_cruise.py b/selfdrive/car/tests/test_redneck_cruise.py index 6df2f11aa..fcdaf4cfb 100644 --- a/selfdrive/car/tests/test_redneck_cruise.py +++ b/selfdrive/car/tests/test_redneck_cruise.py @@ -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, diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 4740fd35f..5972bda20 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index a84bcf9ba..31499adf4 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -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: diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index a2d584294..8e4a45860 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -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]: diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 67990576e..0dcbbae35 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -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: diff --git a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py index 5ca7a2f16..20ac156a4 100644 --- a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py @@ -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: diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 2e806b88a..25c562227 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -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) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 710f3ae75..6aedc6483 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -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) diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index 7db0b5f87..86327634b 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -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] diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index cd81aff5c..ab86572b8 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -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) diff --git a/selfdrive/controls/tests/test_navigation_desires.py b/selfdrive/controls/tests/test_navigation_desires.py index e2101c4bf..3b1f0ef54 100644 --- a/selfdrive/controls/tests/test_navigation_desires.py +++ b/selfdrive/controls/tests/test_navigation_desires.py @@ -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 diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 78670af9f..9558247d9 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -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(), diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 626bbba04..a211837be 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -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) diff --git a/selfdrive/ui/lib/fingerprint_catalog.py b/selfdrive/ui/lib/fingerprint_catalog.py index 38d19667f..8c96970bf 100644 --- a/selfdrive/ui/lib/fingerprint_catalog.py +++ b/selfdrive/ui/lib/fingerprint_catalog.py @@ -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.(),&\-]+$') diff --git a/selfdrive/ui/mici/layouts/onboarding.py b/selfdrive/ui/mici/layouts/onboarding.py index 054d719bd..0ed454481 100644 --- a/selfdrive/ui/mici/layouts/onboarding.py +++ b/selfdrive/ui/mici/layouts/onboarding.py @@ -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) diff --git a/selfdrive/ui/mici/onroad/augmented_road_view.py b/selfdrive/ui/mici/onroad/augmented_road_view.py index 4bf5d4f3c..ef539775f 100644 --- a/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -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 diff --git a/selfdrive/ui/mici/onroad/cameraview.py b/selfdrive/ui/mici/onroad/cameraview.py index d5f7a9d98..8cd0addf6 100644 --- a/selfdrive/ui/mici/onroad/cameraview.py +++ b/selfdrive/ui/mici/onroad/cameraview.py @@ -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 diff --git a/selfdrive/ui/mici/onroad/driver_camera_dialog.py b/selfdrive/ui/mici/onroad/driver_camera_dialog.py index 220542592..7ac9953e3 100644 --- a/selfdrive/ui/mici/onroad/driver_camera_dialog.py +++ b/selfdrive/ui/mici/onroad/driver_camera_dialog.py @@ -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 diff --git a/selfdrive/ui/mici/tests/test_camera_cleanup.py b/selfdrive/ui/mici/tests/test_camera_cleanup.py index 2164d456a..a53d5ef7a 100644 --- a/selfdrive/ui/mici/tests/test_camera_cleanup.py +++ b/selfdrive/ui/mici/tests/test_camera_cleanup.py @@ -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 = [] diff --git a/selfdrive/ui/onroad/cameraview.py b/selfdrive/ui/onroad/cameraview.py index 8be38a389..1e1f53e5c 100644 --- a/selfdrive/ui/onroad/cameraview.py +++ b/selfdrive/ui/onroad/cameraview.py @@ -310,8 +310,8 @@ class CameraView(Widget): y_data = self.frame.data[: self.frame.uv_offset] uv_data = self.frame.data[self.frame.uv_offset:] - rl.update_texture(self.texture_y, rl.ffi.cast("void *", y_data.ctypes.data)) - rl.update_texture(self.texture_uv, rl.ffi.cast("void *", uv_data.ctypes.data)) + rl.update_texture(self.texture_y, rl.ffi.cast("void *", rl.ffi.from_buffer(y_data))) + rl.update_texture(self.texture_uv, rl.ffi.cast("void *", rl.ffi.from_buffer(uv_data))) self._texture_needs_update = False # Render with shader @@ -373,7 +373,8 @@ class CameraView(Widget): self.client = self._target_client self._stream_type = self._target_stream_type 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 diff --git a/selfdrive/ui/tests/test_fingerprint_catalog.py b/selfdrive/ui/tests/test_fingerprint_catalog.py new file mode 100644 index 000000000..bdce31164 --- /dev/null +++ b/selfdrive/ui/tests/test_fingerprint_catalog.py @@ -0,0 +1,20 @@ +from openpilot.selfdrive.ui.lib.fingerprint_catalog import _extract_fingerprint_models_for_make, get_fingerprint_catalog + + +def test_tesla_hardware_specific_docs_are_available_for_manual_fingerprinting(): + tesla_models = _extract_fingerprint_models_for_make("tesla") + + assert ("TESLA_MODEL_3", "Tesla Model 3 (with HW3) 2019-23", "Tesla") in tesla_models + assert ("TESLA_MODEL_3", "Tesla Model 3 (with HW4) 2024-25", "Tesla") in tesla_models + assert ("TESLA_MODEL_Y", "Tesla Model Y (with HW3) 2020-23", "Tesla") in tesla_models + assert ("TESLA_MODEL_X", "Tesla Model X (with HW4) 2024", "Tesla") in tesla_models + + +def test_tesla_model_3_hardware_variants_remain_distinct_menu_options(): + _, models_by_make, _, _ = get_fingerprint_catalog() + model_3_options = [option for option in models_by_make["Tesla"] if option.value == "TESLA_MODEL_3"] + + assert [option.label for option in model_3_options] == [ + "Tesla Model 3 (with HW3) 2019-23", + "Tesla Model 3 (with HW4) 2024-25", + ] diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 50255c94b..00bf37ee6 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -20,7 +20,6 @@ SAFE_MODE_MANAGED_KEYS = ( "DrivingModelVersion", "ModelRandomizer", "DisableOpenpilotLongitudinal", - "ForceFingerprint", "ClusterOffset", "LateralTune", "AdvancedLateralTune", @@ -195,6 +194,10 @@ SAFE_MODE_MANAGED_KEYS = ( "LongPitch", ) +SAFE_MODE_PRESERVED_KEYS = ( + "ForceFingerprint", +) + SAFE_MODE_FIXED_VALUES = { "ExperimentalMode": False, "LongitudinalPersonality": int(log.LongitudinalPersonality.relaxed), @@ -279,11 +282,16 @@ def apply_safe_mode(params: Params, params_raw: Params, params_memory: Params | if ensure_backup: backup = _load_backup(params_raw) + preserved_entries = {key: backup[key] for key in SAFE_MODE_PRESERVED_KEYS if key in backup} missing_backup_keys = [key for key in SAFE_MODE_MANAGED_KEYS if key not in backup] - if missing_backup_keys: + if missing_backup_keys or preserved_entries: backup = dict(backup) for key in missing_backup_keys: backup[key] = _current_entry(params_raw, key) + for key, entry in preserved_entries.items(): + restore_value = entry.get("value") if entry.get("present") else None + changed |= _apply_value(params_raw, key, restore_value) + backup.pop(key, None) params_raw.put(SAFE_MODE_BACKUP_PARAM, backup) changed = True @@ -323,7 +331,7 @@ def restore_safe_mode(params_raw: Params, params_memory: Params | None = None) - _mark_toggle_update(params_memory) return changed - restore_keys = dict.fromkeys((*SAFE_MODE_MANAGED_KEYS, *backup.keys())) + restore_keys = dict.fromkeys((*SAFE_MODE_MANAGED_KEYS, *(key for key in backup if key not in SAFE_MODE_PRESERVED_KEYS))) for key in restore_keys: entry = backup.get(key, {"present": False, "value": None}) restore_value = entry.get("value") if entry.get("present") else None diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 01d28d3a3..593cbe7d2 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -832,6 +832,7 @@ class StarPilotVariables: ) toggle.curve_speed_controller = toggle.openpilot_longitudinal and self.get_value("CurveSpeedController") + toggle.csc_no_lead = self.get_value("CurveSpeedControllerNoLead", condition=toggle.curve_speed_controller) toggle.csc_status = self.get_value("ShowCSCStatus", condition=toggle.curve_speed_controller) or toggle.debug_mode custom_alerts = self.get_value("CustomAlerts") @@ -1412,7 +1413,8 @@ class StarPilotVariables: toggle.startup_alert_top = "Be ready to take over at any time" toggle.startup_alert_bottom = "Always keep hands on wheel and eyes on road" - toggle.subaru_sng = self.get_value("SubaruSNG", condition=toggle.car_make == "subaru" and not (CP.flags & SubaruFlags.GLOBAL_GEN2 or CP.flags & SubaruFlags.HYBRID)) + toggle.subaru_sng = self.get_value("SubaruSNG", condition=toggle.car_make == "subaru" and + not (CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID | SubaruFlags.LKAS_ANGLE))) toggle.subaru_sng_manual_parking_brake = self.get_value("SubaruSNGManualParkingBrake", condition=toggle.subaru_sng) toggle.jeep_brake_hold = self.get_value( diff --git a/starpilot/common/tests/test_safe_mode.py b/starpilot/common/tests/test_safe_mode.py index 7e5543d2a..d9b77a4da 100644 --- a/starpilot/common/tests/test_safe_mode.py +++ b/starpilot/common/tests/test_safe_mode.py @@ -1,5 +1,11 @@ from openpilot.common.params import UnknownKeyName -from openpilot.starpilot.common.safe_mode import _apply_value +from openpilot.starpilot.common.safe_mode import ( + SAFE_MODE_BACKUP_PARAM, + SAFE_MODE_MANAGED_KEYS, + apply_safe_mode, + restore_safe_mode, + _apply_value, +) class RemovedParamStore: @@ -7,5 +13,57 @@ class RemovedParamStore: raise UnknownKeyName(key) +class FakeParamStore: + def __init__(self, values=None): + self.values = dict(values or {}) + + def get(self, key): + return self.values.get(key) + + def get_stock_value(self, key): + return None + + def put(self, key, value): + self.values[key] = value + + def put_bool(self, key, value): + self.values[key] = bool(value) + + def remove(self, key): + self.values.pop(key, None) + + def test_apply_value_ignores_removed_param(): assert not _apply_value(RemovedParamStore(), "RemovedParam", "stale value") + + +def test_safe_mode_does_not_manage_manual_fingerprint(): + assert "ForceFingerprint" not in SAFE_MODE_MANAGED_KEYS + + +def test_safe_mode_migrates_saved_manual_fingerprint_out_of_backup(): + params = FakeParamStore() + params_raw = FakeParamStore({ + "ForceFingerprint": False, + SAFE_MODE_BACKUP_PARAM: { + "ForceFingerprint": {"present": True, "value": True}, + }, + }) + + apply_safe_mode(params, params_raw) + + assert params_raw.get("ForceFingerprint") is True + assert "ForceFingerprint" not in params_raw.get(SAFE_MODE_BACKUP_PARAM) + + +def test_safe_mode_restore_ignores_stale_manual_fingerprint_backup(): + params_raw = FakeParamStore({ + "ForceFingerprint": True, + SAFE_MODE_BACKUP_PARAM: { + "ForceFingerprint": {"present": True, "value": False}, + }, + }) + + restore_safe_mode(params_raw) + + assert params_raw.get("ForceFingerprint") is True diff --git a/starpilot/common/tests/test_starpilot_process.py b/starpilot/common/tests/test_starpilot_process.py index d74af13ea..697e759a5 100644 --- a/starpilot/common/tests/test_starpilot_process.py +++ b/starpilot/common/tests/test_starpilot_process.py @@ -152,3 +152,16 @@ def test_transition_offroad_persists_valid_gps(): ) assert params.writes == [("LastGPSPosition", json.dumps(gps_position))] + + +def test_transition_onroad_stops_dashboard_analysis(monkeypatch, tmp_path): + calls = [] + dashboard_utilities = SimpleNamespace(stop_dashboard_background_analysis=lambda: calls.append("stop")) + monkeypatch.setattr(starpilot_process, "get_dashboard_utilities", lambda: dashboard_utilities) + error_log = tmp_path / "error.txt" + error_log.write_text("old error") + + starpilot_process.transition_onroad(error_log) + + assert calls == ["stop"] + assert not error_log.exists() diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index de73c8eb6..e8bd2d075 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -424,7 +424,13 @@ class StarPilotVCruise: v_ego_diff = v_ego_cluster - v_ego # FrogsGoMoo's Curve Speed Controller - csc_available = long_control_active and v_ego > CRUISING_SPEED and starpilot_toggles.curve_speed_controller + following_lead = bool(getattr(self.starpilot_planner.starpilot_following, "following_lead", False)) + csc_available = ( + long_control_active and + v_ego > CRUISING_SPEED and + starpilot_toggles.curve_speed_controller and + (not getattr(starpilot_toggles, "csc_no_lead", False) or not following_lead) + ) csc_curve_detected = csc_available and self.starpilot_planner.road_curvature_detected if csc_curve_detected: self.csc.update_target(v_ego) diff --git a/starpilot/starpilot_process.py b/starpilot/starpilot_process.py index 935bd9b03..d46ede7a6 100644 --- a/starpilot/starpilot_process.py +++ b/starpilot/starpilot_process.py @@ -193,6 +193,7 @@ def transition_offroad(starpilot_planner, model_manager, theme_manager, thread_m thread_manager.run_with_lock(send_stats) def transition_onroad(error_log): + get_dashboard_utilities().stop_dashboard_background_analysis() if error_log.is_file(): error_log.unlink() diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json index 8a1b26770..b43fe017a 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json @@ -726,6 +726,15 @@ "parent_key": "CurveSpeedController", "settings_tier": "simple" }, + { + "key": "CurveSpeedControllerNoLead", + "label": "Only Without Lead", + "description": "Only use Curve Speed Controller when not following a lead vehicle.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "CurveSpeedController", + "settings_tier": "simple" + }, { "key": "ResetCurveData", "label": "Reset Curve Data", diff --git a/starpilot/system/the_galaxy/assets/components/tools/tuning.js b/starpilot/system/the_galaxy/assets/components/tools/tuning.js index 83dc1d775..3f18ba332 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/tuning.js +++ b/starpilot/system/the_galaxy/assets/components/tools/tuning.js @@ -1070,9 +1070,16 @@ export function Tuning() { ${() => state.status?.currentSegment ? html`

Current Segment: ${state.status.currentSegment}

+ ${state.status.segmentTimeoutSeconds ? html` +

Segments that take longer than ${safeCount(state.status.segmentTimeoutSeconds)} seconds are skipped automatically.

+ ` : ""}
` : ""} + ${() => state.status?.lastSkippedSegment ? html` +

Skipped ${state.status.lastSkippedSegment} after it exceeded the read limit.

+ ` : ""} +
diff --git a/starpilot/system/the_galaxy/flm_workspace.py b/starpilot/system/the_galaxy/flm_workspace.py index f229359a3..073c5b177 100644 --- a/starpilot/system/the_galaxy/flm_workspace.py +++ b/starpilot/system/the_galaxy/flm_workspace.py @@ -46,7 +46,7 @@ FLM_ANALYZER_PROCESS = None FLM_ANALYZER_LOCK = threading.Lock() FLM_PROGRESS_FILENAME = "progress.json" FLM_ONROAD_POLL_INTERVAL_SECONDS = 0.25 -FLM_SEGMENT_TIMEOUT_SECONDS = 180.0 +FLM_SEGMENT_TIMEOUT_SECONDS = 60.0 class FLMAnalysisCancelled(RuntimeError): @@ -2236,6 +2236,8 @@ def analyze_routes(route_names: list[str], footage_paths: list[str], feedback: d used_qlog = False processed_segments = 0 skipped_segments = 0 + last_skipped_segment = "" + last_skip_reason = "" for idx, source in enumerate(sources, start=1): _require_flm_offroad(params) _write_flm_status({ @@ -2248,6 +2250,10 @@ def analyze_routes(route_names: list[str], footage_paths: list[str], feedback: d "progress": idx - 1, "total": len(sources), "currentSegment": source.segment, + "segmentTimeoutSeconds": FLM_SEGMENT_TIMEOUT_SECONDS, + "skippedSegments": skipped_segments, + "lastSkippedSegment": last_skipped_segment, + "lastSkipReason": last_skip_reason, }) try: segment_samples, segment_car_params, segment_init, segment_control_states = _segment_samples_with_timeout(source, params) @@ -2256,11 +2262,28 @@ def analyze_routes(route_names: list[str], footage_paths: list[str], feedback: d except FLMSegmentTimeout as error: warnings.append(str(error) + " The segment was skipped.") skipped_segments += 1 + last_skipped_segment = source.segment + last_skip_reason = str(error) + _write_flm_status({ + "pid": os.getpid(), + "startedAt": time.time(), + "running": True, + "state": "analyzing", + "routes": route_names, + "segmentRanges": segment_ranges, + "progress": idx, + "total": len(sources), + "currentSegment": "", + "segmentTimeoutSeconds": FLM_SEGMENT_TIMEOUT_SECONDS, + "skippedSegments": skipped_segments, + "lastSkippedSegment": last_skipped_segment, + "lastSkipReason": last_skip_reason, + }) continue except Exception as error: - warnings.append( - f"{source.route} segment {source.segment_num} could not be read ({type(error).__name__}). The segment was skipped." - ) + last_skipped_segment = source.segment + last_skip_reason = f"Could not be read ({type(error).__name__})." + warnings.append(f"{source.route} segment {source.segment_num} {last_skip_reason} The segment was skipped.") skipped_segments += 1 continue _require_flm_offroad(params) diff --git a/starpilot/system/the_galaxy/tests/test_dashboard_stats.py b/starpilot/system/the_galaxy/tests/test_dashboard_stats.py index 0101b9e9a..9f1b999a2 100644 --- a/starpilot/system/the_galaxy/tests/test_dashboard_stats.py +++ b/starpilot/system/the_galaxy/tests/test_dashboard_stats.py @@ -248,6 +248,59 @@ class FailingPutParams(FakeParams): raise RuntimeError("unknown key") +class FakeDashboardAnalyzerProcess: + def __init__(self): + self.terminated = False + + def poll(self): + return None + + def terminate(self): + self.terminated = True + + +def test_dashboard_background_analysis_does_not_start_onroad(monkeypatch): + def fail_if_started(*args, **kwargs): + raise AssertionError("worker started onroad") + + monkeypatch.setattr(utilities, "params", FakeParams({"IsOnroad": True})) + monkeypatch.setattr(utilities.subprocess, "Popen", fail_if_started) + + started = utilities._start_dashboard_background_analysis( + ["/tmp/routes"], + [{"name": "route"}], + {}, + [{"name": "route"}], + ) + + assert started is False + + +def test_dashboard_analysis_worker_exits_onroad_before_scanning(monkeypatch): + def fail_if_scanned(*args, **kwargs): + raise AssertionError("routes scanned onroad") + + monkeypatch.setattr(utilities, "params", FakeParams({"IsOnroad": True})) + monkeypatch.setattr(utilities, "_list_dashboard_routes", fail_if_scanned) + + utilities.warm_dashboard_stats(["/tmp/routes"]) + + +def test_stop_dashboard_background_analysis_terminates_owned_worker(monkeypatch, tmp_path): + process = FakeDashboardAnalyzerProcess() + status_path = tmp_path / "dashboard_analyzer_status.json" + status_path.write_text('{"pid":123}') + monkeypatch.setattr(utilities, "_DASHBOARD_ANALYZER_PROCESS", process) + monkeypatch.setattr(utilities, "DASHBOARD_ANALYZER_STATUS_PATH", status_path) + + stopped = utilities.stop_dashboard_background_analysis() + + assert stopped is True + assert process.terminated is True + assert utilities._DASHBOARD_ANALYZER_PROCESS is None + assert not status_path.exists() + + class FakeMessage: def __init__(self, kind, log_mono_time, payload): self._kind = kind diff --git a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py index 3fe48bf4b..b596c3fed 100644 --- a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py +++ b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py @@ -53,6 +53,14 @@ def test_galaxy_layout_contains_basic_mode_controls(): assert {"GalaxyDeveloperMode", "UseOldUI"} <= sections["Developer"].keys() +def test_curve_speed_controller_no_lead_toggle_is_nested_under_csc(): + csc_no_lead = _params_by_section(_layout())["Longitudinal (Speed & Following)"]["CurveSpeedControllerNoLead"] + + assert csc_no_lead["parent_key"] == "CurveSpeedController" + assert csc_no_lead["data_type"] == "bool" + assert _declared_default("CurveSpeedControllerNoLead") == "0" + + def test_every_galaxy_setting_has_a_shared_settings_tier(): layout = _layout() tiers = { diff --git a/starpilot/system/the_galaxy/tests/test_fingerprint_catalog.py b/starpilot/system/the_galaxy/tests/test_fingerprint_catalog.py new file mode 100644 index 000000000..550869410 --- /dev/null +++ b/starpilot/system/the_galaxy/tests/test_fingerprint_catalog.py @@ -0,0 +1,23 @@ +from test_dashboard_stats import MODULE_DIR, _install_server_import_stubs + + +def _load_server_module(): + import importlib.util + + _install_server_import_stubs() + spec = importlib.util.spec_from_file_location("fingerprint_catalog_server", MODULE_DIR / "the_galaxy.py") + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +the_galaxy = _load_server_module() + + +def test_galaxy_lists_tesla_hardware_specific_docs_for_manual_fingerprinting(): + tesla_models = the_galaxy._extract_fingerprint_models_for_make("tesla") + + assert {"value": "TESLA_MODEL_3", "label": "Tesla Model 3 (with HW3) 2019-23"} in tesla_models + assert {"value": "TESLA_MODEL_3", "label": "Tesla Model 3 (with HW4) 2024-25"} in tesla_models + assert {"value": "TESLA_MODEL_Y", "label": "Tesla Model Y (with HW3) 2020-23"} in tesla_models + assert {"value": "TESLA_MODEL_X", "label": "Tesla Model X (with HW4) 2024"} in tesla_models diff --git a/starpilot/system/the_galaxy/tests/test_flm_workspace.py b/starpilot/system/the_galaxy/tests/test_flm_workspace.py index 5fd14214e..31283f6c1 100644 --- a/starpilot/system/the_galaxy/tests/test_flm_workspace.py +++ b/starpilot/system/the_galaxy/tests/test_flm_workspace.py @@ -252,6 +252,8 @@ def test_segment_reader_timeout_interrupts_stalled_log(tmp_path, monkeypatch): with pytest.raises(module.FLMSegmentTimeout, match="segment 41"): module._segment_samples_with_timeout(source, module.Params(), timeout_seconds=0.02) + assert module.FLM_SEGMENT_TIMEOUT_SECONDS == 60.0 + def test_analysis_is_rejected_while_onroad(tmp_path): module, fake_params_cls = _load_flm_workspace_module(tmp_path) diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index 54522f4a8..110524c96 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -802,7 +802,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.(),&\-]+$') diff --git a/starpilot/system/the_galaxy/utilities.py b/starpilot/system/the_galaxy/utilities.py index 6cb040e3a..33ebc12e9 100644 --- a/starpilot/system/the_galaxy/utilities.py +++ b/starpilot/system/the_galaxy/utilities.py @@ -8,6 +8,7 @@ import os import re import secrets import shutil +import signal import socket import subprocess import sys @@ -1672,6 +1673,9 @@ def _invalidate_dashboard_cache(): def warm_dashboard_stats(footage_paths=None): params_obj = params + if params_obj.get_bool("IsOnroad"): + return + route_infos = _list_dashboard_routes(footage_paths or []) if not route_infos: @@ -1689,6 +1693,8 @@ def warm_dashboard_stats(footage_paths=None): persistent_stats = _load_dashboard_persistent_stats(params_obj) candidates = _analysis_candidates(route_infos, persistent_stats)[:DASHBOARD_BACKGROUND_ROUTE_ANALYSIS_LIMIT] for route_info in candidates: + if params_obj.get_bool("IsOnroad"): + break full_route_info = dict(route_info) full_route_info["analysisSegmentCount"] = max(0, _safe_int(route_info.get("segmentCount", 0), 0)) messages = _iter_route_log_messages(full_route_info) @@ -1771,6 +1777,39 @@ def _clear_dashboard_analyzer_status(): pass +def _dashboard_analyzer_pid_matches(pid): + try: + command = Path(f"/proc/{pid}/cmdline").read_bytes() + except OSError: + return False + return b"warm_dashboard_stats" in command + + +def stop_dashboard_background_analysis(): + global _DASHBOARD_ANALYZER_PROCESS + + stopped = False + with _DASHBOARD_ANALYZER_LOCK: + process = _DASHBOARD_ANALYZER_PROCESS + if process is not None and process.poll() is None: + process.terminate() + stopped = True + else: + status = _read_dashboard_analyzer_status() + pid = _safe_int(status.get("pid", 0), 0) + if pid > 0 and _dashboard_analyzer_pid_matches(pid): + try: + os.kill(pid, signal.SIGTERM) + stopped = True + except (ProcessLookupError, PermissionError, OSError): + pass + + _DASHBOARD_ANALYZER_PROCESS = None + _clear_dashboard_analyzer_status() + + return stopped + + def _dashboard_analysis_status(candidates): pending_count = len(candidates or []) return { @@ -1784,10 +1823,12 @@ def _start_dashboard_background_analysis(footage_paths, route_infos, persistent_ global _DASHBOARD_ANALYZER_PROCESS candidates = candidates if candidates is not None else _analysis_candidates(route_infos, persistent_stats) - if not route_infos or not candidates: + if params.get_bool("IsOnroad") or not route_infos or not candidates: return False with _DASHBOARD_ANALYZER_LOCK: + if params.get_bool("IsOnroad"): + return False if _dashboard_analyzer_running(): return True repo_root = Path(__file__).resolve().parents[3] diff --git a/system/manager/manager.py b/system/manager/manager.py index 37e2c15af..f61de3020 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -309,6 +309,44 @@ def migrate_legacy_starpilot_params_cache(params: Params, legacy_cache_root: str cloudlog.exception(f"Failed to write migration flag: {STARPILOT_PARAMS_CACHE_MIGRATION_FLAG}") +def _normalize_secoc_key(candidate) -> str | None: + if isinstance(candidate, bytes): + candidate = candidate.decode("utf-8", errors="ignore") + if not isinstance(candidate, str): + return None + + candidate = candidate.strip() + try: + return candidate if len(bytes.fromhex(candidate)) == 16 else None + except ValueError: + return None + + +def migrate_legacy_secoc_key(params: Params, params_cache: Params, legacy_cache_root: str | Path) -> None: + if _normalize_secoc_key(params.get("SecOCKey")) is not None or _normalize_secoc_key(params_cache.get("SecOCKey")) is not None: + return + + legacy_cache_root = Path(legacy_cache_root) + candidates = [] + try: + candidates.append(Params(str(legacy_cache_root)).get("SecOCKey")) + except Exception: + pass + + try: + candidates.append((legacy_cache_root / "SecOCKey").read_text()) + except OSError: + pass + + for candidate in candidates: + normalized_key = _normalize_secoc_key(candidate) + if normalized_key is not None: + params.put("SecOCKey", normalized_key) + params_cache.put("SecOCKey", normalized_key) + cloudlog.warning("Recovered Toyota SecOC key from legacy params cache") + return + + def cleanup_removed_starpilot_params(params: Params, params_cache: Params) -> None: removed_keys = [] for key in STARPILOT_REMOVED_PARAM_KEYS: @@ -852,6 +890,7 @@ def manager_init() -> None: cache_params_path = Paths.params_cache_root() migrate_legacy_starpilot_params_cache(params, Paths.legacy_params_cache_root(), cache_params_path) params_cache = Params(cache_params_path, return_defaults=True) + migrate_legacy_secoc_key(params, params_cache, Paths.legacy_params_cache_root()) last_timing = _log_boot_timing("manager_init", "params_cache", manager_init_start, last_timing) # Legacy FrogPilot params are unknown to the renamed schema and would be diff --git a/system/manager/test/test_manager.py b/system/manager/test/test_manager.py index 4d80399fd..ad4a26ced 100644 --- a/system/manager/test/test_manager.py +++ b/system/manager/test/test_manager.py @@ -320,6 +320,33 @@ class TestManager: assert (new_store / "ClusterOffset").read_text() == "1.0" assert (new_store / "RemapCancelToDistance").read_text() == "0" + @pytest.mark.parametrize("direct_backup", [False, True]) + def test_migrate_legacy_secoc_key_without_starpilot_marker(self, tmp_path, direct_backup): + params = FileBackedFakeParams(tmp_path / "params") + params_cache = FileBackedFakeParams(tmp_path / "cache") + legacy_cache = tmp_path / "legacy_cache" + legacy_cache.mkdir() + legacy_store = legacy_cache if direct_backup else manager._params_store_path(legacy_cache) + legacy_store.mkdir(exist_ok=True) + (legacy_store / "SecOCKey").write_text("00112233445566778899aabbccddeeff") + + manager.migrate_legacy_secoc_key(params, params_cache, legacy_cache) + + assert params.get("SecOCKey") == "00112233445566778899aabbccddeeff" + assert params_cache.get("SecOCKey") == "00112233445566778899aabbccddeeff" + + def test_migrate_legacy_secoc_key_rejects_invalid_key(self, tmp_path): + params = FileBackedFakeParams(tmp_path / "params") + params_cache = FileBackedFakeParams(tmp_path / "cache") + legacy_cache = tmp_path / "legacy_cache" + legacy_cache.mkdir() + (legacy_cache / "SecOCKey").write_text("not-a-valid-key") + + manager.migrate_legacy_secoc_key(params, params_cache, legacy_cache) + + assert params.get("SecOCKey") is None + assert params_cache.get("SecOCKey") is None + def test_migrate_cluster_offset_default_resets_legacy_default_only(self, tmp_path, monkeypatch): monkeypatch.setattr(manager, "STARPILOT_CLUSTER_OFFSET_MIGRATION_FLAG", tmp_path / "starpilot_cluster_offset_v1")