Switch personalities via steering wheel / onroad UI

Added toggle to switch between the personalities via the steering wheel for GM/Toyota/Volkswagen vehicles and an onroad button for other makes.

Co-Authored-By: henryccy <104284652+henryccy@users.noreply.github.com>
Co-Authored-By: Jason Jackrel <23621790+thinkpad4by3@users.noreply.github.com>
Co-Authored-By: Eric Brown <13560103+nworb-cire@users.noreply.github.com>
Co-Authored-By: Kevin Robert Keegan <3046315+krkeegan@users.noreply.github.com>
Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
Co-Authored-By: mike8643 <98910897+mike8643@users.noreply.github.com>

selfdrive/car/gm/carcontroller.py

@@ -185,7 +185,7 @@ class CarController:
           # Send dashboard UI commands (ACC status)
           send_fcw = hud_alert == VisualAlert.fcw
           can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled,
-                                                              hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw))
+                                                              hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw, CS.display_menu, CS.personality_profile))
       else:
         # to keep accel steady for logs when not sending gas
         accel += self.accel_g

selfdrive/car/gm/carstate.py

@@ -156,6 +156,44 @@ class CarState(CarStateBase):
       ret.cruiseState.speed = pt_cp.vl["ECMCruiseControl"]["CruiseSetSpeed"] * CV.KPH_TO_MS
       ret.cruiseState.enabled = pt_cp.vl["ECMCruiseControl"]["CruiseActive"] != 0
 
+    # Driving personalities function - Credit goes to Mangomoose!
+    if self.personalities_via_wheel and ret.cruiseState.available:
+      # Sync with the onroad UI button
+      if self.param_memory.get_bool("PersonalityChangedViaUI"):
+        self.personality_profile = self.param.get_int("LongitudinalPersonality")
+        self.previous_personality_profile = self.personality_profile
+        self.param_memory.put_bool("PersonalityChangedViaUI", False)
+
+      # Check if the car has a camera
+      has_camera = self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.flags & GMFlags.NO_CAMERA.value and not self.CP.carFingerprint in (CC_ONLY_CAR)
+
+      if has_camera:
+        # Need to subtract by 1 to comply with the personality profiles of "0", "1", and "2"
+        self.personality_profile = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCGapLevel"] - 1
+      else:
+        if self.CP.carFingerprint in SDGM_CAR:
+          self.distance_button = cam_cp.vl["ASCMSteeringButton"]["DistanceButton"]
+        else:
+          self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"]
+
+        if self.distance_button and not self.distance_previously_pressed:
+          if self.display_menu:
+            self.personality_profile = (self.previous_personality_profile + 2) % 3
+          self.display_timer = 350
+        self.distance_previously_pressed = self.distance_button
+
+        # Check if the display is open
+        if self.display_timer > 0:
+          self.display_timer -= 1
+          self.display_menu = True
+        else:
+          self.display_menu = False
+
+      if self.personality_profile != self.previous_personality_profile:
+        self.param.put_int("LongitudinalPersonality", self.personality_profile)
+        self.param_memory.put_bool("PersonalityChangedViaWheel", True)
+        self.previous_personality_profile = self.personality_profile
+
     # Toggle Experimental Mode from steering wheel function
     if self.experimental_mode_via_press and ret.cruiseState.available:
       if self.CP.carFingerprint in SDGM_CAR:

selfdrive/car/gm/gmcan.py

@@ -106,14 +106,15 @@ def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_s
   return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values)
 
 
-def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, lead_car_in_sight, fcw):
+def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, lead_car_in_sight, fcw, display, personality):
   target_speed = min(target_speed_kph, 255)
 
   values = {
     "ACCAlwaysOne": 1,
     "ACCResumeButton": 0,
+    "DisplayDistance": display,
     "ACCSpeedSetpoint": target_speed,
-    "ACCGapLevel": 3 * enabled,  # 3 "far", 0 "inactive"
+    "ACCGapLevel": min(personality + 1, 3),  # 3 "far", 0 "inactive"
     "ACCCmdActive": enabled,
     "ACCAlwaysOne2": 1,
     "ACCLeadCar": lead_car_in_sight,

selfdrive/car/honda/carcontroller.py

@@ -94,7 +94,7 @@ def process_hud_alert(hud_alert):
 
 
 HUDData = namedtuple("HUDData",
-                     ["pcm_accel", "v_cruise", "lead_visible",
+                     ["pcm_accel", "v_cruise", "lead_visible", "distance_lines",
                       "lanes_visible", "fcw", "acc_alert", "steer_required"])
 
 
@@ -251,7 +251,7 @@ class CarController:
 
     # Send dashboard UI commands.
     if self.frame % 10 == 0:
-      hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible,
+      hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible, CS.personality_profile + 1,
                     hud_control.lanesVisible, fcw_display, acc_alert, steer_required)
       can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud))
 

selfdrive/car/honda/carstate.py

@@ -268,6 +268,25 @@ class CarState(CarStateBase):
       ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1
       ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1
 
+    # Driving personalities function
+    if self.personalities_via_wheel and ret.cruiseState.available:
+      # Sync with the onroad UI button
+      if self.param_memory.get_bool("PersonalityChangedViaUI"):
+        self.personality_profile = self.param.get_int("LongitudinalPersonality")
+        self.param_memory.put_bool("PersonalityChangedViaUI", False)
+
+      # Change personality upon steering wheel button press
+      self.distance_button = self.cruise_setting == 3
+
+      if self.distance_button and not self.distance_previously_pressed:
+        self.param_memory.put_bool("PersonalityChangedViaWheel", True)
+        self.personality_profile = (self.previous_personality_profile + 2) % 3
+      self.distance_previously_pressed = self.distance_button
+
+      if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
+        self.param.put_int("LongitudinalPersonality", self.personality_profile)
+        self.previous_personality_profile = self.personality_profile
+
     # Toggle Experimental Mode from steering wheel function
     if self.experimental_mode_via_press and ret.cruiseState.available:
       lkas_pressed = self.cruise_setting == 1

selfdrive/car/honda/hondacan.py

@@ -132,7 +132,7 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud,
     acc_hud_values = {
       'CRUISE_SPEED': hud.v_cruise,
       'ENABLE_MINI_CAR': 1 if enabled else 0,
-      'HUD_DISTANCE': 0,  # max distance setting on display
+      'HUD_DISTANCE': hud.distance_lines,
       'IMPERIAL_UNIT': int(not is_metric),
       'HUD_LEAD': 2 if enabled and hud.lead_visible else 1 if enabled else 0,
       'SET_ME_X01_2': 1,

selfdrive/car/hyundai/carcontroller.py

@@ -131,7 +131,7 @@ class CarController:
           can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
         if self.frame % 2 == 0:
           can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
-                                                           set_speed_in_units))
+                                                           set_speed_in_units, CS.personality_profile))
           self.accel_last = accel
       else:
         # button presses
@@ -151,7 +151,7 @@ class CarController:
         use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
         can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
                                                         hud_control.leadVisible, set_speed_in_units, stopping,
-                                                        CC.cruiseControl.override, use_fca, CS.out.cruiseState.available))
+                                                        CC.cruiseControl.override, use_fca, CS.out.cruiseState.available, CS.personality_profile))
 
       # 20 Hz LFA MFA message
       if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:

selfdrive/car/hyundai/carstate.py

@@ -167,6 +167,22 @@ class CarState(CarStateBase):
     if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
       self.main_enabled = not self.main_enabled
 
+    # Driving personalities function
+    if self.personalities_via_wheel and ret.cruiseState.available:
+      # Sync with the onroad UI button
+      if self.param_memory.get_bool("PersonalityChangedViaUI"):
+        self.personality_profile = self.param.get_int("LongitudinalPersonality")
+        self.param_memory.put_bool("PersonalityChangedViaUI", False)
+
+      # Change personality upon steering wheel button press
+      if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
+        self.param_memory.put_bool("PersonalityChangedViaWheel", True)
+        self.personality_profile = (self.previous_personality_profile + 2) % 3
+
+      if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
+        self.param.put_int("LongitudinalPersonality", self.personality_profile)
+        self.previous_personality_profile = self.personality_profile
+
     # Toggle Experimental Mode from steering wheel function
     if self.experimental_mode_via_press and ret.cruiseState.available:
       lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"]
@@ -267,6 +283,22 @@ class CarState(CarStateBase):
       self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING
                                           else cp_cam.vl["CAM_0x2a4"])
 
+    # Driving personalities function
+    if self.personalities_via_wheel and ret.cruiseState.available:
+      # Sync with the onroad UI button
+      if self.param_memory.get_bool("PersonalityChangedViaUI"):
+        self.personality_profile = self.param.get_int("LongitudinalPersonality")
+        self.param_memory.put_bool("PersonalityChangedViaUI", False)
+
+      # Change personality upon steering wheel button press
+      if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
+        self.param_memory.put_bool("PersonalityChangedViaWheel", True)
+        self.personality_profile = (self.previous_personality_profile + 2) % 3
+
+      if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
+        self.param.put_int("LongitudinalPersonality", self.personality_profile)
+        self.previous_personality_profile = self.personality_profile
+
     # Toggle Experimental Mode from steering wheel function
     if self.experimental_mode_via_press and ret.cruiseState.available:
       lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"]

selfdrive/car/hyundai/hyundaican.py

@@ -126,12 +126,12 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
   }
   return packer.make_can_msg("LFAHDA_MFC", 0, values)
 
-def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca, cruise_available):
+def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca, cruise_available, personality):
   commands = []
 
   scc11_values = {
     "MainMode_ACC": 1 if cruise_available else 0,
-    "TauGapSet": 4,
+    "TauGapSet": personality + 1,
     "VSetDis": set_speed if enabled else 0,
     "AliveCounterACC": idx % 0x10,
     "ObjValid": 1, # close lead makes controls tighter

selfdrive/car/hyundai/hyundaicanfd.py

@@ -121,7 +121,7 @@ def create_lfahda_cluster(packer, CAN, enabled):
   return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)
 
 
-def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed):
+def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, personality):
   jerk = 5
   jn = jerk / 50
   if not enabled or gas_override:
@@ -146,7 +146,7 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov
     "SET_ME_2": 0x4,
     "SET_ME_3": 0x3,
     "SET_ME_TMP_64": 0x64,
-    "DISTANCE_SETTING": 4,
+    "DISTANCE_SETTING": personality + 1,
   }
 
   return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)

selfdrive/car/interfaces.py

@@ -495,8 +495,16 @@ class CarStateBase(ABC):
     self.param = Params()
     self.param_memory = Params("/dev/shm/params")
 
+    self.display_menu = False
+    self.distance_previously_pressed = False
     self.lkas_previously_pressed = False
     self.main_enabled = False
+    self.profile_restored = False
+
+    self.display_timer = 0
+    self.distance_button = 0
+    self.personality_profile = self.param.get_int("LongitudinalPersonality")
+    self.previous_personality_profile = self.param.get_int("LongitudinalPersonality")
 
   def update_speed_kf(self, v_ego_raw):
     if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0:  # Prevent large accelerations when car starts at non zero speed
@@ -589,6 +597,7 @@ class CarStateBase(ABC):
   def update_frogpilot_params(self, params):
     self.conditional_experimental_mode = params.get_bool("ConditionalExperimental")
     self.experimental_mode_via_press = params.get_bool("ExperimentalModeViaPress")
+    self.personalities_via_wheel = params.get_int("AdjustablePersonalities") in {1, 3}
 
 INTERFACE_ATTR_FILE = {
   "FINGERPRINTS": "fingerprints",

selfdrive/car/toyota/carcontroller.py

@@ -163,10 +163,10 @@ class CarController:
       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, self.reverse_cruise_increase))
+        can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert, self.reverse_cruise_increase, CS.distance_button))
         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, self.reverse_cruise_increase))
+        can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.reverse_cruise_increase, CS.distance_button))
 
     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

@@ -165,6 +165,41 @@ class CarState(CarStateBase):
     if self.CP.carFingerprint != CAR.PRIUS_V:
       self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
 
+    # Driving personalities function
+    if self.personalities_via_wheel and ret.cruiseState.available:
+      # Need to subtract by 1 to comply with the personality profiles of "0", "1", and "2"
+      self.personality_profile = cp.vl["PCM_CRUISE_SM"]["DISTANCE_LINES"] - 1
+
+      # Sync with the onroad UI button
+      if self.param_memory.get_bool("PersonalityChangedViaUI"):
+        self.profile_restored = False
+        self.previous_personality_profile = self.param.get_int("LongitudinalPersonality")
+        self.param_memory.put_bool("PersonalityChangedViaUI", False)
+
+      # Set personality to the previous drive's personality or when the user changes it via the UI
+      if self.personality_profile == self.previous_personality_profile:
+        self.profile_restored = True
+      if not self.profile_restored:
+        self.distance_previously_pressed = not self.distance_previously_pressed
+        self.distance_button = not self.distance_previously_pressed
+
+      if self.profile_restored:
+        if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR):
+          # KRKeegan - Add support for toyota distance button
+          self.distance_button = cp_cam.vl["ACC_CONTROL"]["DISTANCE"]
+
+        elif self.CP.carFingerprint in RADAR_ACC_CAR:
+          # These cars have the acc_control on car can
+          self.distance_button = cp.vl["ACC_CONTROL"]["DISTANCE"]
+
+        elif self.CP.flags & ToyotaFlags.SMART_DSU:
+          self.distance_button = cp.vl["SDSU"]["FD_BUTTON"]
+
+        if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
+          self.param.put_int("LongitudinalPersonality", self.personality_profile)
+          self.param_memory.put_bool("PersonalityChangedViaWheel", True)
+          self.previous_personality_profile = self.personality_profile
+
     # Toggle Experimental Mode from steering wheel function
     if self.experimental_mode_via_press and ret.cruiseState.available and self.CP.carFingerprint != CAR.PRIUS_V:
       message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"]
@@ -252,6 +287,9 @@ class CarState(CarStateBase):
         ("PRE_COLLISION", 33),
       ]
 
+    if CP.flags & ToyotaFlags.SMART_DSU:
+      messages.append(("SDSU", 33))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod

selfdrive/car/toyota/toyotacan.py

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

selfdrive/car/volkswagen/carcontroller.py

@@ -104,7 +104,7 @@ class CarController:
       # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem?
       set_speed = hud_control.setSpeed * CV.MS_TO_KPH
       can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed,
-                                                       lead_distance))
+                                                       lead_distance, CS.personality_profile))
 
     # **** Stock ACC Button Controls **************************************** #
 

selfdrive/car/volkswagen/carstate.py

@@ -151,6 +151,25 @@ class CarState(CarStateBase):
     # Digital instrument clusters expect the ACC HUD lead car distance to be scaled differently
     self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"])
 
+    # Driving personalities function
+    if self.personalities_via_wheel and ret.cruiseState.available:
+      # Sync with the onroad UI button
+      if self.param_memory.get_bool("PersonalityChangedViaUI"):
+        self.personality_profile = self.param.get_int("LongitudinalPersonality")
+        self.param_memory.put_bool("PersonalityChangedViaUI", False)
+
+      # Change personality upon steering wheel button press
+      self.distance_button = pt_cp.vl["GRA_ACC_01"]["GRA_Verstellung_Zeitluecke"]
+
+      if self.distance_button and not self.distance_previously_pressed:
+        self.param_memory.put_bool("PersonalityChangedViaWheel", True)
+        self.personality_profile = (self.previous_personality_profile + 2) % 3
+      self.distance_previously_pressed = self.distance_button
+
+      if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
+        self.param.put_int("LongitudinalPersonality", self.personality_profile)
+        self.previous_personality_profile = self.personality_profile
+
     return ret
 
   def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type):

selfdrive/car/volkswagen/mqbcan.py

@@ -125,11 +125,11 @@ def create_acc_accel_control(packer, bus, acc_type, acc_enabled, accel, acc_cont
   return commands
 
 
-def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance):
+def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, personality):
   values = {
     "ACC_Status_Anzeige": acc_hud_status,
     "ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36,
-    "ACC_Gesetzte_Zeitluecke": 3,
+    "ACC_Gesetzte_Zeitluecke": 1 if personality == 0 else 3 if personality == 1 else 5,
     "ACC_Display_Prio": 3,
     "ACC_Abstandsindex": lead_distance,
   }