From 8a0a8ff4d0f77f522937d4e90db87ef1c3d5d821 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/05/27 --- 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/safety/safety/safety_toyota.h | 3 +++ selfdrive/car/card.py | 3 +++ selfdrive/ui/qt/offroad/dp_panel.cc | 5 +++++ system/manager/manager.py | 1 + 9 files changed, 39 insertions(+) diff --git a/common/params_keys.h b/common/params_keys.h index 6b11bfea0..4777ad1c7 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -140,4 +140,5 @@ inline static std::unordered_map keys = { {"dp_lon_no_gas_gating", PERSISTENT}, {"dp_device_auto_shutdown_in", PERSISTENT}, {"dp_ui_radar_tracks", PERSISTENT}, + {"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 80864c2cc..a4e24cc07 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 @@ -286,5 +299,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 b0f75ef7d..5ae1c6731 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 4848a354e..361963069 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/safety/safety_toyota.h b/opendbc_repo/opendbc/safety/safety/safety_toyota.h index 7185001da..031979f0e 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_toyota.h +++ b/opendbc_repo/opendbc/safety/safety/safety_toyota.h @@ -10,6 +10,7 @@ TOYOTA_BASE_TX_MSGS \ {0x2E4, 0, 5, true}, \ {0x343, 0, 8, false}, /* ACC cancel cmd */ \ + {0x750, 0, 8, false}, /* radar diagnostic address */ \ #define TOYOTA_COMMON_SECOC_TX_MSGS \ TOYOTA_BASE_TX_MSGS \ @@ -315,6 +316,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); @@ -322,6 +324,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 067108352..976e5c641 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -102,6 +102,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), experimental_long_allowed, 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 8868a6033..e5a05e0a8 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 da9ed9e2d..7d8dac074 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -62,6 +62,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"):