diff --git a/common/params.cc b/common/params.cc index b59a104..95fc56f 100644 --- a/common/params.cc +++ b/common/params.cc @@ -258,6 +258,7 @@ std::unordered_map keys = { {"LaneLinesWidth", PERSISTENT}, {"LateralTune", PERSISTENT}, {"LeadInfo", PERSISTENT}, + {"LockDoors", PERSISTENT}, {"LongitudinalTune", PERSISTENT}, {"LongPitch", PERSISTENT}, {"ModelUI", PERSISTENT}, diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index f025a5b..71e516e 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -47,7 +47,7 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805; {0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8}, /* DSU bus 0 */ \ {0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4}, /* DSU bus 1 */ \ {0x2E4, 0, 5}, {0x191, 0, 8}, {0x411, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + ACC */ \ - {0x1D3, 0, 8}, \ + {0x1D3, 0, 8}, {0x750, 0, 8}, \ const CanMsg TOYOTA_TX_MSGS[] = { TOYOTA_COMMON_TX_MSGS diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 880a08e..bd40ec9 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -24,6 +24,11 @@ MAX_USER_TORQUE = 500 MAX_LTA_ANGLE = 94.9461 # deg MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows some resistance when changing lanes +# Lock / unlock door commands - Credit goes to AlexandreSato! +LOCK_CMD = b'\x40\x05\x30\x11\x00\x80\x00\x00' +UNLOCK_CMD = b'\x40\x05\x30\x11\x00\x40\x00\x00' +PARK = car.CarState.GearShifter.park + class CarController: def __init__(self, dbc_name, CP, VM): @@ -42,9 +47,14 @@ class CarController: self.accel = 0 # FrogPilot variables + self.lock_doors = False self.reverse_cruise_increase = False + self.doors_locked = False + self.doors_unlocked = True + def update_frogpilot_variables(self, params): + self.lock_doors = params.get_bool("LockDoors") self.reverse_cruise_increase = params.get_bool("ReverseCruise") def update(self, CC, CS, now_nanos, sport_plus): @@ -200,5 +210,16 @@ class CarController: new_actuators.accel = self.accel new_actuators.gas = self.gas + # Lock doors when in drive / unlock doors when in park + if self.lock_doors: + if self.doors_unlocked and CS.out.gearShifter != PARK: + can_sends.append(make_can_msg(0x750, LOCK_CMD, 0)) + self.doors_locked = True + self.doors_unlocked = False + elif self.doors_locked and CS.out.gearShifter == PARK: + can_sends.append(make_can_msg(0x750, UNLOCK_CMD, 0)) + self.doors_locked = False + self.doors_unlocked = True + self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/frogpilot/ui/vehicle_settings.cc b/selfdrive/frogpilot/ui/vehicle_settings.cc index e215fd3..a1b0fd3 100644 --- a/selfdrive/frogpilot/ui/vehicle_settings.cc +++ b/selfdrive/frogpilot/ui/vehicle_settings.cc @@ -101,6 +101,8 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil std::vector> vehicleToggles { {"GasRegenCmd", "Gas Regen Cmd", "", ""}, {"LongPitch", "Long Pitch Compensation", "Reduce speed and acceleration error for greater passenger comfort and improved vehicle efficiency.", ""}, + + {"LockDoors", "Lock Doors In Drive", "Automatically lock the doors when in drive and unlock when in park.", ""}, }; for (auto &[param, title, desc, icon] : vehicleToggles) {