From 5806c159d498c5be9e378fde083c8dd56a5c4de4 Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Mon, 31 Mar 2025 13:32:28 +0800 Subject: [PATCH] Toyota Door Auto Lock/Unlock - 2025-06-13 --- common/params_keys.h | 1 + opendbc_repo/opendbc/car/structs.py | 1 + .../opendbc/car/toyota/carcontroller.py | 21 +++++++++++++++++++ opendbc_repo/opendbc/car/toyota/interface.py | 3 +++ opendbc_repo/opendbc/car/toyota/values.py | 1 + opendbc_repo/opendbc/safety/modes/toyota.h | 5 ++++- selfdrive/car/card.py | 3 +++ selfdrive/ui/qt/offroad/dp_panel.cc | 5 +++++ system/manager/manager.py | 1 + 9 files changed, 40 insertions(+), 1 deletion(-) diff --git a/common/params_keys.h b/common/params_keys.h index 2ddeff6fe..cd374fbc9 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -144,4 +144,5 @@ inline static std::unordered_map keys = { {"dp_device_auto_shutdown_in", PERSISTENT}, {"dp_ui_radar_tracks", PERSISTENT}, {"dp_device_ip", CLEAR_ON_MANAGER_START}, + {"dp_toyota_door_auto_lock_unlock", PERSISTENT}, }; diff --git a/opendbc_repo/opendbc/car/structs.py b/opendbc_repo/opendbc/car/structs.py index 7389cec8b..58e8dd50c 100644 --- a/opendbc_repo/opendbc/car/structs.py +++ b/opendbc_repo/opendbc/car/structs.py @@ -22,4 +22,5 @@ CarParamsT = capnp.lib.capnp._StructModule class DPFlags: LateralALKA = 1 ExtRadar = 2 + ToyotaDoorAutoLockUnlock = 2 ** 2 pass diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index f7e81adab..7b4bb9607 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -33,6 +33,17 @@ MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before torque can be cut # EPS allows user torque above threshold for 50 frames before permanently faulting MAX_USER_TORQUE = 500 +# Lock / unlock door commands - Credit goes to AlexandreSato! +from common.conversions import Conversions as CV +LOCK_SPEED = 20 * CV.KPH_TO_MS + +LOCK_UNLOCK_CAN_ID = 0x750 +UNLOCK_CMD = b'\x40\x05\x30\x11\x00\x40\x00\x00' +LOCK_CMD = b'\x40\x05\x30\x11\x00\x80\x00\x00' + +from cereal import car +PARK = car.CarState.GearShifter.park +DRIVE = car.CarState.GearShifter.drive def get_long_tune(CP, params): if CP.carFingerprint in TSS2_CAR: @@ -75,6 +86,8 @@ class CarController(CarControllerBase): self.secoc_lta_message_counter = 0 self.secoc_prev_reset_counter = 0 + self.doors_locked = False + def update(self, CC, CS, now_nanos): actuators = CC.actuators stopping = actuators.longControlState == LongCtrlState.stopping @@ -287,5 +300,13 @@ class CarController(CarControllerBase): new_actuators.steeringAngleDeg = self.last_angle new_actuators.accel = self.accel + if self.CP.flags & ToyotaFlags.DOOR_AUTO_LOCK_UNLOCK.value: + if not self.doors_locked and CS.out.gearShifter == DRIVE and CS.out.vEgo >= LOCK_SPEED: + can_sends.append(CanData(LOCK_UNLOCK_CAN_ID, LOCK_CMD, 0)) + self.doors_locked = True + elif self.doors_locked and CS.out.gearShifter == PARK: + can_sends.append(CanData(LOCK_UNLOCK_CAN_ID, UNLOCK_CMD, 0)) + self.doors_locked = False + self.frame += 1 return new_actuators, can_sends diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 87d523611..6db63bfb1 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -151,6 +151,9 @@ class CarInterface(CarInterfaceBase): if ret.flags & ToyotaFlags.HYBRID.value: ret.longitudinalActuatorDelay = 0.05 + if dp_params & structs.DPFlags.ToyotaDoorAutoLockUnlock: + ret.flags |= ToyotaFlags.DOOR_AUTO_LOCK_UNLOCK.value + return ret @staticmethod diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index f10f7d0ce..00f6b5861 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -77,6 +77,7 @@ class ToyotaFlags(IntFlag): SECOC = 2048 ALKA = 2 ** 12 + DOOR_AUTO_LOCK_UNLOCK = 2 ** 13 class Footnote(Enum): CAMRY = CarFootnote( diff --git a/opendbc_repo/opendbc/safety/modes/toyota.h b/opendbc_repo/opendbc/safety/modes/toyota.h index 4c6ae77d6..2f00fa24b 100644 --- a/opendbc_repo/opendbc/safety/modes/toyota.h +++ b/opendbc_repo/opendbc/safety/modes/toyota.h @@ -10,6 +10,7 @@ TOYOTA_BASE_TX_MSGS \ {0x2E4, 0, 5, .check_relay = true}, \ {0x343, 0, 8, .check_relay = false}, /* ACC cancel cmd */ \ + {0x750, 0, 8, .check_relay = false}, /* radar diagnostic address */ \ #define TOYOTA_COMMON_SECOC_TX_MSGS \ TOYOTA_BASE_TX_MSGS \ @@ -27,7 +28,7 @@ /* PCS_HUD */ \ {0x411, 0, 8, .check_relay = false}, \ /* radar diagnostic address */ \ - {0x750, 0, 8, .check_relay = false}, \ + /* {0x750, 0, 8, .check_relay = false},*/ \ /* ACC */ \ {0x343, 0, 8, .check_relay = true}, \ @@ -321,6 +322,7 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) { } // UDS: Only tester present ("\x0F\x02\x3E\x00\x00\x00\x00\x00") allowed on diagnostics address + /* if (addr == 0x750) { // this address is sub-addressed. only allow tester present to radar (0xF) bool invalid_uds_msg = (GET_BYTES(to_send, 0, 4) != 0x003E020FU) || (GET_BYTES(to_send, 4, 4) != 0x0U); @@ -328,6 +330,7 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) { tx = 0; } } + */ return tx; } diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index fda553b24..e657ecedf 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -105,6 +105,9 @@ class Car: if self.params.get_bool("dp_lat_alka"): dp_params |= structs.DPFlags.LateralALKA + if self.params.get_bool("dp_toyota_door_auto_lock_unlock"): + dp_params |= structs.DPFlags.ToyotaDoorAutoLockUnlock + self.CI = get_car(*self.can_callbacks, obd_callback(self.params), alpha_long_allowed, is_release, num_pandas, dp_params, cached_params) self.RI = interfaces[self.CI.CP.carFingerprint].RadarInterface(self.CI.CP) self.CP = self.CI.CP diff --git a/selfdrive/ui/qt/offroad/dp_panel.cc b/selfdrive/ui/qt/offroad/dp_panel.cc index f9d2b8b72..b67ca3725 100644 --- a/selfdrive/ui/qt/offroad/dp_panel.cc +++ b/selfdrive/ui/qt/offroad/dp_panel.cc @@ -7,6 +7,11 @@ void DPPanel::add_toyota_toggles() { QString::fromUtf8("🐉 ") + tr("Toyota / Lexus"), "", }, + { + "dp_toyota_door_auto_lock_unlock", + tr("Enable Door Auto Lock/Unlock"), + "", + }, }; QWidget *label = nullptr; diff --git a/system/manager/manager.py b/system/manager/manager.py index 2cf6debc6..454ff6391 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -61,6 +61,7 @@ def manager_init() -> None: ("dp_lon_no_gas_gating", "0"), ("dp_device_auto_shutdown_in", "-5"), ("dp_ui_radar_tracks", "0"), + ("dp_toyota_door_auto_lock_unlock", "0"), ] if params.get_bool("RecordFrontLock"):