📏 Enable Toyota Distance Button to Change Driving Personality

Pressing the distance change button in the car changes the driving
personality in openpilot.

Distance lines displayed on dashboard match the selected driving personality
3 = relaxed, 2 = standard, 1 = aggressive

Thanks to sshane and sunnyhaibin for help with this

selfdrive/car/toyota/carcontroller.py

@@ -135,10 +135,11 @@ def update(self, CC, CS, now_nanos):
       if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
         can_sends.append(toyotacan.create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert))
+        can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type,
+                                                        fcw_alert, CS.distance_send))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False))
+        can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, CS.distance_send))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.

selfdrive/car/toyota/carstate.py

@@ -1,6 +1,7 @@
 import copy
 
 from cereal import car
+from openpilot.common.params import Params, put_nonblocking
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import mean
 from openpilot.common.filter_simple import FirstOrderFilter
@@ -44,6 +45,13 @@ def __init__(self, CP):
     self.acc_type = 1
     self.lkas_hud = {}
 
+    # KRKeegan - Add support for toyota distance button
+    self.distance_btn = 0
+    self.prev_distance_btn = 0
+    self.distance_send = 0
+    self.distance_send_counter = 0
+    self.params = Params()
+
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
 
@@ -132,6 +140,30 @@ def update(self, cp, cp_cam):
         self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"]
       ret.stockFcw = bool(cp_acc.vl["PCS_HUD"]["FCW"])
 
+    if self.CP.carFingerprint in TSS2_CAR:
+      # KRKeegan - Read distance button from car
+      if self.CP.carFingerprint in RADAR_ACC_CAR:
+        # these cars have the acc_control on car can
+        self.distance_btn = 1 if cp.vl["ACC_CONTROL"]["DISTANCE"] == 1 else 0
+      else:
+        self.distance_btn = 1 if cp_cam.vl["ACC_CONTROL"]["DISTANCE"] == 1 else 0
+
+      if self.prev_distance_btn and not self.distance_btn:
+        # Update on release of button. Sunny checks if less than 50 frames not sure why
+        # maybe he has another feature if you hold button?
+        newGap = int(self.params.get("LongitudinalPersonality")) - 1
+        newGap = 2 if newGap < 0 or newGap > 2 else newGap
+        put_nonblocking("LongitudinalPersonality", str(newGap))
+
+      self.prev_distance_btn = self.distance_btn
+      # update dash if needed
+      if (int(cp.vl["PCM_CRUISE_SM"]["DISTANCE_LINES"]) - 1) != int(self.params.get("LongitudinalPersonality")) and self.distance_send_counter < 10:
+        self.distance_send = 1
+        self.distance_send_counter += 1
+      else:
+        self.distance_send = 0
+        self.distance_send_counter += 0
+
     # some TSS2 cars have low speed lockout permanently set, so ignore on those cars
     # these cars are identified by an ACC_TYPE value of 2.
     # TODO: it is possible to avoid the lockout and gain stop and go if you

selfdrive/car/toyota/toyotacan.py

@@ -27,12 +27,12 @@ def create_lta_steer_command(packer, steer_angle, steer_req, frame, setme_x64):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, send_distance):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
     "ACC_TYPE": acc_type,
-    "DISTANCE": 0,
+    "DISTANCE": send_distance,
     "MINI_CAR": lead,
     "PERMIT_BRAKING": 1,
     "RELEASE_STANDSTILL": not standstill_req,