diff --git a/common/params.cc b/common/params.cc index 7ef390a..008af8b 100644 --- a/common/params.cc +++ b/common/params.cc @@ -282,6 +282,7 @@ std::unordered_map keys = { {"LateralTune", PERSISTENT}, {"LeadDepartingAlert", PERSISTENT}, {"LeadInfo", PERSISTENT}, + {"LockDoors", PERSISTENT}, {"LongitudinalTune", PERSISTENT}, {"LongPitch", PERSISTENT}, {"ManualUpdateInitiated", CLEAR_ON_MANAGER_START}, diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 40358f5..f581b40 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -32,6 +32,12 @@ MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows COMPENSATORY_CALCULATION_THRESHOLD_V = [-0.3, -0.25, 0.] # m/s^2 COMPENSATORY_CALCULATION_THRESHOLD_BP = [0., 11., 23.] # m/s +# 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): self.CP = CP @@ -55,6 +61,9 @@ class CarController: self.cydia_tune = params.get_bool("CydiaTune") self.frogs_go_moo_tune = params.get_bool("FrogsGoMooTune") + self.doors_locked = False + self.doors_unlocked = True + def update(self, CC, CS, now_nanos, frogpilot_variables): actuators = CC.actuators hud_control = CC.hudControl @@ -234,5 +243,16 @@ class CarController: new_actuators.accel = self.accel new_actuators.gas = self.gas + # Lock doors when in drive / unlock doors when in park + if frogpilot_variables.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/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2fad25a..43ed4d6 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1073,6 +1073,7 @@ class Controls: lateral_tune = self.params.get_bool("LateralTune") self.force_auto_tune = lateral_tune and self.params.get_float("ForceAutoTune") + self.frogpilot_variables.lock_doors = self.params.get_bool("LockDoors") self.frogpilot_variables.long_pitch = self.params.get_bool("LongPitch") longitudinal_tune = self.params.get_bool("LongitudinalTune") diff --git a/selfdrive/frogpilot/ui/vehicle_settings.cc b/selfdrive/frogpilot/ui/vehicle_settings.cc index 5f8152b..0ee8a0e 100644 --- a/selfdrive/frogpilot/ui/vehicle_settings.cc +++ b/selfdrive/frogpilot/ui/vehicle_settings.cc @@ -113,6 +113,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil {"GasRegenCmd", "GM Truck Gas Tune", "Increase acceleration and smoothen brake to stop. For use on Silverado/Sierra only.", ""}, {"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.", ""}, {"LongitudinalTune", "Longitudinal Tune", "Use a custom Toyota longitudinal tune.\n\nCydia = More focused on TSS-P vehicles but works for all Toyotas\n\nDragonPilot = Focused on TSS2 vehicles\n\nFrogPilot = Takes the best of both worlds with some personal tweaks focused around my 2019 Lexus ES 350", ""}, }; diff --git a/selfdrive/frogpilot/ui/vehicle_settings.h b/selfdrive/frogpilot/ui/vehicle_settings.h index af78907..ba2b3db 100644 --- a/selfdrive/frogpilot/ui/vehicle_settings.h +++ b/selfdrive/frogpilot/ui/vehicle_settings.h @@ -30,7 +30,7 @@ private: std::set gmKeys = {"GasRegenCmd", "LongPitch"}; std::set subaruKeys = {}; - std::set toyotaKeys = {"LongitudinalTune"}; + std::set toyotaKeys = {"LockDoors", "LongitudinalTune"}; Params params; Params paramsMemory{"/dev/shm/params"};