From 1db1ff9b9156cf1f7c7747d94a6ccf2e42a15329 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 15 Sep 2026 12:15:34 -0500 Subject: [PATCH] waffles --- cereal/libcereal.a | Bin 645812 -> 645924 bytes opendbc_repo/MANIFEST.in | 1 + opendbc_repo/opendbc/car/__init__.py | 1 + .../opendbc/car/hyundai/carcontroller.py | 33 ++- opendbc_repo/opendbc/car/hyundai/carstate.py | 11 + opendbc_repo/opendbc/car/hyundai/interface.py | 13 + .../car/hyundai/tests/test_ray_pedal.py | 133 ++++++++++ opendbc_repo/opendbc/car/interfaces.py | 7 +- .../opendbc/car/tesla/carcontroller.py | 30 ++- opendbc_repo/opendbc/car/tesla/carstate.py | 106 +++++++- .../opendbc/car/tesla/fingerprints.py | 6 + opendbc_repo/opendbc/car/tesla/interface.py | 18 +- .../opendbc/car/tesla/radar_interface.py | 2 +- .../opendbc/car/tesla/teslacan_legacy.py | 53 ++++ .../car/tesla/tests/replay_hw1_route.py | 148 +++++++++++ .../opendbc/car/tesla/tests/test_hw1.py | 132 ++++++++++ opendbc_repo/opendbc/car/tesla/values.py | 16 ++ .../opendbc/car/torque_data/override.toml | 1 + .../opendbc/car/toyota/carcontroller.py | 18 +- .../opendbc/car/toyota/tests/test_toyota.py | 14 +- .../opendbc/dbc/hyundai_kia_ray_pedal.dbc | 23 ++ opendbc_repo/opendbc/dbc/tesla_can.dbc | 6 +- opendbc_repo/opendbc/safety/declarations.h | 1 + opendbc_repo/opendbc/safety/modes/hyundai.h | 60 +++++ .../opendbc/safety/modes/tesla_legacy.h | 241 ++++++++++++++++++ opendbc_repo/opendbc/safety/safety.h | 4 +- .../safety/tests/test_hyundai_ray_pedal.py | 49 ++++ .../opendbc/safety/tests/test_tesla_legacy.py | 73 ++++++ opendbc_repo/pyproject.toml | 1 + panda/board/obj/gitversion.h | 2 +- panda/board/obj/version | 2 +- selfdrive/controls/lib/desire_helper.py | 46 +++- .../controls/lib/latcontrol_vehicle_tunes.py | 33 ++- selfdrive/controls/tests/test_latcontrol.py | 7 +- .../controls/tests/test_navigation_desires.py | 70 +++++ .../controls/tests/test_starpilot_vcruise.py | 27 ++ starpilot/car/ford/lateral.py | 48 +++- starpilot/car/ford/tests/test_lateral.py | 157 +++++++++++- starpilot/controls/lib/starpilot_vcruise.py | 5 +- 39 files changed, 1546 insertions(+), 52 deletions(-) create mode 100644 opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py create mode 100644 opendbc_repo/opendbc/car/tesla/teslacan_legacy.py create mode 100644 opendbc_repo/opendbc/car/tesla/tests/replay_hw1_route.py create mode 100644 opendbc_repo/opendbc/car/tesla/tests/test_hw1.py create mode 100644 opendbc_repo/opendbc/dbc/hyundai_kia_ray_pedal.dbc create mode 100644 opendbc_repo/opendbc/safety/modes/tesla_legacy.h create mode 100644 opendbc_repo/opendbc/safety/tests/test_hyundai_ray_pedal.py create mode 100644 opendbc_repo/opendbc/safety/tests/test_tesla_legacy.py diff --git a/cereal/libcereal.a b/cereal/libcereal.a index a242c1f0f8c7a5c14e69223dcdd5d1777eabdf34..da7b1582abf1ab51979c52ffc8be8ae3f5345e4f 100644 GIT binary patch delta 2707 zcmc&#e@sS3U$@`(phtRov-)Wk{|~jd z{QqBvJ5x#|5;LtUkvy&_b**8Itzj~!g)@XRlv4?`hFO=Cg(R%?Af->N4#`*}PVEqHiY~IUWWjIuWT)2bwIQk^)S)dnU{gk9NlMf*OH^gZ z@b;<706mvh+oH;T9KRw*BKEFK^%xO-KHZLY4C|@~((fa)^9@uSm)}FsCEiiIm`uF3+Ufgz(d*TcQnK zQPi(^i}E#(Yl7mjrzs76)!-3!9Qaa3$n31mB6WIq*ma zU)BrZb%K@^GM43t2*pikCk7L^YA4-z4C*PrWV86A1iMYK6LjGZf{ILcS0FP-{3*M4 z6_xX7B&lfkypc)^_I{YZv0!)p`<0RuNXZr7IpjhpUU5wJp~EXy%l9$l6}fU2qKieE zJd7gUjKW_`t_Syu$t5GTgi#mDN*GPzb_pdlaPOtbL&08hh3MEzt{)F|kx?nf>xDiZ~ zcTSmnn3=DJ8oU?oN-lc_n<_=5Gt^99VyeD(8LAmnk#!xZTwD9*w_9gkxe?s_&FMnb*FwTz}?^Lcv& zuSjn_EA`-EJ=^vmxjVnBRq*!|XD-)u?{o{GMQ!N&J48sv2x+ zWNHlx8bz)%Z>K3X&mkzz!$c!17b4onoI*7C*tH*XK7##7J;KO`fg>#B!|fyUpu34t zKN6Z4&7w+=X3^2abO#>lpaV%qxjwJ)a-=R^1$_itdNFcTq&buMfb_B4yb?h=8LOIE z#DlVC?t=&2&H6qB+$}=L+=%E_hEeEjB^iYgM$Q}Oj7l};83ildK+Z9)stp6jxW+cz zX5^ACnc{_p4T||HHn)lQ*s!;aY7d6mxNWv?O!cD2)ixv@XX!C?9;bK=en!sV+o|Eb z(nqlLC{~>iX|C9t1}r%Y8>IP!T2F`w{tk4X5UX8rBL-dbgiL=)x@-!H#&7o`!7v3h zt{L-Kz%cg~tr*Yl4z%D@OKa3|*4`BONCmJUYSFwnv6$qwA^5Q~>t3L`P~wYYAHv*7Gi zvB8oxj_Ipnt;I8gTff@ZS@LH1UmBr}{_Q}sUqtbab^FCzgd3;)g3}h5^4o8{_zwYc B>hAym delta 2701 zcmc&#eMnVj7(eIcx$n_wUOTC&X*%W_W2TcCm!P(W){q(6NX+%&KY|M+wakwZUd@l2 zb^SWte%g@fW*aSxTkCoqeayiMM>&cSNvmP7M#Ln?CKy!gcRc69>Ywe8MF-ydJn!%K zd!FY#@B2RI4v(%p<6r4JAR59YG5P{W!;37=+6^!C?MK5K{&oA+-<{I)8x7Ah`G3g0 z`Tu`A-1$w3#hL>9%XsIOq8#W5YwHM;Icyw@I6^p-Ku4H;Y4xJFvR32Ns7RNmac5Ml zz`1Fg8hF%TuZ@&1=tH{`P>~Q4qKCIF(|x<0fnPiAYeJFSZ+|s%v7|{Wq!dY#UXo&@ zFl_C&4+Q4>?Y?j22CRQzPsR^hMM88h1+rx0ule|8Nm`>fR%pryy<%PdSW}KKU19lcDw zu~}0p>9G_Gd_-?EQc8uUlo~WrF4;AxPMbr2|E#KPAy%xfZ`YK91ux|bP03pbsr-z( z36ITzH+BgX(_hOWaxR95Dr*fXhE7o6_EPZZL!l~J1eN&Jt0$%@Qa} zSGm4nyNd7CPzRQ8r|L1It57h^g+8QPj8ld?M>jJtv|aou2hQvi;|{q5)fJ*sc0s9J zxC2?0VyB!BZ>88MkK?vsCXiZ1ZV+3l$T{IRW_`G8%%+gDhmu-&3{!;bd&reQ*-Orc zQp31#aW6q1<_t57)M_eKlvk50!(=raym!CM>n^HX8WYWvr?bG4VGo-fro3{8ci`PO-UV^8N%f65Ebs{$IiKSXEvDi>x*$1kz6XIk`WD@oT4 zB-OJ>w#5e*QEDyTjl6nh&$VzyEj+!EX7)1dujd*zVT3BVFmSP6tQ0(fnMR@|NNf@{ zajtMPrq7C%d4mLf7e<=6d()U}5?|5`9H2Nq+f+?ZWIRD}KGK?b9OLjbv(Pw>ab}yy z#f7gzV|YapTbOAOr7bKw2yct1;Wt-BV2zWd~$lM5T2d551bdq#nZ6^zj9XB%t&nTE_43nK)l^bbYT%#K~ zU1Dea{7Fk3VcMXWU2&mH>|nvfZmKho)y-{_zOmGcpH@p4AHpY)#HetzO+91sqdeS3eD^~IF@_N|;uAfYL@EO?>?C%xnHullUUiMMf4O5o9gdx#zt TZxe1S92FdDpl~$w$5;La$#j&p diff --git a/opendbc_repo/MANIFEST.in b/opendbc_repo/MANIFEST.in index a8583dc97b..f560b3a6e8 100644 --- a/opendbc_repo/MANIFEST.in +++ b/opendbc_repo/MANIFEST.in @@ -1,3 +1,4 @@ include opendbc/car/car.capnp include opendbc/car/include/c++.capnp +include opendbc/dbc/hyundai_kia_ray_pedal.dbc recursive-include opendbc/safety *.h diff --git a/opendbc_repo/opendbc/car/__init__.py b/opendbc_repo/opendbc/car/__init__.py index ce9bd1a224..a330472ef6 100644 --- a/opendbc_repo/opendbc/car/__init__.py +++ b/opendbc_repo/opendbc/car/__init__.py @@ -90,6 +90,7 @@ class Bus(StrEnum): main = auto() party = auto() ap_party = auto() + ap_pt = auto() def rate_limit(new_value, last_value, dw_step, up_step): diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 82eadae1b7..65cf38c18a 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -4,7 +4,7 @@ from dataclasses import dataclass # hkg-angle-steering-2025 branch at cc4b08625. See CREDITS.md and THIRD_PARTY_NOTICES.md. import numpy as np from opendbc.can import CANPacker -from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, rate_limit, structs +from opendbc.car import Bus, DT_CTRL, create_gas_interceptor_command, make_tester_present_msg, rate_limit, structs from opendbc.car.common.filter_simple import FirstOrderFilter from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance, get_max_angle_delta_vm, get_max_angle_vm from opendbc.car.common.conversions import Conversions as CV @@ -42,6 +42,9 @@ IONIQ_6_RESPONSE_MULTIPLIER = 1.2 IONIQ_6_CANFD_SCC_ACCEL_STEP = (6.0 / 50.0) * IONIQ_6_RESPONSE_MULTIPLIER IONIQ_6_CANFD_SCC_DECEL_STEP = (15.0 / 50.0) * IONIQ_6_RESPONSE_MULTIPLIER EV9_CANFD_SCC_DECEL_STEP = 10.0 / 50.0 +RAY_PEDAL_COMMAND_CAP = 0.35 # Ray firmware voltage scaling is route-derived; validate before raising. +RAY_PEDAL_RATE_UP = 0.012 # per 25 Hz command (0.30 normalized pedal per second) +RAY_PEDAL_RATE_DOWN = 0.06 GENESIS_G90_STOP_HOLD_SPEED_BP = [0.0, 0.03, 0.08, 0.16, 0.3, 0.5, 0.8, 1.2, 2.0, 3.0] GENESIS_G90_STOP_HOLD_ACCEL_V = [-0.10, -0.10, -0.12, -0.18, -0.30, -0.50, -0.75, -1.00, -1.40, -1.80] GENESIS_G90_STOP_HOLD_RELAX_SPEED_BP = [0.0, 0.08, 0.16, 0.3, 0.5, 0.8, 1.2, 2.0, 3.0] @@ -485,6 +488,9 @@ class CarController(CarControllerBase): CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_REFRESH_MSGS ) self._ray_lfa_packer = CANPacker("hyundai_kia_ray_lfa") if self._ray_lfa_8byte else None + self._ray_pedal = CP.carFingerprint == CAR.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED + self._ray_pedal_packer = CANPacker("hyundai_kia_ray_pedal") if self._ray_pedal else None + self._ray_pedal_gas_last = 0.0 def _update_dash_icon_state(self, CC): if CC.latActive: @@ -811,7 +817,11 @@ class CarController(CarControllerBase): if not self.long_active_ecu: if self.cancel_counter > CANCEL_BUTTON_DELAY_FRAMES: can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP)) - elif CC.cruiseControl.resume: + elif self._ray_pedal and CC.longActive and CS.out.cruiseState.enabled: + if (self.frame - self.last_button_frame) * DT_CTRL > 0.1: + can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP)) + self.last_button_frame = self.frame + elif CC.cruiseControl.resume and not self._ray_pedal: # send resume at a max freq of 10Hz if (self.frame - self.last_button_frame) * DT_CTRL > 0.1: # send 25 messages at a time to increases the likelihood of resume being accepted @@ -819,7 +829,24 @@ class CarController(CarControllerBase): if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15: self.last_button_frame = self.frame else: - can_sends.extend(self._create_can_redneck_button_messages(CS)) + if not self._ray_pedal: + can_sends.extend(self._create_can_redneck_button_messages(CS)) + + if self._ray_pedal and self.frame % 4 == 0: + pedal_ready = CS.ray_pedal_valid and CS.ray_pedal_state == 0 + pedal_active = (CC.longActive and pedal_ready and not CC.cruiseControl.override and + not CS.out.gasPressed and not CS.out.brakePressed and + not CS.out.cruiseState.enabled and CS.out.vEgo >= self.CP.minEnableSpeed) + if pedal_active: + target = float(np.clip(accel / CarControllerParams.ACCEL_MAX * RAY_PEDAL_COMMAND_CAP, + 0.0, RAY_PEDAL_COMMAND_CAP)) + self._ray_pedal_gas_last = rate_limit( + target, self._ray_pedal_gas_last, -RAY_PEDAL_RATE_DOWN, RAY_PEDAL_RATE_UP, + ) + else: + self._ray_pedal_gas_last = 0.0 + can_sends.append(create_gas_interceptor_command( + self._ray_pedal_packer, self._ray_pedal_gas_last, (self.frame // 4) & 0xF)) if self.long_active_ecu and can_canfd_blended: if blended_hda2: diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 3e361619f6..c2d58219b4 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -138,6 +138,9 @@ class CarState(CarStateBase): self.buttons_counter = 0 self.main_cruise_on = False self.main_cruise_tracking = bool(getattr(FPCP, "flags", 0) & HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING) + if CP.carFingerprint == CAR.KIA_RAY_EV: + self.ray_pedal_state = 5 + self.ray_pedal_valid = False self.cruise_info = {} self.msg_161 = {} @@ -300,6 +303,7 @@ class CarState(CarStateBase): cp = can_parsers[Bus.pt] cp_cam = can_parsers[Bus.cam] cp_alt = can_parsers.get(Bus.alt) + cp_pedal = can_parsers.get(Bus.party) if self.CP.flags & HyundaiFlags.CANFD: return self.update_canfd(can_parsers) @@ -393,6 +397,11 @@ class CarState(CarStateBase): ret.espDisabled = cp.vl["TCS11"]["TCS_PAS"] == 1 ret.espActive = cp.vl["TCS11"]["ABS_ACT"] == 1 ret.accFaulted = False if no_scc else cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED + if self.CP.carFingerprint == CAR.KIA_RAY_EV and self.CP.enableGasInterceptorDEPRECATED: + self.ray_pedal_valid = bool(cp_pedal is not None and cp_pedal.can_valid and + cp_pedal.ts_nanos["GAS_SENSOR"]["STATE"] > 0) + self.ray_pedal_state = int(cp_pedal.vl["GAS_SENSOR"]["STATE"]) if cp_pedal is not None else 5 + ret.accFaulted = not self.ray_pedal_valid or self.ray_pedal_state != 0 if self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV | HyundaiFlags.FCEV): if self.CP.flags & HyundaiFlags.FCEV: @@ -748,4 +757,6 @@ class CarState(CarStateBase): } if CP.carFingerprint in ALT_BUS_LDA_BUTTON_CARS: parsers[Bus.alt] = CANParser(DBC[CP.carFingerprint][Bus.pt], [("CLU13", 0)], 1) + if CP.carFingerprint == CAR.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED: + parsers[Bus.party] = CANParser("hyundai_kia_ray_pedal", [("GAS_SENSOR", 50)], 0) return parsers diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index a17b841f6b..8baffa0ec1 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -43,6 +43,7 @@ ENABLE_BUTTONS = (ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.can ECU_DISABLE_TIMESTAMP = 0.0 KONA_NON_SCC_FCA_RADAR_ADDR = 0x602 KIA_EV9_ACCEL_MAX = 2.2 +RAY_PEDAL_SENSOR_ADDR = 0x201 def apply_platform_longitudinal_params(ret: structs.CarParams) -> None: @@ -302,6 +303,18 @@ class CarInterface(CarInterfaceBase): elif ret.flags & HyundaiFlags.FCEV: ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.FCEV_GAS.value + if (candidate == CAR.KIA_RAY_EV and fingerprint[0].get(RAY_PEDAL_SENSOR_ADDR) == 6 and + ret.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_REFRESH_MSGS and + ret.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON): + ret.enableGasInterceptorDEPRECATED = True + ret.alphaLongitudinalAvailable = True + ret.openpilotLongitudinalControl = True + ret.pcmCruise = False + ret.radarUnavailable = True + ret.autoResumeSng = False + ret.minEnableSpeed = 5.0 # pedal-only: no commanded friction brake/standstill hold + ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.LONG.value + # Car specific configuration overrides if candidate == CAR.GENESIS_G90: diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py b/opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py new file mode 100644 index 0000000000..fe8a339619 --- /dev/null +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py @@ -0,0 +1,133 @@ +from types import SimpleNamespace + +import pytest + +from opendbc.can import CANPacker, CANParser +from opendbc.car import Bus, gen_empty_fingerprint +from opendbc.car.hyundai.carcontroller import CarController +from opendbc.car.hyundai.carstate import CarState +from opendbc.car.hyundai.interface import CarInterface +from opendbc.car.hyundai.values import CAR, DBC, HyundaiSafetyFlags +from opendbc.car.structs import CarControl + + +def ray_fingerprint(sensor_length=6, lfa_length=8): + fingerprint = gen_empty_fingerprint() + fingerprint[0][0x201] = sensor_length + fingerprint[0][0x391] = 8 + fingerprint[2][0x485] = lfa_length + return fingerprint + + +@pytest.mark.parametrize(("candidate", "fingerprint", "has_pedal"), [ + (CAR.KIA_RAY_EV, ray_fingerprint(), True), + (CAR.KIA_RAY_EV, ray_fingerprint(sensor_length=8), False), + (CAR.KIA_RAY_EV, ray_fingerprint(lfa_length=4), False), + (CAR.HYUNDAI_KONA_EV_NON_SCC, ray_fingerprint(), False), +]) +def test_ray_pedal_fingerprint_isolation(candidate, fingerprint, has_pedal): + CP = CarInterface.get_params(candidate, fingerprint, [], False, False, False, None) + assert CP.enableGasInterceptorDEPRECATED is has_pedal + assert CP.openpilotLongitudinalControl is has_pedal + if has_pedal: + assert not CP.pcmCruise + assert CP.safetyConfigs[-1].safetyParam == 0x9405 + assert CP.minEnableSpeed == 5.0 + assert not CP.autoResumeSng + FPCP = CarInterface.get_starpilot_params(candidate, fingerprint, [], CP, SimpleNamespace()) + assert FPCP.canUsePedal + assert not FPCP.pcmCruiseSpeed + assert not FPCP.redneckCruiseAvailable + else: + assert CP.pcmCruise + assert not (CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG) + + +def test_ray_pedal_safety_signature_is_unique_across_hyundai_platforms(): + for candidate in CAR: + for alpha_long in (False, True): + CP = CarInterface.get_params(candidate, ray_fingerprint(), [], + alpha_long, False, False, None) + has_ray_signature = (CP.safetyConfigs[-1].safetyParam & ~(32 | 128 | 2048)) == 0x9405 + assert has_ray_signature is (candidate == CAR.KIA_RAY_EV) + assert CP.enableGasInterceptorDEPRECATED is (candidate == CAR.KIA_RAY_EV) + + +def test_ray_pedal_parser_validates_actual_route_frames(): + CP = CarInterface.get_params(CAR.KIA_RAY_EV, ray_fingerprint(), [], False, False, False, None) + parser = CarState(CP, None).get_can_parsers(CP)[Bus.party] + assert parser.dbc_name == "hyundai_kia_ray_pedal" + # Consecutive bus-0 GAS_SENSOR frames from Sept. 15 Ray rlog segment 4. + samples = [bytes.fromhex(s) for s in ( + "01f403d55de8", "01f603d55ef1", "01f403d55f51", "01f603d3503f", + "01f903d551ab", "01f903d552a4", "01f703d55370", + )] + for idx, dat in enumerate(samples): + parser.update([(1_000_000_000 + idx * 20_000_000, [(0x201, dat, 0)])]) + assert parser.can_valid + assert parser.vl["GAS_SENSOR"]["STATE"] == 5 # FAULT_TIMEOUT: no 0x200 was sent + + prior = parser.vl_raw["GAS_SENSOR"] + bad = bytearray(samples[-1]) + bad[-1] ^= 1 + parser.update([(1_160_000_000, [(0x201, bytes(bad), 0)])]) + assert parser.vl_raw["GAS_SENSOR"] == prior + + +def test_ray_pedal_fault_clears_only_with_healthy_sensor_state(): + CP = CarInterface.get_params(CAR.KIA_RAY_EV, ray_fingerprint(), [], False, False, False, None) + state = CarState(CP, None) + parsers = state.get_can_parsers(CP) + packer = CANPacker("hyundai_kia_ray_pedal") + sensor = packer.make_can_msg("GAS_SENSOR", 0, { + "INTERCEPTOR_GAS": 0, "INTERCEPTOR_GAS2": 0, + "STATE": 0, "COUNTER_PEDAL": 1, + }) + for parser in parsers.values(): + parser.update([(1_000_000_000, [sensor])]) + ret, _ = state.update(parsers, SimpleNamespace()) + assert state.ray_pedal_valid + assert state.ray_pedal_state == 0 + assert not ret.accFaulted + + +def test_ray_controller_heartbeats_and_only_actuates_when_ready(): + CP = CarInterface.get_params(CAR.KIA_RAY_EV, ray_fingerprint(), [], False, False, False, None) + controller = CarController(DBC[CP.carFingerprint], CP) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS11", 0), ("CLU11", 0)], 0) + CS = SimpleNamespace( + lkas11=parser.vl["LKAS11"], clu11=parser.vl["CLU11"], + out=SimpleNamespace(vEgo=12.0, gasPressed=False, brakePressed=False, + cruiseState=SimpleNamespace(enabled=False)), + ray_pedal_valid=True, ray_pedal_state=5, is_metric=True, + ) + CC = SimpleNamespace( + enabled=True, longActive=True, latActive=True, + cruiseControl=SimpleNamespace(cancel=False, resume=False, override=False), + ) + hud = SimpleNamespace( + visualAlert=CarControl.HUDControl.VisualAlert.none, + leftLaneVisible=True, rightLaneVisible=True, + leftLaneDepart=False, rightLaneDepart=False, + ) + actuators = SimpleNamespace(longControlState=CarControl.Actuators.LongControlState.pid) + + def pedal_msg(accel, frame): + controller.frame = frame + messages = controller.create_can_msgs(True, 0, False, 0.0, accel, False, + hud, actuators, CS, CC, 2, 0) + return next(dat for addr, dat, bus in messages if addr == 0x200 and bus == 0) + + assert pedal_msg(2.0, 0)[:4] == bytes(4) # fault timeout: heartbeat only + CS.ray_pedal_state = 0 + assert pedal_msg(2.0, 4)[:4] != bytes(4) + CS.out.gasPressed = True + assert pedal_msg(2.0, 8)[:4] == bytes(4) + CS.out.gasPressed = False + assert pedal_msg(-1.0, 12)[:4] == bytes(4) # decel = EV lift/regen, not gas + CS.out.cruiseState.enabled = True + controller.frame = 16 + messages = controller.create_can_msgs(True, 0, False, 0.0, 2.0, False, + hud, actuators, CS, CC, 2, 0) + assert next(dat for addr, dat, bus in messages if addr == 0x200 and bus == 0)[:4] == bytes(4) + assert any(addr == 0x4F1 and bus == 0 for addr, _, bus in messages) # cancel stock CC diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index 1144714215..ece8506250 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -233,6 +233,9 @@ class CarInterfaceBase(ABC): fp_ret.flags |= int(HondaStarPilotFlags.HAS_CAMERA_MESSAGES) elif platform in HYUNDAI: + if candidate == HYUNDAI.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED: + fp_ret.canUsePedal = True + fp_ret.pcmCruiseSpeed = False if candidate in CANFD_CAR: hda2 = Ecu.adas in [fw.ecu for fw in car_fw] CAN = CanBus(None, fingerprint, bool(CP.flags & HyundaiFlags.CANFD_LKA_STEERING)) @@ -244,7 +247,9 @@ class CarInterfaceBase(ABC): if candidate != HYUNDAI.KIA_RAY_EV and not (CP.flags & HyundaiFlags.CANFD) and 0x53E in fingerprint[2]: fp_ret.flags |= HyundaiStarPilotFlags.HAS_LKAS12.value - fp_ret.redneckCruiseAvailable = bool(CP.flags & HyundaiFlags.NON_SCC) and not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) + fp_ret.redneckCruiseAvailable = (bool(CP.flags & HyundaiFlags.NON_SCC) and + not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) and + not (candidate == HYUNDAI.KIA_RAY_EV and CP.enableGasInterceptorDEPRECATED)) if fp_ret.redneckCruiseAvailable and params.get_bool("RedneckCruise"): fp_ret.pcmCruiseSpeed = False CP.openpilotLongitudinalControl = True diff --git a/opendbc_repo/opendbc/car/tesla/carcontroller.py b/opendbc_repo/opendbc/car/tesla/carcontroller.py index c29a836fba..6f75c95adb 100644 --- a/opendbc_repo/opendbc/car/tesla/carcontroller.py +++ b/opendbc_repo/opendbc/car/tesla/carcontroller.py @@ -5,9 +5,10 @@ from opendbc.car.lateral import apply_steer_angle_limits_vm from opendbc.car.interfaces import CarControllerBase from opendbc.car.tesla.coop_steering import CooperativeSteeringController from opendbc.car.tesla.teslacan import TeslaCAN +from opendbc.car.tesla.teslacan_legacy import TeslaCANRaven from opendbc.car.tesla.preap.carcontroller import PreAPLongController, init_preap_can from opendbc.car.tesla.preap.stock_cc_spoofer import StockCCSpoofer -from opendbc.car.tesla.values import CANBUS, CAR, CarControllerParams, TeslaSafetyFlags +from opendbc.car.tesla.values import CANBUS, CAR, CarControllerParams, TeslaSafetyFlags, LEGACY_CARS from opendbc.car.vehicle_model import VehicleModel def get_safety_CP(): @@ -39,6 +40,13 @@ class CarController(CarControllerBase): self.stock_cc = StockCCSpoofer() from opendbc.car.tesla.interface import CarInterface self.VM = VehicleModel(CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP)) + elif CP.carFingerprint in LEGACY_CARS: + self.packers = { + CANBUS.party: CANPacker(dbc_names[Bus.party]), + } + self.tesla_can = TeslaCANRaven(self.packers) + from opendbc.car.tesla.interface import CarInterface + self.VM = VehicleModel(CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1)) def _clear_steering_limit_info(self): self.steering_limit_info_valid = False @@ -104,9 +112,13 @@ class CarController(CarControllerBase): else: self._clear_steering_limit_info() - can_sends.append(self.tesla_can.create_steering_control(self.apply_angle_command_last, lat_active)) + if self.CP.carFingerprint in LEGACY_CARS: + cntr = (self.frame // 2) % 16 + can_sends.append(self.tesla_can.create_steering_control(cntr, self.apply_angle_command_last, lat_active)) + else: + can_sends.append(self.tesla_can.create_steering_control(self.apply_angle_command_last, lat_active)) - if self.frame % 10 == 0: + if self.frame % 10 == 0 and self.CP.carFingerprint not in LEGACY_CARS: can_sends.append(self.tesla_can.create_steering_allowed()) # Longitudinal control @@ -115,13 +127,21 @@ class CarController(CarControllerBase): state = 13 if CC.cruiseControl.cancel else 4 # 4=ACC_ON, 13=ACC_CANCEL_GENERIC_SILENT accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)) cntr = (self.frame // 4) % 8 - can_sends.append(self.tesla_can.create_longitudinal_command(state, accel, cntr, self.frame, CS.out.vEgo, CS.out.gasPressed)) + if self.CP.carFingerprint in LEGACY_CARS: + hw1_accel = accel if CC.longActive and not CC.cruiseControl.cancel else 0. + hw1_active = CC.longActive and not CC.cruiseControl.cancel + can_sends.append(self.tesla_can.create_longitudinal_command(state, hw1_accel, cntr, CS.out.vEgo, hw1_active, CS.out.gasPressed)) + else: + can_sends.append(self.tesla_can.create_longitudinal_command(state, accel, cntr, self.frame, CS.out.vEgo, CS.out.gasPressed)) else: # Increment counter so cancel is prioritized even without openpilot longitudinal if CC.cruiseControl.cancel: cntr = (CS.das_control["DAS_controlCounter"] + 1) % 8 - can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, self.frame, CS.out.vEgo, False)) + if self.CP.carFingerprint in LEGACY_CARS: + can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, CS.out.vEgo, False, CS.out.gasPressed)) + else: + can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, self.frame, CS.out.vEgo, False)) # TODO: HUD control new_actuators = actuators.as_builder() diff --git a/opendbc_repo/opendbc/car/tesla/carstate.py b/opendbc_repo/opendbc/car/tesla/carstate.py index 1748d7792b..3a83afbb8e 100644 --- a/opendbc_repo/opendbc/car/tesla/carstate.py +++ b/opendbc_repo/opendbc/car/tesla/carstate.py @@ -4,7 +4,10 @@ from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.interfaces import CarStateBase -from opendbc.car.tesla.values import DBC, CANBUS, GEAR_MAP, STEER_DISENGAGE_THRESHOLD, STEER_THRESHOLD, TeslaSafetyFlags, CAR +from opendbc.car.tesla.values import ( + DBC, CANBUS, GEAR_MAP, STEER_DISENGAGE_THRESHOLD, STEER_THRESHOLD, TeslaSafetyFlags, + CAR, LEGACY_CARS, +) from opendbc.car.tesla.preap.carstate import get_preap_can_parsers, update_preap from opendbc.car.tesla.preap.engagement import PreAPEngagement from opendbc.car.tesla.preap.nap_conf import nap_conf @@ -25,8 +28,19 @@ class CarState(CarStateBase): def __init__(self, CP, FPCP): super().__init__(CP, FPCP) self.can_define = CANDefine(DBC[CP.carFingerprint][Bus.party]) - self.shifter_values = self.can_define.dv["DI_systemStatus"]["DI_gear"] if CP.carFingerprint != CAR.TESLA_MODEL_S_PREAP else \ - self.can_define.dv["DI_torque2"]["DI_gear"] + if CP.carFingerprint in LEGACY_CARS: + self.can_define_party = CANDefine(DBC[CP.carFingerprint][Bus.party]) + self.can_define_pt = CANDefine(DBC[CP.carFingerprint][Bus.pt]) + self.can_define_chassis = CANDefine(DBC[CP.carFingerprint][Bus.chassis]) + self.can_defines = { + **self.can_define_party.dv, + **self.can_define_pt.dv, + **self.can_define_chassis.dv, + } + self.shifter_values = self.can_defines["DI_torque2"]["DI_gear"] + else: + self.shifter_values = self.can_define.dv["DI_systemStatus"]["DI_gear"] if CP.carFingerprint != CAR.TESLA_MODEL_S_PREAP else \ + self.can_define.dv["DI_torque2"]["DI_gear"] self.autopark = False self.autopark_prev = False @@ -75,6 +89,8 @@ class CarState(CarStateBase): def update(self, can_parsers, starpilot_toggles) -> structs.CarState: if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP: return update_preap(self, can_parsers) + if self.CP.carFingerprint in LEGACY_CARS: + return self.update_legacy(can_parsers) cp_party = can_parsers[Bus.party] cp_ap_party = can_parsers[Bus.ap_party] @@ -173,10 +189,94 @@ class CarState(CarStateBase): return ret, fp_ret + def update_legacy(self, can_parsers): + cp_party = can_parsers[Bus.party] + cp_ap_party = can_parsers[Bus.ap_party] + cp_pt = can_parsers[Bus.pt] + cp_ap_pt = can_parsers[Bus.ap_pt] + cp_chassis = can_parsers[Bus.chassis] + ret = structs.CarState() + fp_ret = custom.StarPilotCarState.new_message() + + # Vehicle speed + ret.vEgoRaw = cp_chassis.vl["ESP_B"]["ESP_vehicleSpeed"] * CV.KPH_TO_MS + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + + # Gas and brake + ret.gasPressed = cp_pt.vl["DI_torque1"]["DI_pedalPos"] > 0 + ret.brake = 0 + ret.brakePressed = cp_chassis.vl["BrakeMessage"]["driverBrakeStatus"] != 1 + + # Steering wheel and EPAS status + epas_status = cp_chassis.vl["EPAS_sysStatus"] + self.hands_on_level = epas_status["EPAS_handsOnLevel"] + ret.steeringAngleDeg = -epas_status["EPAS_internalSAS"] + ret.steeringRateDeg = -cp_chassis.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"] + ret.steeringTorque = -epas_status["EPAS_torsionBarTorque"] + ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > STEER_THRESHOLD, 5) + + eac_status = self.can_defines["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(epas_status["EPAS_eacStatus"]), None) + ret.steerFaultPermanent = eac_status == "EAC_FAULT" + ret.steerFaultTemporary = eac_status == "EAC_INHIBITED" + eac_error_code = self.can_defines["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(epas_status["EPAS_eacErrorCode"]), None) + ret.steeringDisengage = self.hands_on_level >= 3 or ( + eac_status == "EAC_INHIBITED" and eac_error_code == "EAC_ERROR_HIGH_ANGLE_RATE_SAFETY" + ) + + # Cruise + cruise_state = self.can_defines["DI_state"]["DI_cruiseState"].get(int(cp_chassis.vl["DI_state"]["DI_cruiseState"]), None) + speed_units = self.can_defines["DI_state"]["DI_speedUnits"].get(int(cp_chassis.vl["DI_state"]["DI_speedUnits"]), None) + cruise_enabled = cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL") + ret.cruiseState.enabled = cruise_enabled + if speed_units == "KPH": + ret.cruiseState.speed = max(cp_chassis.vl["DI_state"]["DI_hw1CruiseSet"] * CV.KPH_TO_MS, 1e-3) + elif speed_units == "MPH": + ret.cruiseState.speed = max(cp_chassis.vl["DI_state"]["DI_hw1CruiseSet"] * CV.MPH_TO_MS, 1e-3) + ret.cruiseState.available = cruise_state == "STANDBY" or ret.cruiseState.enabled + ret.cruiseState.standstill = False + ret.standstill = ret.vEgoRaw < 0.1 + ret.accFaulted = cruise_state == "FAULT" + + # Gear, body state, and safety state + ret.gearShifter = GEAR_MAP[self.can_defines["DI_torque2"]["DI_gear"].get( + int(cp_chassis.vl["DI_torque2"]["DI_gear"]), "DI_GEAR_INVALID")] + + doors = ("DOOR_STATE_FL", "DOOR_STATE_FR", "DOOR_STATE_RL", "DOOR_STATE_RR", "DOOR_STATE_FrontTrunk", "BOOT_STATE") + ret.doorOpen = any( + self.can_defines["GTW_carState"][door].get(int(cp_chassis.vl["GTW_carState"][door]), "OPEN") == "OPEN" + for door in doors + ) + ret.leftBlinker = cp_chassis.vl["GTW_carState"]["BC_indicatorLStatus"] == 1 + ret.rightBlinker = cp_chassis.vl["GTW_carState"]["BC_indicatorRStatus"] == 1 + + _ = cp_chassis.vl["SDM1"] + _ = cp_chassis.vl["RCM_status"] + sd_time = cp_chassis.ts_nanos["SDM1"]["SDM_bcklDrivStatus"] + rcm_time = cp_chassis.ts_nanos["RCM_status"]["RCM_buckleDriverStatus"] + if sd_time and cp_chassis._last_update_nanos - sd_time <= 1_000_000_000: + ret.seatbeltUnlatched = cp_chassis.vl["SDM1"]["SDM_bcklDrivStatus"] != 1 + elif rcm_time and cp_chassis._last_update_nanos - rcm_time <= 1_000_000_000: + ret.seatbeltUnlatched = cp_chassis.vl["RCM_status"]["RCM_buckleDriverStatus"] != 1 + else: + ret.seatbeltUnlatched = True + + ret.stockAeb = cp_ap_pt.vl["DAS_control"]["DAS_aebEvent"] == 1 + ret.stockLkas = cp_ap_party.vl["DAS_steeringControl"]["DAS_steeringControlType"] == 2 + self.das_control = copy.copy(cp_ap_pt.vl["DAS_control"]) + return ret, fp_ret + @staticmethod def get_can_parsers(CP): if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP: return get_preap_can_parsers(CP) + if CP.carFingerprint in LEGACY_CARS: + return { + Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.party), + Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party), + Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CANBUS.party), + Bus.ap_pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CANBUS.autopilot_party), + Bus.chassis: CANParser(DBC[CP.carFingerprint][Bus.chassis], [("SDM1", 0), ("RCM_status", 0)], CANBUS.party), + } return { Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.party), Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party) diff --git a/opendbc_repo/opendbc/car/tesla/fingerprints.py b/opendbc_repo/opendbc/car/tesla/fingerprints.py index 2149801cdf..92e00e99d4 100644 --- a/opendbc_repo/opendbc/car/tesla/fingerprints.py +++ b/opendbc_repo/opendbc/car/tesla/fingerprints.py @@ -5,6 +5,12 @@ from opendbc.car.tesla.values import CAR Ecu = CarParams.Ecu FW_VERSIONS = { + CAR.TESLA_MODEL_S_HW1: { + (Ecu.eps, 0x730, None): [ + b'1016704-00-HAA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x10\x00A', + ], + }, CAR.TESLA_MODEL_3: { (Ecu.eps, 0x730, None): [ b'TeM3_E014p10_0.0.0 (16),E014.17.00', diff --git a/opendbc_repo/opendbc/car/tesla/interface.py b/opendbc_repo/opendbc/car/tesla/interface.py index ae298bfadf..b08058abd9 100644 --- a/opendbc_repo/opendbc/car/tesla/interface.py +++ b/opendbc_repo/opendbc/car/tesla/interface.py @@ -1,9 +1,9 @@ -from opendbc.car import get_safety_config, structs +from opendbc.car import Bus, get_safety_config, structs from opendbc.car.interfaces import CarInterfaceBase from opendbc.car.tesla.carcontroller import CarController from opendbc.car.tesla.carstate import CarState from opendbc.car.tesla.radar_interface import RadarInterface -from opendbc.car.tesla.values import TeslaSafetyFlags, CAR +from opendbc.car.tesla.values import TeslaSafetyFlags, CAR, DBC, LEGACY_CARS from opendbc.car.tesla.preap.interface import get_preap_accel_limits, get_preap_params @@ -32,6 +32,20 @@ class CarInterface(CarInterfaceBase): if candidate == CAR.TESLA_MODEL_S_PREAP: return get_preap_params(ret) + if candidate in LEGACY_CARS: + ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.tesla, TeslaSafetyFlags.FLAG_HW1.value)] + ret.steerLimitTimer = 0.4 + ret.steerActuatorDelay = 0.1 + ret.steerAtStandstill = True + ret.steerControlType = structs.CarParams.SteerControlType.angle + ret.radarUnavailable = Bus.radar not in DBC[candidate] + ret.alphaLongitudinalAvailable = True + + if alpha_long: + ret.openpilotLongitudinalControl = True + ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.LONG_CONTROL.value + return ret + ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.tesla)] ret.steerLimitTimer = 0.4 diff --git a/opendbc_repo/opendbc/car/tesla/radar_interface.py b/opendbc_repo/opendbc/car/tesla/radar_interface.py index 302023169b..f2e453ebc7 100644 --- a/opendbc_repo/opendbc/car/tesla/radar_interface.py +++ b/opendbc_repo/opendbc/car/tesla/radar_interface.py @@ -16,7 +16,7 @@ class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) - self.radar_off_can = CP.radarUnavailable or CP.carFingerprint != CAR.TESLA_MODEL_S_PREAP + self.radar_off_can = CP.radarUnavailable or Bus.radar not in DBC[CP.carFingerprint] self.updated_messages: set[int] = set() self.track_id = 0 self.radar_offset = float(nap_conf.radar_offset) if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP else 0.0 diff --git a/opendbc_repo/opendbc/car/tesla/teslacan_legacy.py b/opendbc_repo/opendbc/car/tesla/teslacan_legacy.py new file mode 100644 index 0000000000..82005ff913 --- /dev/null +++ b/opendbc_repo/opendbc/car/tesla/teslacan_legacy.py @@ -0,0 +1,53 @@ +from opendbc.car.common.conversions import Conversions as CV +from opendbc.car.interfaces import V_CRUISE_MAX +from opendbc.car.tesla.values import CANBUS, CarControllerParams + + +class TeslaCANRaven: + """CAN commands used by the legacy Model S/X powertrain and EPAS buses.""" + + def __init__(self, packers): + self.packers = packers + self.CCP = CarControllerParams + self.jerk_upper = self.CCP.JERK_LIMIT_MAX + self.jerk_lower = self.CCP.JERK_LIMIT_MIN + + @staticmethod + def checksum(msg_id, dat): + return ((msg_id & 0xFF) + ((msg_id >> 8) & 0xFF) + sum(dat)) & 0xFF + + def create_steering_control(self, counter, angle, enabled): + values = { + "DAS_steeringControlCounter": counter, + "DAS_steeringAngleRequest": -angle, + "DAS_steeringHapticRequest": 0, + "DAS_steeringControlType": 1 if enabled else 0, + } + data = self.packers[CANBUS.party].make_can_msg("DAS_steeringControl", CANBUS.party, values)[1] + values["DAS_steeringControlChecksum"] = self.checksum(0x488, data[:3]) + return self.packers[CANBUS.party].make_can_msg("DAS_steeringControl", CANBUS.party, values) + + def create_longitudinal_command(self, acc_state, accel, counter, v_ego, active, gas_pressed): + set_speed = max(v_ego * CV.MS_TO_KPH, 0) + if active: + set_speed = 0 if accel < 0 else V_CRUISE_MAX + + if gas_pressed: + self.jerk_upper = self.jerk_lower = 0.0 + else: + self.jerk_lower = max(self.jerk_lower - self.CCP.JERK_RAMP_RATE, self.CCP.JERK_LIMIT_MIN) + self.jerk_upper = min(self.jerk_upper + self.CCP.JERK_RAMP_RATE, self.CCP.JERK_LIMIT_MAX) + + values = { + "DAS_setSpeed": set_speed, + "DAS_accState": acc_state, + "DAS_aebEvent": 0, + "DAS_jerkMin": self.jerk_lower, + "DAS_jerkMax": self.jerk_upper, + "DAS_accelMin": accel, + "DAS_accelMax": max(accel, 0), + "DAS_controlCounter": counter, + } + data = self.packers[CANBUS.party].make_can_msg("DAS_control", CANBUS.party, values)[1] + values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7]) + return self.packers[CANBUS.party].make_can_msg("DAS_control", CANBUS.party, values) diff --git a/opendbc_repo/opendbc/car/tesla/tests/replay_hw1_route.py b/opendbc_repo/opendbc/car/tesla/tests/replay_hw1_route.py new file mode 100644 index 0000000000..2c7a6979d7 --- /dev/null +++ b/opendbc_repo/opendbc/car/tesla/tests/replay_hw1_route.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 +"""Offline AP1/HW1 CAN, radar, controller, and panda-safety replay. + +Usage: PYTHONPATH=. python -m opendbc.car.tesla.tests.replay_hw1_route PATH_TO_RLOGS + +The supplied Pre-AP recording contains stock AP commands copied to bus 0 while +ELM327/old firmware forwarded traffic. The counterfactual run skips those copies: +the HW1 safety mode blocks stock 0x488/0x2b9 forwarding on bus 2. This is NOT a +physical HW1 drive; it cannot validate engagement or steering actuation on-car. +""" + +import argparse +from collections import Counter +from pathlib import Path + +from cereal import custom +from openpilot.tools.lib.logreader import LogReader +from opendbc.car import Bus, structs +from opendbc.car.tesla.carcontroller import CarController +from opendbc.car.tesla.carstate import CarState +from opendbc.car.tesla.interface import CarInterface +from opendbc.car.tesla.radar_interface import RadarInterface +from opendbc.car.tesla.values import CAR, DBC, TeslaSafetyFlags +from opendbc.safety.tests.libsafety import libsafety_py + + +def replay(paths: list[Path], simulate_active: bool = False): + fp = {0: {0x201: 5}, 1: {}, 2: {}} + cp = CarInterface.get_params(CAR.TESLA_MODEL_S_HW1, fp, [], True, False, False, None) + assert cp.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.tesla + assert cp.safetyConfigs[0].safetyParam == TeslaSafetyFlags.FLAG_HW1.value | TeslaSafetyFlags.LONG_CONTROL.value + safety = libsafety_py.libsafety + assert safety.set_safety_hooks(int(structs.CarParams.SafetyModel.tesla), cp.safetyConfigs[0].safetyParam) == 0 + safety.init_tests() + + parsers = CarState.get_can_parsers(cp) + cs = CarState(cp, custom.StarPilotCarParams.new_message()) + controller = CarController(DBC[CAR.TESLA_MODEL_S_HW1], cp) + active_controller = CarController(DBC[CAR.TESLA_MODEL_S_HW1], cp) if simulate_active else None + radar = RadarInterface(cp) + stats = Counter() + first_rejected = [] + active_rejected = [] + last_ap_command: dict[tuple[int, bytes], int] = {} + suppressed_examples = [] + + for path in paths: + for event in LogReader(str(path)): + if event.which() != "can": + continue + t = event.logMonoTime + frames = [(x.address, bytes(x.dat), x.src) for x in event.can] + stock_in_event = {(a, d) for a, d, b in frames if b == 2 and a in (0x488, 0x2b9)} + for a, d, b in frames: + if b == 2 and a in (0x488, 0x2b9): + last_ap_command[(a, d)] = t + if b == 0 and a in (0x488, 0x2b9): + seen = last_ap_command.get((a, d), -1) + if (a, d) in stock_in_event or (0 <= t - seen < 250_000_000): + stats["suppressed_bus0_stock_copies"] += 1 + continue + stats["unmatched_bus0_stock_commands"] += 1 + if len(suppressed_examples) < 5: + suppressed_examples.append((path.name, t, hex(a), d.hex())) + if b < 128: + stats["physical_rx"] += 1 + if not safety.safety_rx_hook(libsafety_py.make_CANPacket(a, b, d)): + stats["rx_rejected"] += 1 + if b == 2 and a in (0x488, 0x2b9): + stats["stock_forward_blocked"] += safety.safety_fwd_hook(b, a) == -1 + + safety.set_timer((t // 1000) & 0xffffffff) + safety.safety_tick_current_safety_config() + stats["safety_invalid_ticks"] += not safety.safety_config_valid() + stats["relay_malfunction_ticks"] += safety.get_relay_malfunction() + stats["controls_allowed_ticks"] += safety.get_controls_allowed() + + batch = [(t, frames)] + for parser in parsers.values(): + parser.update(batch) + stats["invalid_car_parser_ticks"] += not parser.can_valid + out, _ = cs.update(parsers, None) + cs.out = out + stats["carstate_faulted_ticks"] += out.accFaulted + stats["seatbelt_unlatched_ticks"] += out.seatbeltUnlatched + stats["steering_inhibited_ticks"] += out.steerFaultTemporary + stats["cruise_engaged_ticks"] += out.cruiseState.enabled + + radar_data = radar.update(batch) + if radar_data is not None: + stats["radar_updates"] += 1 + stats["radar_points"] += len(radar_data.points) + stats["radar_error_updates"] += radar_data.errors.canError or radar_data.errors.radarFault + + cc = structs.CarControl.new_message() + cc.actuators.steeringAngleDeg = out.steeringAngleDeg + cc.actuators.accel = 0. + # Do not fabricate engagement on the actual faulted/standby route. + _, sends = controller.update(cc.as_reader(), cs, t, None) + for a, d, b in sends: + stats["generated_tx"] += 1 + stats[f"generated_{hex(a)}"] += 1 + if not safety.safety_tx_hook(libsafety_py.make_CANPacket(a, b, d)): + stats["tx_rejected"] += 1 + if len(first_rejected) < 5: + first_rejected.append((path.name, t, hex(a), d.hex(), out.steeringAngleDeg, safety.get_relay_malfunction())) + + if active_controller is not None: + # A synthetic gate test only. This recording never engaged cruise, so + # enabling controls here does NOT represent an actual car-state transition. + eligible = (not (out.steerFaultTemporary or out.steerFaultPermanent or out.steeringDisengage or out.accFaulted or + out.gasPressed or out.brakePressed or out.stockAeb or out.stockLkas) and out.vEgoRaw > 2.) + simulated = structs.CarControl.new_message() + simulated.latActive = eligible + simulated.longActive = eligible + simulated.actuators.steeringAngleDeg = out.steeringAngleDeg + simulated.actuators.accel = 0.5 if eligible else 0. + _, active_sends = active_controller.update(simulated.as_reader(), cs, t, None) + if eligible: + stats["simulated_eligible_ticks"] += 1 + safety.set_controls_allowed(True) + for a, d, b in active_sends: + stats["simulated_tx"] += 1 + stats[f"simulated_{hex(a)}"] += 1 + if not safety.safety_tx_hook(libsafety_py.make_CANPacket(a, b, d)): + stats["simulated_tx_rejected"] += 1 + if len(active_rejected) < 5: + active_rejected.append((path.name, t, hex(a), d.hex(), out.steeringAngleDeg, out.vEgoRaw)) + safety.set_controls_allowed(False) + stats["can_events"] += 1 + print(f"{path.name}: {dict(stats)}", flush=True) + + print(f"unmatched bus-0 command examples: {suppressed_examples}") + print(f"rejected TX examples: {first_rejected}") + print(f"rejected synthetic-active TX examples: {active_rejected}") + print(f"final: {dict(stats)}") + return stats + + +if __name__ == "__main__": + argp = argparse.ArgumentParser(description=__doc__) + argp.add_argument("rlogs", type=Path, help="directory containing segment rlog.zst files") + argp.add_argument("--simulate-active", action="store_true", help="force safety engagement only on healthy standby samples") + args = argp.parse_args() + files = sorted(args.rlogs.glob("*.rlog.zst")) + if not files: + argp.error("no *.rlog.zst files found") + replay(files, args.simulate_active) diff --git a/opendbc_repo/opendbc/car/tesla/tests/test_hw1.py b/opendbc_repo/opendbc/car/tesla/tests/test_hw1.py new file mode 100644 index 0000000000..ca610a3344 --- /dev/null +++ b/opendbc_repo/opendbc/car/tesla/tests/test_hw1.py @@ -0,0 +1,132 @@ +import pytest + +from cereal import custom + +from opendbc.can import CANPacker, CANParser +from opendbc.car import Bus, structs +from opendbc.car.fw_versions import match_fw_to_car +from opendbc.car.tesla.carcontroller import CarController +from opendbc.car.tesla.carstate import CarState +from opendbc.car.tesla.fingerprints import FW_VERSIONS +from opendbc.car.tesla.interface import CarInterface +from opendbc.car.tesla.teslacan_legacy import TeslaCANRaven +from opendbc.car.tesla.values import CANBUS, CAR, DBC, TeslaSafetyFlags + + +def test_hw1_is_distinct_from_preap_and_does_not_change_can_buses(): + preap = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP) + hw1 = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1) + + assert preap.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.teslaPreAP + assert hw1.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.tesla + assert hw1.safetyConfigs[0].safetyParam == TeslaSafetyFlags.FLAG_HW1.value + assert CANBUS.party == 0 and CANBUS.autopilot_party == 2 + assert CarState.get_can_parsers(hw1)[Bus.ap_pt].bus == 2 + assert CarState.get_can_parsers(hw1)[Bus.chassis].message_states[0x211].ignore_alive + assert CarState.get_can_parsers(preap)[Bus.party].bus == 0 + + +def test_hw1_requires_explicit_alpha_long_for_acceleration(): + ret = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1) + hw1 = CarInterface._get_params(ret, CAR.TESLA_MODEL_S_HW1, {0: {0x201: 5}}, [], True, False, False) + assert hw1.openpilotLongitudinalControl + assert hw1.safetyConfigs[0].safetyParam == TeslaSafetyFlags.FLAG_HW1.value | TeslaSafetyFlags.LONG_CONTROL.value + + +def test_ap1_eps_fw_matches_hw1_without_matching_preap(): + version = FW_VERSIONS[CAR.TESLA_MODEL_S_HW1][(structs.CarParams.Ecu.eps, 0x730, None)][0] + fw = structs.CarParams.CarFw(ecu=structs.CarParams.Ecu.eps, address=0x730, brand="tesla", fwVersion=version) + exact, candidates = match_fw_to_car([fw], "", log=False) + assert exact and candidates == {CAR.TESLA_MODEL_S_HW1} + + +def test_hw1_display_and_cruise_bytes_do_not_change_preap_signals(): + # Captured bus-0 DI_state (0x368) from the AP1 route: display 9 MPH, set speed 10 MPH. + parser = CANParser("tesla_can", [(0x368, 0)], 0) + parser.message_states[0x368].ignore_counter = True + frames = [(0x368, bytes.fromhex("84185e3009980a2d"), 0)] + parser.update([(1_000_000_000, frames)]) + parser.update([(2_000_000_000, frames)]) + state = parser.vl["DI_state"] + assert state["DI_hw1DigitalSpeed"] == 9 + assert state["DI_hw1CruiseSet"] == 10 + assert state["DI_digitalSpeed"] == 10 + assert state["DI_cruiseSet"] != 10 + + +def test_hw1_packer_emits_bus_zero_with_matching_checksums(): + packer = CANPacker(DBC[CAR.TESLA_MODEL_S_HW1][Bus.party]) + tesla_can = TeslaCANRaven({CANBUS.party: packer}) + for msg, expected_addr, checksum_index in ( + (tesla_can.create_steering_control(0, 0, False), 0x488, 3), + (tesla_can.create_longitudinal_command(13, 0, 0, 10, False, False), 0x2b9, 7), + ): + addr, data, bus = msg + assert addr == expected_addr and bus == 0 + assert data[checksum_index] == TeslaCANRaven.checksum(addr, data[:checksum_index]) + + +def test_hw1_cancel_clears_acceleration_and_does_not_request_max_speed(): + cp = CarInterface._get_params(CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1), + CAR.TESLA_MODEL_S_HW1, {0: {}}, [], True, False, False) + controller = CarController(DBC[CAR.TESLA_MODEL_S_HW1], cp) + state = CarState(cp, custom.StarPilotCarParams.new_message()) + state.out.vEgo = 10. + cc = structs.CarControl.new_message() + cc.longActive = True + cc.cruiseControl.cancel = True + cc.actuators.accel = 2. + _, sends = controller.update(cc.as_reader(), state, 0, None) + _, data, bus = next(msg for msg in sends if msg[0] == 0x2b9) + assert bus == 0 + parser = CANParser("tesla_can", [(0x2b9, 0)], 0) + parser.update([(1_000_000_000, [(0x2b9, data, bus)])]) + decoded = parser.vl["DAS_control"] + assert decoded["DAS_accState"] == 13 + assert decoded["DAS_accelMin"] == pytest.approx(0, abs=0.05) + assert decoded["DAS_accelMax"] == pytest.approx(0, abs=0.05) + assert decoded["DAS_setSpeed"] != 200 + + +def test_hw1_carstate_uses_ap1_powertrain_and_chassis(): + cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1) + parsers = CarState.get_can_parsers(cp) + frames = [ + (0x155, bytes.fromhex("000000000005e308"), 0), # ESP speed 15.07 kph + (0x368, bytes.fromhex("84185e3009980a2d"), 0), + (0x201, bytes.fromhex("5444008df2"), 0), + ] + for parser in parsers.values(): + for addr in (0x155, 0x368, 0x201): + _ = parser.vl[addr] + parser.message_states[addr].ignore_counter = True + parser.message_states[addr].ignore_checksum = True + parser.update([(1_000_000_000, frames)]) + parser.update([(2_000_000_000, frames)]) + state = CarState(cp, custom.StarPilotCarParams.new_message()) + ret, _ = state.update(parsers, None) + assert not ret.cruiseState.enabled + assert ret.cruiseState.speed == pytest.approx(10 * 0.44704) + assert ret.vEgoRaw == pytest.approx(15.07 / 3.6) + assert not ret.seatbeltUnlatched + + # A stale belt frame cannot allow an engagement indefinitely. + for parser in parsers.values(): + parser.update([(4_000_000_000, [])]) + ret, _ = state.update(parsers, None) + assert ret.seatbeltUnlatched + + +def test_hw1_can_use_rcm_buckle_when_sdm1_is_absent(): + cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_HW1) + parsers = CarState.get_can_parsers(cp) + packer = CANPacker(DBC[CAR.TESLA_MODEL_S_HW1][Bus.chassis]) + addr, data, bus = packer.make_can_msg("RCM_status", 0, {"RCM_buckleDriverStatus": 1}) + assert addr == 0x211 and bus == 0 + frames = [(addr, data, bus)] + for parser in parsers.values(): + _ = parser.vl["RCM_status"] + parser.update([(1_000_000_000, frames)]) + state = CarState(cp, custom.StarPilotCarParams.new_message()) + out, _ = state.update(parsers, None) + assert not out.seatbeltUnlatched diff --git a/opendbc_repo/opendbc/car/tesla/values.py b/opendbc_repo/opendbc/car/tesla/values.py index 92b35920b0..50c74f381f 100644 --- a/opendbc_repo/opendbc/car/tesla/values.py +++ b/opendbc_repo/opendbc/car/tesla/values.py @@ -70,6 +70,16 @@ class CAR(Platforms): Bus.radar: 'tesla_radar_bosch_generated', }, ) + TESLA_MODEL_S_HW1 = TeslaPlatformConfig( + [CarDocs("Tesla Model S (with HW1) 2014-16", "All", support_type=SupportType.COMMUNITY, support_link="#community")], + CarSpecs(mass=2100., wheelbase=2.960, steerRatio=15.0), + { + Bus.chassis: 'tesla_can', + Bus.party: 'tesla_can', + Bus.pt: 'tesla_can', + Bus.radar: 'tesla_radar_bosch_generated', + }, + ) FW_QUERY_CONFIG = FwQueryConfig( @@ -125,10 +135,14 @@ class CarControllerParams: ACCEL_MAX = 2.0 # m/s^2 ACCEL_MIN = -3.48 # m/s^2 JERK_LIMIT_MAX = 4.9 # m/s^3, ACC faults at 5.0 + JERK_LIMIT_MIN = -4.9 # m/s^3, ACC faults at 5.0 + JERK_RAMP_RATE = JERK_LIMIT_MAX * 0.002 class TeslaSafetyFlags(IntFlag): LONG_CONTROL = 1 + FLAG_EXTERNAL_PANDA = 4 + FLAG_HW1 = 8 COOP_STEERING = 256 @@ -157,5 +171,7 @@ class CruiseButtons: DBC = CAR.create_dbc_map() +LEGACY_CARS = (CAR.TESLA_MODEL_S_HW1,) + STEER_THRESHOLD = 1 STEER_DISENGAGE_THRESHOLD = 5.0 diff --git a/opendbc_repo/opendbc/car/torque_data/override.toml b/opendbc_repo/opendbc/car/torque_data/override.toml index 862aec2c0a..33c6304673 100644 --- a/opendbc_repo/opendbc/car/torque_data/override.toml +++ b/opendbc_repo/opendbc/car/torque_data/override.toml @@ -26,6 +26,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "TESLA_MODEL_3" = [nan, 2.5, nan] "TESLA_MODEL_Y" = [nan, 2.5, nan] "TESLA_MODEL_X" = [nan, 2.5, nan] +"TESLA_MODEL_S_HW1" = [nan, 2.5, nan] # Guess "VOLKSWAGEN_ID3_MK1" = [nan, 2.5, nan] diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 10ad8cdced..da4eba6054 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -47,7 +47,6 @@ TOYOTA_AUTO_HOLD_ACTIVATION_FRAMES = 100 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long MAX_STEER_RATE = 100 # deg/s MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before torque can be cut -TOYOTA_COROLLA_TSS2_MAX_STEER_RATE_FRAMES = 8 # EPS allows user torque above threshold for 50 frames before permanently faulting MAX_USER_TORQUE = 500 @@ -78,9 +77,13 @@ def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool: ) or highlander_sdsu) -def get_steer_rate_limit_frames(car_fingerprint) -> int: - return (TOYOTA_COROLLA_TSS2_MAX_STEER_RATE_FRAMES - if car_fingerprint == CAR.TOYOTA_COROLLA_TSS2 else MAX_STEER_RATE_FRAMES) +def apply_toyota_corolla_steer_rate_guard(car_fingerprint, steering_rate_deg: float, lat_active: bool, + apply_torque: int, apply_steer_req: bool) -> tuple[int, bool]: + if (car_fingerprint == CAR.TOYOTA_COROLLA_TSS2 and lat_active and + abs(steering_rate_deg) >= MAX_STEER_RATE): + return 0, True + + return apply_torque, apply_steer_req def supports_toyota_auto_hold(CP, auto_hold_enabled: bool) -> bool: @@ -250,7 +253,6 @@ class CarController(CarControllerBase): self.standstill_req = False self.permit_braking = True self.steer_rate_counter = 0 - self.steer_rate_limit_frames = get_steer_rate_limit_frames(self.CP.carFingerprint) self.distance_button = 0 # *** start long control state *** @@ -370,7 +372,11 @@ class CarController(CarControllerBase): # >100 degree/sec steering fault prevention self.steer_rate_counter, apply_steer_req = common_fault_avoidance( abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE, lat_active, - self.steer_rate_counter, self.steer_rate_limit_frames, + self.steer_rate_counter, MAX_STEER_RATE_FRAMES, + ) + + apply_torque, apply_steer_req = apply_toyota_corolla_steer_rate_guard( + self.CP.carFingerprint, CS.out.steeringRateDeg, lat_active, apply_torque, apply_steer_req, ) if not lat_active: diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index c88b31d494..064f7930d0 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -11,7 +11,7 @@ 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, \ get_rav4_interceptor_pedal_scale, \ - get_steer_rate_limit_frames, \ + apply_toyota_corolla_steer_rate_guard, \ limit_interceptor_pcm_accel, \ limit_interceptor_stopping_accel, limit_no_lead_cruise_sign_flip, \ limit_prius_stopping_accel, should_bypass_toyota_long_pid, supports_toyota_auto_hold, \ @@ -735,9 +735,15 @@ class TestToyotaFingerprint: class TestToyotaCarController: - def test_corolla_tss2_uses_early_steer_rate_fault_guard(self): - assert get_steer_rate_limit_frames(CAR.TOYOTA_COROLLA_TSS2) == 8 - assert get_steer_rate_limit_frames(CAR.TOYOTA_RAV4_TSS2) == 18 + def test_corolla_tss2_cuts_torque_but_keeps_lka_request_at_high_steer_rate(self): + assert apply_toyota_corolla_steer_rate_guard(CAR.TOYOTA_COROLLA_TSS2, 100.0, True, 250, True) == (0, True) + assert apply_toyota_corolla_steer_rate_guard(CAR.TOYOTA_COROLLA_TSS2, 120.0, True, 0, False) == (0, True) + + def test_corolla_tss2_steer_rate_guard_leaves_normal_rate_unchanged(self): + assert apply_toyota_corolla_steer_rate_guard(CAR.TOYOTA_COROLLA_TSS2, 99.9, True, 250, True) == (250, True) + + def test_corolla_tss2_steer_rate_guard_is_corolla_only(self): + assert apply_toyota_corolla_steer_rate_guard(CAR.TOYOTA_RAV4_TSS2, 120.0, True, 250, True) == (250, True) @staticmethod def _make_controller(*, standstill_req=False, last_standstill=False): diff --git a/opendbc_repo/opendbc/dbc/hyundai_kia_ray_pedal.dbc b/opendbc_repo/opendbc/dbc/hyundai_kia_ray_pedal.dbc new file mode 100644 index 0000000000..a2dfe9d926 --- /dev/null +++ b/opendbc_repo/opendbc/dbc/hyundai_kia_ray_pedal.dbc @@ -0,0 +1,23 @@ +VERSION "" + +NS_ : +BS_: +BU_: INTERCEPTOR NEO + +BO_ 512 GAS_COMMAND: 6 NEO + SG_ GAS_COMMAND : 7|16@0+ (0.672,-177.408) [0|255] "" INTERCEPTOR + SG_ GAS_COMMAND2 : 23|16@0+ (0.332,-165.004) [0|255] "" INTERCEPTOR + SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR + SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" INTERCEPTOR + SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" INTERCEPTOR + +BO_ 513 GAS_SENSOR: 6 INTERCEPTOR + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.672,-177.408) [0|255] "" NEO + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.332,-165.004) [0|255] "" NEO + SG_ STATE : 39|4@0+ (1,0) [0|15] "" NEO + SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" NEO + SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" NEO + +VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; + +CM_ "Kia Ray EV comma pedal uses the standard 0x200/0x201 rolling counter and CRC8 protocol. Channel scaling is Ray-only, estimated from the September 15 route: sensor rest raw 264/497, slopes about 3.79/7.68 raw counts per native E_EMS11 pedal unit, and 100 native units mapped to 255 comma pedal command units. Confirm against installed Ray firmware before increasing the command cap."; diff --git a/opendbc_repo/opendbc/dbc/tesla_can.dbc b/opendbc_repo/opendbc/dbc/tesla_can.dbc index dc394f36af..3d1806985c 100644 --- a/opendbc_repo/opendbc/dbc/tesla_can.dbc +++ b/opendbc_repo/opendbc/dbc/tesla_can.dbc @@ -217,6 +217,9 @@ BO_ 513 SDM1: 5 GTW SG_ SDM_bcklPassStatus : 3|2@0+ (1,0) [0|3] "" NEO SG_ SDM_bcklDrivStatus : 5|2@0+ (1,0) [0|3] "" NEO +BO_ 529 RCM_status: 8 RCM + SG_ RCM_buckleDriverStatus : 15|2@0+ (1,0) [0|3] "" GTW,OCS,DAS + BO_ 532 EPB_epasControl: 3 EPB SG_ EPB_epasControlChecksum : 23|8@0+ (1,0) [0|255] "" NEO,EPAS SG_ EPB_epasControlCounter : 11|4@0+ (1,0) [0|15] "" NEO,EPAS @@ -256,9 +259,11 @@ BO_ 872 DI_state: 8 DI SG_ DI_immobilizerState : 28|3@1+ (1,0) [0|0] "" NEO SG_ DI_speedUnits : 31|1@1+ (1,0) [0|1] "" NEO SG_ DI_cruiseSet : 32|9@1+ (0.5,0) [0|255.5] "speed" NEO + SG_ DI_hw1DigitalSpeed : 32|8@1+ (1,0) [0|250] "speed" NEO SG_ DI_aebState : 41|3@1+ (1,0) [0|0] "" NEO SG_ DI_stateCounter : 44|4@1+ (1,0) [0|0] "" NEO SG_ DI_digitalSpeed : 48|8@1+ (1,0) [0|250] "" NEO + SG_ DI_hw1CruiseSet : 48|8@1+ (1,0) [0|250] "speed" NEO SG_ DI_stateChecksum : 56|8@1+ (1,0) [0|0] "" NEO BO_ 109 SBW_RQ_SCCM: 4 STW @@ -906,4 +911,3 @@ VAL_ 1001 DAS_turnIndicatorRequestReason 6 "DAS_ACTIVE_COMMANDED_LANE_CHANGE" 5 VAL_ 1160 DAS_steeringAngleRequest 16384 "ZERO_ANGLE" ; VAL_ 1160 DAS_steeringControlType 1 "ANGLE_CONTROL" 3 "DISABLED" 0 "NONE" 2 "RESERVED" ; VAL_ 1160 DAS_steeringHapticRequest 1 "ACTIVE" 0 "IDLE" ; - diff --git a/opendbc_repo/opendbc/safety/declarations.h b/opendbc_repo/opendbc/safety/declarations.h index bfeb2a2813..4c2c0c6395 100644 --- a/opendbc_repo/opendbc/safety/declarations.h +++ b/opendbc_repo/opendbc/safety/declarations.h @@ -384,3 +384,4 @@ extern const safety_hooks rivian_hooks; extern const safety_hooks psa_hooks; extern const safety_hooks volvo_hooks; extern const safety_hooks tesla_preap_hooks; +extern const safety_hooks tesla_legacy_hooks; diff --git a/opendbc_repo/opendbc/safety/modes/hyundai.h b/opendbc_repo/opendbc/safety/modes/hyundai.h index 4a3e7af721..67ace4925b 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai.h @@ -67,6 +67,9 @@ const LongitudinalLimits HYUNDAI_LONG_LIMITS = { #define HYUNDAI_NON_SCC_EV_ADDR_CHECK \ {.msg = {{0x592U, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ +#define HYUNDAI_RAY_PEDAL_ADDR_CHECK \ + {.msg = {{0x201U, 0, 6, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + static const CanMsg HYUNDAI_TX_MSGS[] = { HYUNDAI_COMMON_TX_MSGS(0, false) }; @@ -75,6 +78,11 @@ static const CanMsg HYUNDAI_REFRESH_TX_MSGS[] = { HYUNDAI_COMMON_TX_MSGS(0, true) }; +static const CanMsg HYUNDAI_RAY_PEDAL_TX_MSGS[] = { + HYUNDAI_COMMON_TX_MSGS(0, true) + {0x200, 0, 6, .check_relay = false}, // comma pedal only, not Hyundai EMS20 +}; + static const CanMsg HYUNDAI_LONG_TX_MSGS[] = { HYUNDAI_LONG_COMMON_TX_MSGS(0, false) {0x38D, 0, 8, .check_relay = false}, // FCA11 Bus 0 @@ -90,6 +98,7 @@ static const CanMsg HYUNDAI_LONG_REFRESH_TX_MSGS[] = { }; static bool hyundai_legacy = false; +static bool hyundai_ray_pedal = false; static bool hyundai_can_canfd_blended_hda2 = false; static bool hyundai_acc_main_on_rx_prev = false; @@ -120,6 +129,8 @@ static uint8_t hyundai_get_counter(const CANPacket_t *msg) { cnt = byte_421 & 0xFU; } else if (msg->addr == 0x4F1U) { cnt = (msg->data[3] >> 4) & 0xFU; + } else if (hyundai_ray_pedal && (msg->addr == 0x201U)) { + cnt = msg->data[4] & 0xFU; } else { } return cnt; @@ -136,11 +147,24 @@ static uint32_t hyundai_get_checksum(const CANPacket_t *msg) { chksum = msg->data[6] & 0xFU; } else if (msg->addr == 0x421U) { chksum = hyundai_can_canfd_blended ? msg->data[0] : msg->data[7] >> 4; + } else if (hyundai_ray_pedal && (msg->addr == 0x201U)) { + chksum = msg->data[5]; } else { } return chksum; } +static uint8_t hyundai_ray_pedal_checksum(const CANPacket_t *msg) { + uint8_t crc = 0xFFU; + for (int i = 4; i >= 0; i--) { + crc ^= msg->data[i]; + for (int j = 0; j < 8; j++) { + crc = (crc & 0x80U) ? (uint8_t)((crc << 1U) ^ 0xD5U) : (uint8_t)(crc << 1U); + } + } + return crc; +} + static void hyundai_rx_all_hook(const CANPacket_t *msg) { if ((msg->addr == 0x53EU) && (msg->bus == 2U) && (GET_LEN(msg) == 6U)) { hyundai_has_lkas12 = true; @@ -148,6 +172,10 @@ static void hyundai_rx_all_hook(const CANPacket_t *msg) { } static uint32_t hyundai_compute_checksum(const CANPacket_t *msg) { + if (hyundai_ray_pedal && (msg->addr == 0x201U)) { + return hyundai_ray_pedal_checksum(msg); + } + uint8_t chksum = 0; if (msg->addr == 0x386U) { // count the bits @@ -291,6 +319,22 @@ static bool hyundai_tx_hook(const CANPacket_t *msg) { bool tx = true; + if (hyundai_ray_pedal && (msg->addr == 0x200U)) { + const uint16_t track1 = ((uint16_t)msg->data[0] << 8U) | msg->data[1]; + const uint16_t track2 = ((uint16_t)msg->data[2] << 8U) | msg->data[3]; + const bool enabled = (msg->data[4] & 0x80U) != 0U; + const int expected_track2 = 497 + (2 * ((int)track1 - 264)); + if ((msg->data[4] & 0x70U) != 0U || + (msg->data[5] != hyundai_ray_pedal_checksum(msg)) || + (enabled && (track1 < 264U || track1 > 397U || track2 < 497U || track2 > 766U || + SAFETY_ABS((int)track2 - expected_track2) > 40)) || + (!enabled && ((track1 != 0U) || (track2 != 0U))) || + longitudinal_interceptor_checks(msg) || + (enabled && (!get_longitudinal_allowed() || brake_pressed_prev))) { + tx = false; + } + } + if ((msg->addr == 0x53EU) && !hyundai_has_lkas12) { tx = false; } @@ -457,6 +501,10 @@ static safety_config hyundai_init(uint16_t param) { }; hyundai_common_init(param); + hyundai_ray_pedal = (param & (uint16_t)~(32U | 128U | 2048U)) == 0x9405U; + if (hyundai_ray_pedal) { + hyundai_longitudinal = true; // button engagement; no Hyundai SCC TX + } 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); @@ -467,6 +515,17 @@ static safety_config hyundai_init(uint16_t param) { } safety_config ret; + if (hyundai_ray_pedal) { + static RxCheck hyundai_ray_pedal_rx_checks[] = { + HYUNDAI_COMMON_RX_CHECKS(false) + HYUNDAI_NON_SCC_EV_ADDR_CHECK + HYUNDAI_LDA_BUTTON_ADDR_CHECK + HYUNDAI_RAY_PEDAL_ADDR_CHECK + }; + SET_RX_CHECKS(hyundai_ray_pedal_rx_checks, ret); + SET_TX_MSGS(HYUNDAI_RAY_PEDAL_TX_MSGS, ret); + return ret; + } if (hyundai_longitudinal) { // Use CLU11 (buttons) to manage controls allowed instead of SCC cruise state static RxCheck hyundai_long_rx_checks[] = { @@ -696,6 +755,7 @@ static safety_config hyundai_legacy_init(uint16_t param) { hyundai_common_init(param); hyundai_legacy = true; + hyundai_ray_pedal = false; hyundai_can_canfd_blended_hda2 = false; hyundai_camera_scc = false; hyundai_can_refresh_msgs = false; diff --git a/opendbc_repo/opendbc/safety/modes/tesla_legacy.h b/opendbc_repo/opendbc/safety/modes/tesla_legacy.h new file mode 100644 index 0000000000..1c34d27be5 --- /dev/null +++ b/opendbc_repo/opendbc/safety/modes/tesla_legacy.h @@ -0,0 +1,241 @@ +#pragma once + +#include "opendbc/safety/declarations.h" + +#define TESLA_LEGACY_FLAG_HW1 8U + +static bool tesla_external_panda = false; +static bool tesla_hw1 = false; +static bool tesla_hw2 = false; +static bool tesla_hw3 = false; +static bool tesla_legacy_longitudinal = false; + +static int chassis_bus = 0U; +static int das_control_msg = 0x2bfU; +static int di_torque1_msg = 0x106U; + +static bool tesla_legacy_stock_aeb = false; +static bool tesla_legacy_stock_lkas = false; +static bool tesla_legacy_stock_lkas_prev = false; + +static void tesla_legacy_rx_hook(const CANPacket_t *msg) { + // EPAS_sysStatus: steering angle, driver hands, and EAC status. + if (!tesla_external_panda && (msg->bus == 0U) && (msg->addr == 0x370U)) { + const int angle_meas_new = (((msg->data[4] & 0x3FU) << 8) | msg->data[5]) - 8192U; + update_sample(&angle_meas, angle_meas_new); + + const int hands_on_level = msg->data[4] >> 6; + const int eac_status = msg->data[6] >> 5; + const int eac_error_code = msg->data[2] >> 4; + steering_disengage = (hands_on_level >= 3) || ((eac_status == 0) && (eac_error_code == 9)); + } + + // ESP_B: ESP_vehicleSpeed. + if (!tesla_external_panda && (msg->bus == (unsigned)chassis_bus) && (msg->addr == 0x155U)) { + const float speed = ((msg->data[6] | (msg->data[5] << 8)) * 0.01) * KPH_TO_MS; + UPDATE_VEHICLE_SPEED(speed); + } + + // DI_torque1: pedal position. HW1 uses the 0x108 message variant. + if ((tesla_external_panda || tesla_hw1) && (msg->bus == 0U) && (msg->addr == di_torque1_msg)) { + gas_pressed = msg->data[6] != 0U; + } + + if (((tesla_external_panda) && (msg->bus == 0U) && (msg->addr == 0x1f8U)) || + ((!tesla_external_panda) && (msg->bus == (unsigned)chassis_bus) && (msg->addr == 0x20aU))) { + brake_pressed = (((msg->data[0] & 0x0CU) >> 2) != 1U); + } + + // DI_state: cruise state. + if (((tesla_external_panda) && (msg->bus == 0U) && (msg->addr == 0x256U)) || + ((!tesla_external_panda) && (msg->bus == (unsigned)chassis_bus) && (msg->addr == 0x368U))) { + const int cruise_state = (msg->data[1] >> 4) & 0x07U; + const bool cruise_engaged = (cruise_state == 2) || (cruise_state == 3) || (cruise_state == 4) || + (cruise_state == 6) || (cruise_state == 7); + vehicle_moving = cruise_state != 3; + pcm_cruise_check(cruise_engaged); + } + + if (msg->bus == 2U) { + if ((tesla_external_panda || tesla_hw1) && msg->addr == das_control_msg) { + tesla_legacy_stock_aeb = (msg->data[2] & 0x03U) == 1U; + } + + if (!tesla_external_panda && msg->addr == 0x488U) { + const int steering_control_type = msg->data[2] >> 6; + const bool stock_lkas_now = steering_control_type == 2; + if (stock_lkas_now && !tesla_legacy_stock_lkas_prev && !controls_allowed) { + tesla_legacy_stock_lkas = true; + } + if (!stock_lkas_now) { + tesla_legacy_stock_lkas = false; + } + tesla_legacy_stock_lkas_prev = stock_lkas_now; + } + } +} + +static bool tesla_legacy_tx_hook(const CANPacket_t *msg) { + const AngleSteeringLimits TESLA_STEERING_LIMITS = { + .max_angle = 3600, + .angle_deg_to_can = 10, + .frequency = 50U, + }; + + const AngleSteeringParams TESLA_LEGACY_STEERING_PARAMS = { + .slip_factor = -0.0005666493436310427, + .steer_ratio = 15., + .wheelbase = 2.96, + }; + + const LongitudinalLimits TESLA_LONG_LIMITS = { + .max_accel = 425, + .min_accel = 288, + .inactive_accel = 375, + }; + + bool violation = false; + + // DAS_steeringControl: angle is encoded in 0.1 degree units with a 1638.35 offset. + if (!tesla_external_panda && (msg->addr == 0x488U)) { + const int raw_angle_can = ((msg->data[0] & 0x7FU) << 8) | msg->data[1]; + const int desired_angle = raw_angle_can - 16384; + const int steer_control_type = msg->data[2] >> 6; + const bool steer_control_enabled = steer_control_type == 1; + + violation |= steer_angle_cmd_checks_vm(desired_angle, steer_control_enabled, + TESLA_STEERING_LIMITS, TESLA_LEGACY_STEERING_PARAMS); + + const bool valid_steer_control_type = (steer_control_type == 0) || (steer_control_type == 1); + violation |= !valid_steer_control_type; + violation |= tesla_legacy_stock_lkas; + } + + // DAS_control: HW1 longitudinal control is sent to the powertrain bus (bus 0). + if ((tesla_external_panda || tesla_hw1) && (msg->addr == das_control_msg)) { + const int aeb_event = msg->data[2] & 0x03U; + violation |= aeb_event != 0; + violation |= tesla_legacy_stock_aeb; + + const int raw_accel_max = ((msg->data[6] & 0x1FU) << 4) | (msg->data[5] >> 4); + const int raw_accel_min = ((msg->data[5] & 0x0FU) << 5) | (msg->data[4] >> 3); + if (tesla_legacy_longitudinal) { + violation |= (raw_accel_max < TESLA_LONG_LIMITS.inactive_accel) && + (raw_accel_min < TESLA_LONG_LIMITS.inactive_accel); + violation |= longitudinal_accel_checks(raw_accel_max, TESLA_LONG_LIMITS); + violation |= longitudinal_accel_checks(raw_accel_min, TESLA_LONG_LIMITS); + } else { + // Stock ACC may only be cancelled, never spoofed or accelerated. + const int acc_state = msg->data[1] >> 4; + violation |= acc_state != 13; + violation |= (raw_accel_max != TESLA_LONG_LIMITS.inactive_accel) || + (raw_accel_min != TESLA_LONG_LIMITS.inactive_accel); + } + } + + return !violation; +} + +static bool tesla_legacy_fwd_hook(int bus_num, int addr) { + bool block_msg = false; + + if (bus_num == 2) { + if (!tesla_external_panda && !tesla_hw1 && (addr == 0x27dU)) { + block_msg = true; + } + if (!tesla_external_panda && (addr == 0x488U) && !tesla_legacy_stock_lkas) { + block_msg = true; + } + if ((tesla_external_panda || tesla_hw1) && (addr == das_control_msg) && !tesla_legacy_stock_aeb) { + block_msg = true; + } + } + + return block_msg; +} + +static safety_config tesla_legacy_init(uint16_t param) { + const int TESLA_FLAG_EXTERNAL_PANDA = 4; + const int TESLA_FLAG_HW2 = 16; + const int TESLA_FLAG_HW3 = 32; + + tesla_external_panda = GET_FLAG(param, TESLA_FLAG_EXTERNAL_PANDA); + tesla_hw1 = GET_FLAG(param, TESLA_LEGACY_FLAG_HW1); + tesla_hw2 = GET_FLAG(param, TESLA_FLAG_HW2); + tesla_hw3 = GET_FLAG(param, TESLA_FLAG_HW3); + tesla_legacy_longitudinal = GET_FLAG(param, 1); + + tesla_legacy_stock_aeb = false; + tesla_legacy_stock_lkas = false; + tesla_legacy_stock_lkas_prev = false; + chassis_bus = 0U; + di_torque1_msg = 0x106U; + das_control_msg = tesla_external_panda ? 0x2bfU : 0x2b9U; + + static const CanMsg TESLA_TX_LEGACY_MSGS[] = { + {0x488, 0, 4, .check_relay = true, .disable_static_blocking = true}, + {0x27D, 0, 3, .check_relay = true, .disable_static_blocking = true}, + }; + + static const CanMsg TESLA_LEGACY_PT_MSGS[] = { + {0x2bf, 0, 8, .check_relay = true, .disable_static_blocking = true}, + }; + + static const CanMsg TESLA_TX_LEGACY_HW1_MSGS[] = { + {0x488, 0, 4, .check_relay = true, .disable_static_blocking = true}, + {0x2b9, 0, 8, .check_relay = true, .disable_static_blocking = true}, + }; + + static RxCheck tesla_legacy_pt_rx_checks[] = { + {.msg = {{0x106, 0, 8, 100U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x1f8, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x2bf, 2, 8, 25U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x256, 0, 8, 10U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + }; + + static RxCheck tesla_legacy_hw1_rx_checks[] = { + {.msg = {{0x108, 0, 8, 100U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x2b9, 2, 8, 25U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x370, 0, 8, 25U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x155, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x20a, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x368, 0, 8, 10U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x488, 2, 4, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + }; + + static RxCheck tesla_legacy_hw2_rx_checks[] = { + {.msg = {{0x370, 0, 8, 25U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x155, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x20a, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x368, 0, 8, 10U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x488, 2, 4, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + }; + + static RxCheck tesla_legacy_hw3_rx_checks[] = { + {.msg = {{0x370, 0, 8, 100U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x155, 1, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x20a, 1, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x368, 1, 8, 10U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + {.msg = {{0x488, 2, 4, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}}, + }; + + if (tesla_external_panda && (tesla_hw3 || tesla_hw2)) { + return BUILD_SAFETY_CFG(tesla_legacy_pt_rx_checks, TESLA_LEGACY_PT_MSGS); + } + if (tesla_hw3) { + chassis_bus = 1U; + return BUILD_SAFETY_CFG(tesla_legacy_hw3_rx_checks, TESLA_TX_LEGACY_MSGS); + } + if (tesla_hw1) { + di_torque1_msg = 0x108U; + return BUILD_SAFETY_CFG(tesla_legacy_hw1_rx_checks, TESLA_TX_LEGACY_HW1_MSGS); + } + return BUILD_SAFETY_CFG(tesla_legacy_hw2_rx_checks, TESLA_TX_LEGACY_MSGS); +} + +const safety_hooks tesla_legacy_hooks = { + .init = tesla_legacy_init, + .rx = tesla_legacy_rx_hook, + .tx = tesla_legacy_tx_hook, + .fwd = tesla_legacy_fwd_hook, +}; diff --git a/opendbc_repo/opendbc/safety/safety.h b/opendbc_repo/opendbc/safety/safety.h index 0aa280fbe5..b4fb6d0d01 100644 --- a/opendbc_repo/opendbc/safety/safety.h +++ b/opendbc_repo/opendbc/safety/safety.h @@ -12,6 +12,7 @@ #include "opendbc/safety/modes/toyota.h" #include "opendbc/safety/modes/tesla.h" #include "opendbc/safety/modes/tesla_preap.h" +#include "opendbc/safety/modes/tesla_legacy.h" #include "opendbc/safety/modes/gm.h" #include "opendbc/safety/modes/ford.h" #include "opendbc/safety/modes/hyundai.h" @@ -502,7 +503,8 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { int hook_config_count = sizeof(safety_hook_registry) / sizeof(safety_hook_config); for (int i = 0; i < hook_config_count; i++) { if (safety_hook_registry[i].id == mode) { - current_hooks = safety_hook_registry[i].hooks; + current_hooks = ((mode == SAFETY_TESLA) && GET_FLAG(param, TESLA_LEGACY_FLAG_HW1)) ? + &tesla_legacy_hooks : safety_hook_registry[i].hooks; current_safety_mode = mode; current_safety_param = param; set_status = 0; // set diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_ray_pedal.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_ray_pedal.py new file mode 100644 index 0000000000..7dd0280e9a --- /dev/null +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_ray_pedal.py @@ -0,0 +1,49 @@ +import pytest + +from opendbc.can import CANPacker +from opendbc.car import create_gas_interceptor_command +from opendbc.car.structs import CarParams +from opendbc.safety.tests.libsafety import libsafety_py + + +@pytest.mark.parametrize("param", [0x9405, 0x9C05, 0x9401, 0x1005, 0]) +def test_ray_pedal_tx_isolation_and_limits(param): + safety = libsafety_py.libsafety + safety.set_safety_hooks(CarParams.SafetyModel.hyundai, param) + safety.init_tests() + safety.set_controls_allowed(True) + packer = CANPacker("hyundai_kia_ray_pedal") + + def tx(gas): + addr, dat, bus = create_gas_interceptor_command(packer, gas, 3) + return safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, dat)) + + has_ray_signature = param in (0x9405, 0x9C05) + assert tx(0) is has_ray_signature + assert tx(0.35) is has_ray_signature + assert not tx(0.36) # above the Ray-only initial command cap + assert not tx(1.0) + + if has_ray_signature: + safety.set_controls_allowed(False) + assert tx(0) + assert not tx(0.1) + safety.set_controls_allowed(True) + safety.set_gas_pressed_prev(True) + assert not tx(0.1) + safety.set_gas_pressed_prev(False) + addr, dat, bus = create_gas_interceptor_command(packer, 0.1, 3) + bad_crc = bytearray(dat) + bad_crc[-1] ^= 1 + assert not safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, bytes(bad_crc))) + + +def test_ray_pedal_rx_crc_is_checked_only_for_ray_signature(): + safety = libsafety_py.libsafety + safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0x9405) + safety.init_tests() + dat = bytes.fromhex("01f403d55de8") + assert safety.safety_rx_hook(libsafety_py.make_CANPacket(0x201, 0, dat)) + bad_crc = bytearray(dat) + bad_crc[-1] ^= 1 + assert not safety.safety_rx_hook(libsafety_py.make_CANPacket(0x201, 0, bytes(bad_crc))) diff --git a/opendbc_repo/opendbc/safety/tests/test_tesla_legacy.py b/opendbc_repo/opendbc/safety/tests/test_tesla_legacy.py new file mode 100644 index 0000000000..6839e38409 --- /dev/null +++ b/opendbc_repo/opendbc/safety/tests/test_tesla_legacy.py @@ -0,0 +1,73 @@ +import pytest + +from opendbc.can import CANPacker +from opendbc.car import Bus +from opendbc.car.tesla.teslacan_legacy import TeslaCANRaven +from opendbc.car.tesla.values import CANBUS, CAR, DBC, TeslaSafetyFlags +from opendbc.safety.tests.libsafety import libsafety_py + + +@pytest.fixture +def legacy_safety(): + safety = libsafety_py.libsafety + packer = CANPacker(DBC[CAR.TESLA_MODEL_S_HW1][Bus.pt]) + return safety, TeslaCANRaven({CANBUS.party: packer}) + + +def tx(safety, msg): + addr, data, bus = msg + return safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, data)) + + +def test_hw1_steering_requires_controls_allowed(legacy_safety): + safety, can = legacy_safety + safety.set_safety_hooks(10, TeslaSafetyFlags.FLAG_HW1.value) + safety.init_tests() + safety.set_angle_meas(0, 0) + safety.set_controls_allowed(False) + assert tx(safety, can.create_steering_control(0, 0, False)) + assert not tx(safety, can.create_steering_control(0, 0, True)) + safety.set_controls_allowed(True) + assert tx(safety, can.create_steering_control(0, 0, True)) + + +@pytest.mark.parametrize("alpha_long", [False, True]) +def test_hw1_accel_only_allowed_with_alpha_long_and_engagement(legacy_safety, alpha_long): + safety, can = legacy_safety + param = TeslaSafetyFlags.FLAG_HW1.value | (TeslaSafetyFlags.LONG_CONTROL.value if alpha_long else 0) + safety.set_safety_hooks(10, param) + safety.init_tests() + safety.set_controls_allowed(True) + assert tx(safety, can.create_longitudinal_command(13, 0, 0, 10, False, False)) + assert tx(safety, can.create_longitudinal_command(4, 1, 0, 10, True, False)) == alpha_long + safety.set_controls_allowed(False) + assert not tx(safety, can.create_longitudinal_command(4, 1, 0, 10, True, False)) + + +def test_hw1_stock_ap_steer_and_acc_are_blocked_from_forwarding(legacy_safety): + safety, _ = legacy_safety + safety.set_safety_hooks(10, TeslaSafetyFlags.FLAG_HW1.value) + safety.init_tests() + assert safety.safety_fwd_hook(2, 0x488) == -1 + assert safety.safety_fwd_hook(2, 0x2b9) == -1 + assert safety.safety_fwd_hook(2, 0x370) == 0 + + +def test_hw1_flag_dispatch_does_not_change_modern_or_preap_hooks(legacy_safety): + safety, can = legacy_safety + steer = can.create_steering_control(0, 0, False) + safety.set_safety_hooks(10, TeslaSafetyFlags.FLAG_HW1.value) + safety.init_tests() + assert tx(safety, steer) + + # APS monitor exists only in the modern Tesla TX whitelist; HW1 must not + # accidentally inherit it from the unflagged hook. + monitor = libsafety_py.make_CANPacket(0x27d, 0, bytes(3)) + assert not safety.safety_tx_hook(monitor) + safety.set_safety_hooks(10, 0) + safety.init_tests() + assert safety.safety_tx_hook(monitor) + + safety.set_safety_hooks(35, 0) + safety.init_tests() + assert safety.safety_fwd_hook(2, 0x370) == -1 diff --git a/opendbc_repo/pyproject.toml b/opendbc_repo/pyproject.toml index 00659b3bec..e807a76472 100644 --- a/opendbc_repo/pyproject.toml +++ b/opendbc_repo/pyproject.toml @@ -130,4 +130,5 @@ flake8-implicit-str-concat.allow-multiline=false include-package-data = true [tool.setuptools.package-data] +"opendbc.dbc" = ["hyundai_kia_ray_pedal.dbc"] "opendbc.safety" = ["*.h", "board/*.h", "board/drivers/*.h", "modes/*.h"] diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index e09ca0728c..8395404660 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-c51b9687-DEBUG"; +const uint8_t gitversion[19] = "DEV-3ba36ed4-DEBUG"; diff --git a/panda/board/obj/version b/panda/board/obj/version index c4a048cfd9..d6914641d5 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-c51b9687-DEBUG \ No newline at end of file +DEV-3ba36ed4-DEBUG \ No newline at end of file diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 8d1330b498..5f0d24bd1c 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -14,6 +14,13 @@ LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. NAV_TURN_DISTANCE_SPEED_BREAKPOINTS = [0.0, 5.0, 10.0] NAV_TURN_DISTANCE_BREAKPOINTS = [20.0, 25.0, 30.0] +# A driver normally signals an intersection before slowing below the lane-change +# speed threshold. Use the route to classify that early signal so it does not +# start a lane change while approaching the matching turn. +NAV_TURN_SIGNAL_LEAD_TIME = 12.0 +NAV_TURN_SIGNAL_BASE_DISTANCE = 15.0 +NAV_TURN_SIGNAL_MIN_DISTANCE = 30.0 +NAV_TURN_SIGNAL_MAX_DISTANCE = 250.0 NAV_KEEP_DISTANCE_SPEED_BREAKPOINTS = [0.0, 15.0, 30.0] NAV_KEEP_DISTANCE_BREAKPOINTS = [25.0, 90.0, 160.0] NAV_KEEP_AMBIGUOUS_SPLIT_DISTANCE_SCALE = 0.6 @@ -117,6 +124,34 @@ class DesireHelper: return distance <= float(np.interp(carstate.vEgo, NAV_TURN_DISTANCE_SPEED_BREAKPOINTS, NAV_TURN_DISTANCE_BREAKPOINTS)) + @staticmethod + def _nav_turn_signal_matches(carstate, nav_instruction_state): + if not bool(nav_instruction_state.get("valid", False)): + return False + if str(nav_instruction_state.get("maneuverType", "")).strip().lower() != "turn": + return False + + modifier = str(nav_instruction_state.get("maneuverModifier", "")).strip() + matching_signal = ( + modifier in ("left", "sharpLeft") and carstate.leftBlinker and not carstate.rightBlinker + ) or ( + modifier in ("right", "sharpRight") and carstate.rightBlinker and not carstate.leftBlinker + ) + if not matching_signal: + return False + + try: + maneuver_distance = float(nav_instruction_state.get("maneuverDistance", 0.0)) + except (TypeError, ValueError): + return False + + signal_distance = float(np.clip( + NAV_TURN_SIGNAL_BASE_DISTANCE + max(float(carstate.vEgo), 0.0) * NAV_TURN_SIGNAL_LEAD_TIME, + NAV_TURN_SIGNAL_MIN_DISTANCE, + NAV_TURN_SIGNAL_MAX_DISTANCE, + )) + return 0.0 <= maneuver_distance <= signal_distance + @staticmethod def _nudgeless_enabled(starpilot_toggles, controls_enabled): nudgeless = bool(getattr(starpilot_toggles, "nudgeless", False)) @@ -245,6 +280,10 @@ class DesireHelper: one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < starpilot_toggles.minimum_lane_change_speed + self._update_nav_params() + self.nav_desires_allowed = bool(getattr(starpilot_toggles, "nav_desires_allowed", self.nav_desires_allowed)) + nav_turn_signal = self.nav_desires_allowed and self._nav_turn_signal_matches(carstate, self._nav_instruction_state) + stop_imminent = (bool(getattr(starpilotPlan, "redLight", False)) or bool(getattr(starpilotPlan, "forcingStop", False)) or bool(getattr(starpilotPlan, "stopSignConfirmed", False))) @@ -264,8 +303,13 @@ class DesireHelper: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: + if nav_turn_signal and self.lane_change_state == LaneChangeState.preLaneChange: + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none + # LaneChangeState.off - if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: + if (self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker + and not below_lane_change_speed and not nav_turn_signal): self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 # Initialize lane change direction to prevent UI alert flicker diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 0987164d26..29e0eeb40b 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -268,8 +268,8 @@ GENESIS_GV70_OUTPUT_SMOOTHING_SPEED_WIDTH = 6.0 * CV.MPH_TO_MS GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT = 0.48 GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH = 0.16 GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_RC = 0.42 -GENESIS_GV70_OUTPUT_SMOOTHING_CURVE_RC = 0.14 -GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_RC = 0.12 +GENESIS_GV70_OUTPUT_SMOOTHING_CURVE_RC = 0.20 +GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_RC = 0.16 GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE = 0.04 GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08 GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.55 @@ -360,6 +360,13 @@ GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE = 0.04 GENESIS_G70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08 GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.45 GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC = 0.28 +GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_SPEED = 45.0 * CV.MPH_TO_MS +GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_SPEED_WIDTH = 5.0 * CV.MPH_TO_MS +GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT = 0.35 +GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT_WIDTH = 0.15 +GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_JERK = 0.25 +GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_JERK_WIDTH = 0.15 +GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_RC = 0.55 GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN = 0.45 GENESIS_G70_ANGLE_OUTPUT_TAPER_START = 70.0 GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH = 6.0 @@ -654,7 +661,7 @@ KIA_CARNIVAL_UNWIND_FF_OVERSHOOT = 0.08 KIA_CARNIVAL_UNWIND_FF_OVERSHOOT_WIDTH = 0.06 KIA_CARNIVAL_UNWIND_FF_JERK = 0.45 KIA_CARNIVAL_UNWIND_FF_JERK_WIDTH = 0.20 -KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_MAX = 0.28 +KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_MAX = 0.35 KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED = 8.0 KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_WIDTH = 2.0 KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_CUTOFF = 16.0 @@ -1321,7 +1328,7 @@ KONA_EV_2022_CENTER_FRICTION_THRESHOLD_LAT = 0.20 KONA_EV_2022_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.05 KONA_EV_2022_CENTER_FRICTION_THRESHOLD_SPEED = 18.0 KONA_EV_2022_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.5 -KONA_EV_2022_CENTER_OUTPUT_TAPER_MAX = 0.045 +KONA_EV_2022_CENTER_OUTPUT_TAPER_MAX = 0.08 KONA_EV_2022_CENTER_OUTPUT_TAPER_LAT = 0.20 KONA_EV_2022_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.05 KONA_EV_2022_CENTER_OUTPUT_TAPER_SPEED = 18.0 @@ -3485,6 +3492,24 @@ def get_genesis_g70_stabilized_output(output_torque: float, prev_output_torque: if changing_direction: response_time = max(response_time, GENESIS_G70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC) + output_reversal = (prev_output_torque * output_torque < -0.0025 and + abs(desired_lateral_accel) >= GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT) + if output_reversal: + reversal_speed_weight = _sigmoid( + (max(v_ego, 0.0) - GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_SPEED) / + GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_SPEED_WIDTH + ) + reversal_lat_weight = _sigmoid( + (abs(desired_lateral_accel) - GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT) / + GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_LAT_WIDTH + ) + reversal_jerk_weight = _sigmoid( + (abs(desired_lateral_jerk) - GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_JERK) / + GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_JERK_WIDTH + ) + reversal_weight = reversal_speed_weight * reversal_lat_weight * reversal_jerk_weight + response_time += reversal_weight * max(GENESIS_G70_OUTPUT_SMOOTHING_REVERSAL_RC - response_time, 0.0) + output_alpha = dt / (max(response_time, 0.0) + dt) smoothed_output = prev_output_torque + output_alpha * (output_torque - prev_output_torque) return float(output_torque + speed_weight * (smoothed_output - output_torque)) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 60a34a00d8..86eeff200b 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -767,6 +767,7 @@ class TestLatControl: assert steady_turn == pytest.approx(1.0) assert clean_unwind == pytest.approx(1.0) assert 0.70 < overshooting_unwind < 1.0 + assert overshooting_unwind < 0.80 assert high_speed_overshoot > overshooting_unwind def test_genesis_g90_ff_scale_curve(self): @@ -1399,9 +1400,9 @@ class TestLatControl: assert low_speed_threshold == pytest.approx(get_standard_friction_threshold(8.0), abs=0.001) assert highway_threshold > highway_base assert highway_curve_threshold == pytest.approx(highway_base, abs=0.001) - assert get_kona_ev_2022_center_output_scale(0.0, 27.0) < 0.96 + assert get_kona_ev_2022_center_output_scale(0.0, 27.0) < 0.94 assert get_kona_ev_2022_center_output_scale(0.6, 27.0) == pytest.approx(1.0, abs=0.001) - assert get_kona_ev_2022_center_output_scale(0.0, 8.0) == pytest.approx(1.0, abs=0.001) + assert get_kona_ev_2022_center_output_scale(0.0, 8.0) == pytest.approx(1.0, abs=0.002) def test_kona_ev_2022_center_output_taper_update_path(self, monkeypatch): monkeypatch.setattr(latcontrol_torque, "get_kona_ev_2022_center_output_scale", lambda *_args: 1.0) @@ -1830,11 +1831,13 @@ class TestLatControl: high_speed_wind = get_genesis_g70_stabilized_output(0.1, 0.3, 0.8, 0.5, 30.0, DT_CTRL) high_speed_unwind = get_genesis_g70_stabilized_output(0.1, 0.3, 0.8, -0.5, 30.0, DT_CTRL) high_speed_direction_change = get_genesis_g70_stabilized_output(-0.3, 0.3, -0.8, -0.5, 30.0, DT_CTRL) + low_speed_direction_change = get_genesis_g70_stabilized_output(-0.3, 0.3, -0.8, -0.5, 10.0, DT_CTRL) assert low_speed == pytest.approx(-0.2, abs=0.005) assert abs(high_speed_center - 0.2) < abs(low_speed - 0.2) assert high_speed_unwind > high_speed_wind > 0.1 assert 0.2 < high_speed_direction_change < 0.3 + assert high_speed_direction_change > low_speed_direction_change def test_genesis_g70_output_stabilizer_update_path(self, monkeypatch): calls = [] diff --git a/selfdrive/controls/tests/test_navigation_desires.py b/selfdrive/controls/tests/test_navigation_desires.py index 13128651ed..a57d5b4b7a 100644 --- a/selfdrive/controls/tests/test_navigation_desires.py +++ b/selfdrive/controls/tests/test_navigation_desires.py @@ -116,6 +116,76 @@ def test_nav_desires_turn_right_waits_until_turn_is_close(): assert helper.desire == log.Desire.none +def test_matching_routed_turn_does_not_start_lane_change_above_threshold(): + helper = DesireHelper() + helper._update_nav_params = lambda: None + helper._nav_instruction_state = { + "valid": True, + "maneuverType": "turn", + "maneuverModifier": "right", + "maneuverDistance": 111.0, + } + + helper.update( + make_car_state(vEgo=16.0, rightBlinker=True), + True, + 0.0, + make_plan(), + make_toggles(minimum_lane_change_speed=11.1), + ) + + assert helper.lane_change_state == LaneChangeState.off + assert helper.lane_change_direction == LaneChangeDirection.none + assert helper.desire == log.Desire.none + + +def test_distant_routed_turn_does_not_block_lane_change(): + helper = DesireHelper() + helper._update_nav_params = lambda: None + helper._nav_instruction_state = { + "valid": True, + "maneuverType": "turn", + "maneuverModifier": "right", + "maneuverDistance": 794.0, + } + + helper.update( + make_car_state(vEgo=16.0, rightBlinker=True), + True, + 0.0, + make_plan(), + make_toggles(minimum_lane_change_speed=11.1), + ) + + assert helper.lane_change_state == LaneChangeState.preLaneChange + assert helper.lane_change_direction == LaneChangeDirection.right + + +def test_matching_routed_turn_cancels_pending_lane_change_before_it_starts(): + helper = DesireHelper() + helper._update_nav_params = lambda: None + helper._nav_instruction_state = { + "valid": True, + "maneuverType": "turn", + "maneuverModifier": "left", + "maneuverDistance": 125.0, + } + helper.lane_change_state = LaneChangeState.preLaneChange + helper.lane_change_direction = LaneChangeDirection.left + helper.prev_one_blinker = True + + helper.update( + make_car_state(vEgo=11.0, leftBlinker=True), + True, + 0.0, + make_plan(), + make_toggles(minimum_lane_change_speed=10.0), + ) + + assert helper.lane_change_state == LaneChangeState.off + assert helper.lane_change_direction == LaneChangeDirection.none + + def test_nav_desires_off_ramp_lane_guidance_becomes_keep_right(): helper = DesireHelper() helper.nav_desires_allowed = True diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 5d4a779af6..3c836224f8 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -1284,6 +1284,33 @@ def test_nav_turn_speed_control_slows_for_imminent_turn(): assert result > 0.0 +def test_nav_turn_speed_control_begins_before_reported_intersection_approach(): + _, vcruise = make_vcruise(nav_state={ + "valid": True, + "maneuverType": "turn", + "maneuverModifier": "right", + "maneuverDistance": 111.0, + "nextManeuverType": "", + "nextManeuverModifier": "", + "nextManeuverDistance": 0.0, + }) + + toggles = make_toggles() + toggles.nav_longitudinal_allowed = True + result = vcruise.update( + controls_enabled=True, + now=0.0, + time_validated=True, + v_cruise=16.1, + v_ego=16.0, + sm=make_sm(standstill=False), + starpilot_toggles=toggles, + ) + + assert result < 16.1 + assert result == pytest.approx(vcruise.nav_turn_target) + + def test_nav_turn_speed_control_ignores_distant_turn(): _, vcruise = make_vcruise(nav_state={ "valid": True, diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index 5170bf4658..9212183c9d 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -47,17 +47,22 @@ MACH_E_DIRECTION_CHANGE_MIN_SPEED = 9.0 MACH_E_DIRECTION_CHANGE_LOOKAHEAD_RAMP_SPEED = 10.0 MACH_E_DIRECTION_CHANGE_LOOKAHEAD_FULL_SPEED = 12.0 MACH_E_DIRECTION_CHANGE_LOOKAHEAD_FADE_SPEED = 15.0 -MACH_E_DIRECTION_CHANGE_LOOKAHEAD_EXTRA = 1.60 +MACH_E_DIRECTION_CHANGE_LOOKAHEAD_EXTRA = 2.40 MACH_E_DIRECTION_CHANGE_MIN_PREVIEW_CURVATURE = 0.0005 MACH_E_DIRECTION_CHANGE_FULL_PREVIEW_CURVATURE = 0.002 MACH_E_DIRECTION_CHANGE_MIN_LAG_CURVATURE = 0.0008 MACH_E_DIRECTION_CHANGE_FULL_LAG_CURVATURE = 0.0015 +MACH_E_DIRECTION_CHANGE_EARLY_MIN_LAG_CURVATURE = -0.001 +MACH_E_DIRECTION_CHANGE_EARLY_FULL_LAG_CURVATURE = 0.0008 +MACH_E_DIRECTION_CHANGE_EARLY_MIN_CURVATURE = 0.002 +MACH_E_DIRECTION_CHANGE_EARLY_FULL_CURVATURE = 0.004 MACH_E_LOW_SPEED_DIRECTION_CHANGE_START_SPEED = 1.8 MACH_E_LOW_SPEED_DIRECTION_CHANGE_FULL_SPEED = 2.0 MACH_E_LOW_SPEED_DIRECTION_CHANGE_HOLD_SPEED = 2.8 MACH_E_LOW_SPEED_DIRECTION_CHANGE_FADE_SPEED = 3.5 MACH_E_LOW_SPEED_DIRECTION_CHANGE_MIN_CURVATURE = 0.0004 MACH_E_LOW_SPEED_DIRECTION_CHANGE_FULL_CURVATURE = 0.0006 +MACH_E_LOW_SPEED_DIRECTION_CHANGE_MAX_CURVATURE = 0.0015 FORD_CURVATURE_LOOKAHEAD = { CAR.FORD_EXPLORER_MK6: 0.20, } @@ -246,13 +251,27 @@ class FordLateralController: MACH_E_DIRECTION_CHANGE_LOOKAHEAD_EXTRA, MACH_E_TURN_IN_LOOKAHEAD_EXTRA], )) - def _direction_change_preview_weight(self, desired: float, preview: float, current: float) -> float: + def _direction_change_preview_weight(self, desired: float, preview: float, current: float, + allow_rising_desired: bool = False, early_handoff_weight: float = 0.0) -> float: if self.CP.carFingerprint not in FORD_CONSERVATIVE_PREVIEW_CARS: return 0.0 - if desired * preview >= 0.0 or desired * self.desired_curvature_last <= 0.0: + if desired * preview >= 0.0 or desired * self.desired_curvature_last <= 0.0 or desired * current <= 0.0: return 0.0 - if (abs(desired) >= abs(self.desired_curvature_last) or desired * current <= 0.0 or - abs(current) <= abs(desired)): + early_handoff_weight = float(np.clip(early_handoff_weight, 0.0, 1.0)) + lag = abs(current) - abs(desired) + lag_min = float(np.interp( + early_handoff_weight, [0.0, 1.0], + [MACH_E_DIRECTION_CHANGE_MIN_LAG_CURVATURE, MACH_E_DIRECTION_CHANGE_EARLY_MIN_LAG_CURVATURE], + )) + lag_full = float(np.interp( + early_handoff_weight, [0.0, 1.0], + [MACH_E_DIRECTION_CHANGE_FULL_LAG_CURVATURE, MACH_E_DIRECTION_CHANGE_EARLY_FULL_LAG_CURVATURE], + )) + desired_rising = abs(desired) >= abs(self.desired_curvature_last) + rising_handoff = (allow_rising_desired and abs(desired) > abs(self.desired_curvature_last) and + abs(desired) <= MACH_E_LOW_SPEED_DIRECTION_CHANGE_MAX_CURVATURE) + early_rising_handoff = early_handoff_weight > 0.0 and lag > lag_min + if desired_rising and not rising_handoff and not early_rising_handoff: return 0.0 preview_weight = float(np.interp( @@ -261,8 +280,8 @@ class FordLateralController: [0.0, 1.0], )) lag_weight = float(np.interp( - abs(current) - abs(desired), - [MACH_E_DIRECTION_CHANGE_MIN_LAG_CURVATURE, MACH_E_DIRECTION_CHANGE_FULL_LAG_CURVATURE], + lag, + [lag_min, lag_full], [0.0, 1.0], )) return preview_weight * lag_weight @@ -351,13 +370,26 @@ class FordLateralController: direction_change_predicted = turn_in_predicted direction_change_weight = 0.0 direction_change_speed_weight = float(v_ego > MACH_E_DIRECTION_CHANGE_MIN_SPEED) + low_speed_direction_change = direction_change_speed_weight == 0.0 if direction_change_speed_weight == 0.0: direction_change_speed_weight = self._low_speed_direction_change_weight(v_ego, desired) if direction_change_speed_weight > 0.0 and not CS.out.steeringPressed and not self._lane_change()[0]: direction_change_lookahead_extra = self._direction_change_lookahead_extra(v_ego) + early_handoff_weight = float(np.interp( + direction_change_lookahead_extra, + [MACH_E_TURN_IN_LOOKAHEAD_EXTRA, MACH_E_DIRECTION_CHANGE_LOOKAHEAD_EXTRA], + [0.0, 1.0], + )) + early_handoff_weight *= float(np.interp( + abs(desired), + [MACH_E_DIRECTION_CHANGE_EARLY_MIN_CURVATURE, MACH_E_DIRECTION_CHANGE_EARLY_FULL_CURVATURE], + [0.0, 1.0], + )) if direction_change_lookahead_extra > MACH_E_TURN_IN_LOOKAHEAD_EXTRA: direction_change_predicted = self._predicted_curvature(v_ego, lookahead + direction_change_lookahead_extra) - direction_change_weight = self._direction_change_preview_weight(desired, direction_change_predicted, current) + direction_change_weight = self._direction_change_preview_weight( + desired, direction_change_predicted, current, allow_rising_desired=low_speed_direction_change, + early_handoff_weight=early_handoff_weight) direction_change_weight *= direction_change_speed_weight if direction_change_weight > 0.0: predicted = float(np.interp(direction_change_weight, [0.0, 1.0], [predicted, direction_change_predicted])) diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index 20cde4c0ef..c8fa07aebe 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -188,10 +188,10 @@ def test_mach_e_turn_in_lookahead_extra_fades_by_speed(controller, speed, expect @pytest.mark.parametrize("speed,expected", ( (8.0, 0.80), (9.0, 0.80), - (9.5, 1.20), - (10.0, 1.60), - (12.0, 1.60), - (13.5, 1.20), + (9.5, 1.60), + (10.0, 2.40), + (12.0, 2.40), + (13.5, 1.60), (15.0, 0.80), (16.0, 0.80), )) @@ -225,6 +225,52 @@ def test_mach_e_direction_change_preview_leads_a_lagging_unwind(controller, sign assert weight == pytest.approx(1.0) +def test_mach_e_low_speed_direction_change_preview_can_lead_a_rising_near_path(controller): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.desired_curvature_last = 0.0006 + + assert controller._direction_change_preview_weight( + desired=0.0008, preview=-0.002, current=0.003, allow_rising_desired=True) == pytest.approx(1.0) + assert controller._direction_change_preview_weight( + desired=0.0008, preview=-0.002, current=0.003) == 0.0 + + +def test_mach_e_low_speed_direction_change_preview_rejects_large_rising_near_path(controller): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.desired_curvature_last = 0.0014 + + assert controller._direction_change_preview_weight( + desired=0.0016, preview=-0.002, current=0.003, allow_rising_desired=True) == 0.0 + + +def test_mach_e_extended_direction_preview_begins_before_measured_curvature_catches_desired(controller): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.desired_curvature_last = 0.0148 + + assert controller._direction_change_preview_weight( + desired=0.0147, preview=-0.002, current=0.0140, early_handoff_weight=1.0) == pytest.approx(1.0 / 6.0) + assert controller._direction_change_preview_weight( + desired=0.0147, preview=-0.002, current=0.0140) == 0.0 + + +def test_mach_e_extended_direction_preview_tolerates_small_desired_jitter(controller): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.desired_curvature_last = 0.0146 + + assert controller._direction_change_preview_weight( + desired=0.0147, preview=-0.002, current=0.0141, early_handoff_weight=1.0) == pytest.approx(2.0 / 9.0) + assert controller._direction_change_preview_weight( + desired=0.0147, preview=-0.002, current=0.0141) == 0.0 + + +def test_mach_e_extended_direction_preview_preserves_turn_in_when_vehicle_lags(controller): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.desired_curvature_last = 0.0146 + + assert controller._direction_change_preview_weight( + desired=0.0147, preview=-0.002, current=0.0130, early_handoff_weight=1.0) == 0.0 + + @pytest.mark.parametrize("desired,preview,current,last", ( (0.0015, 0.002, 0.003, 0.002), # no predicted direction change (0.002, -0.002, 0.003, 0.0015), # desired curvature is still rising @@ -296,6 +342,51 @@ def test_mach_e_direction_change_preview_leads_low_speed_handoff(controller, mon pytest.approx(0.003), True)] +def test_mach_e_direction_change_preview_leads_rising_low_speed_handoff(controller, monkeypatch): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.sm["liveDelay"].lateralDelay = 0.4 + controller.desired_curvature_last = 0.0006 + blend_inputs = [] + monkeypatch.setattr( + controller, "_predicted_curvature", + lambda _v_ego, lookahead: 0.0008 if lookahead < 1.0 else -0.002, + ) + monkeypatch.setattr( + controller, "_blend_and_scale", + lambda desired, predicted, v_ego, current, allow_opposite_preview=False: + blend_inputs.append((desired, predicted, v_ego, current, allow_opposite_preview)) or (0.0, 1), + ) + + controller.update( + SimpleNamespace(latActive=True), car_state(speed=2.5, curvature=0.003), + SimpleNamespace(curvature=0.0008), + ) + + assert blend_inputs == [(pytest.approx(0.0008), pytest.approx(-0.002), pytest.approx(2.5), + pytest.approx(0.003), True)] + + +def test_mach_e_direction_change_preview_does_not_lead_rising_high_speed_path(controller, monkeypatch): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.sm["liveDelay"].lateralDelay = 0.4 + controller.desired_curvature_last = 0.0006 + blend_inputs = [] + monkeypatch.setattr(controller, "_predicted_curvature", lambda _v_ego, _lookahead: -0.002) + monkeypatch.setattr( + controller, "_blend_and_scale", + lambda desired, predicted, v_ego, current, allow_opposite_preview=False: + blend_inputs.append((desired, predicted, v_ego, current, allow_opposite_preview)) or (0.0, 1), + ) + + controller.update( + SimpleNamespace(latActive=True), car_state(speed=15.0, curvature=0.003), + SimpleNamespace(curvature=0.0008), + ) + + assert blend_inputs == [(pytest.approx(0.0008), pytest.approx(-0.002), pytest.approx(15.0), + pytest.approx(0.003), False)] + + def test_mach_e_direction_change_preview_uses_extended_horizon_at_medium_speed(controller, monkeypatch): controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 controller.sm["liveDelay"].lateralDelay = 0.4 @@ -305,7 +396,7 @@ def test_mach_e_direction_change_preview_uses_extended_horizon_at_medium_speed(c def predicted_curvature(_v_ego, lookahead): lookaheads.append(lookahead) - return {0.4: 0.002, 1.2: 0.001, 2.0: -0.002}[round(lookahead, 1)] + return {0.4: 0.002, 1.2: 0.001, 2.8: -0.002}[round(lookahead, 1)] monkeypatch.setattr(controller, "_predicted_curvature", predicted_curvature) monkeypatch.setattr( @@ -319,11 +410,63 @@ def test_mach_e_direction_change_preview_uses_extended_horizon_at_medium_speed(c SimpleNamespace(curvature=0.0015), ) - assert lookaheads == [pytest.approx(0.4), pytest.approx(1.2), pytest.approx(2.0)] + assert lookaheads == [pytest.approx(0.4), pytest.approx(1.2), pytest.approx(2.8)] assert blend_inputs == [(pytest.approx(0.0015), pytest.approx(-0.002), pytest.approx(10.5), pytest.approx(0.003), True)] +def test_mach_e_extended_direction_preview_advances_large_curve_exit(controller, monkeypatch): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.sm["liveDelay"].lateralDelay = 0.4 + controller.desired_curvature_last = 0.0146 + blend_inputs = [] + + def predicted_curvature(_v_ego, lookahead): + return {0.4: 0.0144, 1.2: 0.011, 2.8: -0.002}[round(lookahead, 1)] + + monkeypatch.setattr(controller, "_predicted_curvature", predicted_curvature) + monkeypatch.setattr( + controller, "_blend_and_scale", + lambda desired, predicted, v_ego, current, allow_opposite_preview=False: + blend_inputs.append((desired, predicted, v_ego, current, allow_opposite_preview)) or (0.0, 1), + ) + + controller.update( + SimpleNamespace(latActive=True), car_state(speed=12.0, curvature=0.0141), + SimpleNamespace(curvature=0.0147), + ) + + assert len(blend_inputs) == 1 + assert blend_inputs[0][0] == pytest.approx(0.0147) + assert blend_inputs[0][1] < 0.0144 + assert blend_inputs[0][4] + + +def test_mach_e_extended_direction_preview_preserves_small_medium_speed_path(controller, monkeypatch): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.sm["liveDelay"].lateralDelay = 0.4 + controller.desired_curvature_last = 0.0007 + blend_inputs = [] + + def predicted_curvature(_v_ego, lookahead): + return 0.0008 if lookahead < 2.0 else -0.002 + + monkeypatch.setattr(controller, "_predicted_curvature", predicted_curvature) + monkeypatch.setattr( + controller, "_blend_and_scale", + lambda desired, predicted, v_ego, current, allow_opposite_preview=False: + blend_inputs.append((desired, predicted, v_ego, current, allow_opposite_preview)) or (0.0, 1), + ) + + controller.update( + SimpleNamespace(latActive=True), car_state(speed=12.0, curvature=0.0014), + SimpleNamespace(curvature=0.0008), + ) + + assert blend_inputs == [(pytest.approx(0.0008), pytest.approx(0.0008), pytest.approx(12.0), + pytest.approx(0.0014), False)] + + def test_mach_e_extended_direction_horizon_does_not_replace_turn_in_preview(controller, monkeypatch): controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 controller.sm["liveDelay"].lateralDelay = 0.4 @@ -331,7 +474,7 @@ def test_mach_e_extended_direction_horizon_does_not_replace_turn_in_preview(cont blend_inputs = [] def predicted_curvature(_v_ego, lookahead): - return {0.4: 0.006, 1.2: 0.010, 1.6: 0.004, 2.0: -0.002}[round(lookahead, 1)] + return {0.4: 0.006, 1.2: 0.010, 1.6: 0.004, 2.8: -0.002}[round(lookahead, 1)] monkeypatch.setattr(controller, "_predicted_curvature", predicted_curvature) monkeypatch.setattr( diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 2d46e1f2de..5f3468eed2 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -35,7 +35,10 @@ SLC_LEAD_DROP_RELAXATION_MAX_POST_DROP_CLOSING_SPEED = 0.35 SLC_LEAD_DROP_RELAXATION_MAX_LEAD_BRAKE = 0.25 SLC_LEAD_DROP_RELAXATION_OVERSPEED_BP = [0.0, 5.0 * CV.MPH_TO_MS, 10.0 * CV.MPH_TO_MS, 15.0 * CV.MPH_TO_MS] SLC_LEAD_DROP_RELAXATION_DECEL_V = [0.7, 0.9, 1.15, 1.35] -NAV_TURN_COMFORT_DECEL = 1.25 +# This is an approach envelope, not a request for harder braking. A gentler +# deceleration value lowers the target farther from the turn and gives the MPC +# more time to settle before the intersection. +NAV_TURN_COMFORT_DECEL = 0.85 NAV_TURN_DISTANCE_BUFFER = 8.0 NAV_TURN_MIN_TARGET_DELTA = 0.25 NAV_TURN_TARGET_SPEEDS = {