Merge branch 'master-priv' into dynamic_personality

selfdrive/car/toyota/carcontroller.py

@@ -1,4 +1,5 @@
 from cereal import car
+from common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import clip, interp
 from openpilot.common.params import Params
 from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \
@@ -30,6 +31,9 @@ MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150  # slightly above steering pressed allows
 LEFT_BLINDSPOT = b"\x41"
 RIGHT_BLINDSPOT = b"\x42"
 
+UNLOCK_CMD = b"\x40\x05\x30\x11\x00\x40\x00\x00"
+LOCK_CMD = b"\x40\x05\x30\x11\x00\x80\x00\x00"
+
 
 class CarController(CarControllerBase):
   def __init__(self, dbc_name, CP, VM):
@@ -49,6 +53,7 @@ class CarController(CarControllerBase):
     self.accel = 0
 
     self.param_s = Params()
+    self._is_metric = self.param_s.get_bool("IsMetric")
     self._reverse_acc_change = self.param_s.get_bool("ReverseAccChange")
     self._sng_hack = self.param_s.get_bool("ToyotaSnG")
 
@@ -62,7 +67,19 @@ class CarController(CarControllerBase):
       self._brake_hold_reset: bool = False
       self._prev_brake_pressed: bool = False
 
+    self._auto_lock_by_speed = self.param_s.get_bool("ToyotaAutoLockBySpeed")
+    self._auto_unlock_by_shifter = self.param_s.get_bool("ToyotaAutoUnlockByShifter")
+    self._auto_lock_speed = 10 * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)
+    self._auto_lock_once = False
+    self._gear_prev = GearShifter.park
+
   def update(self, CC, CS, now_nanos):
+    if self.frame % 200 == 0:
+      self._is_metric = self.param_s.get_bool("IsMetric")
+      self._auto_lock_by_speed = self.param_s.get_bool("ToyotaAutoLockBySpeed")
+      self._auto_unlock_by_shifter = self.param_s.get_bool("ToyotaAutoUnlockByShifter")
+      self._auto_lock_speed = 10 * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)
+
     actuators = CC.actuators
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel
@@ -71,6 +88,21 @@ class CarController(CarControllerBase):
     # *** control msgs ***
     can_sends = []
 
+    # automatic door locking and unlocking logic (@dragonpilot-community)
+    # thanks to AlexandreSato & cydia2020
+    # https://github.com/AlexandreSato/animalpilot/blob/personal/doors.py
+    gear = CS.out.gearShifter
+    if not CS.out.doorOpen:
+      if gear == GearShifter.park and self._gear_prev != gear:
+        if self._auto_unlock_by_shifter:
+          can_sends.append(make_can_msg(0x750, UNLOCK_CMD, 0))
+        self._auto_lock_once = False
+      elif gear == GearShifter.drive and not self._auto_lock_once and CS.out.vEgo >= self._auto_lock_speed:
+        if self._auto_lock_by_speed:
+          can_sends.append(make_can_msg(0x750, LOCK_CMD, 0))
+        self._auto_lock_once = True
+    self._gear_prev = gear
+
     # *** steer torque ***
     new_steer = int(round(actuators.steer * self.params.STEER_MAX))
     apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params)

selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc

@@ -154,6 +154,24 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
   toyotaSngHack->setConfirmation(true, false);
   addItem(toyotaSngHack);
 
+  auto toyotaAutoLock = new ParamControl(
+    "ToyotaAutoLock",
+    tr("Enable Toyota Door Auto Locking"),
+    tr("sunnypilot will attempt to lock the doors when drive above 10 km/h (6.2 mph).\nReboot Required."),
+    "../assets/offroad/icon_blank.png"
+  );
+  toyotaAutoLock->setConfirmation(true, false);
+  addItem(toyotaAutoLock);
+
+  auto toyotaAutoUnlock = new ParamControl(
+    "ToyotaAutoUnlockByShifter",
+    tr("Enable Toyota Door Auto Unlocking"),
+    tr("sunnypilot will attempt to unlock the doors when shift to gear P.\nReboot Required."),
+    "../assets/offroad/icon_blank.png"
+  );
+  toyotaAutoUnlock->setConfirmation(true, false);
+  addItem(toyotaAutoUnlock);
+
   // Volkswagen
   addItem(new LabelControl(tr("Volkswagen")));
   auto volkswagenCCOnly = new ParamControl(