Match Drive Mode with Accel/Decel Profiles

opendbc_repo/opendbc/car/hyundai/carstate.py

@@ -131,6 +131,7 @@ class CarState(CarStateBase):
     self.ccnc_1b5_ts_ns = 0
     self.dashboard_limit = LimitTracker()
     self.dashboard_stop_sign = None
+    self.drive_mode = None
     self.ioniq6_bsm_sources = (Ioniq6BlindspotSources()
                                if CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and CP.openpilotLongitudinalControl else None)
     self.ioniq6_bsm_now_ns = 0
@@ -608,6 +609,10 @@ class CarState(CarStateBase):
     stamp = cp_cam.ts_nanos["ADAS_0x380"]["STOP_SIGN"]
     self.dashboard_stop_sign = (bool(cp_cam.vl["ADAS_0x380"]["STOP_SIGN"]), stamp) if stamp > 0 else None
 
+    if self.CP.flags & HyundaiFlags.EV:
+      stamp = cp.ts_nanos["DRIVE_MODE_EV"]["DRIVE_MODE"]
+      self.drive_mode = (int(cp.vl["DRIVE_MODE_EV"]["DRIVE_MODE"]), stamp) if stamp > 0 else None
+
     return ret
 
   def get_can_parsers_canfd(self, CP):
@@ -622,6 +627,7 @@ class CarState(CarStateBase):
       msgs.extend((("ADAS_CMD_50_50ms", math.nan), ("BLINDSPOTS_FRONT_CORNER_2", math.nan)))
     if CP.flags & HyundaiFlags.EV:
       msgs.append(("MANUAL_SPEED_LIMIT_ASSIST", math.nan))
+      msgs.append(("DRIVE_MODE_EV", math.nan))
     sign_subscription = ("FR_CMR_02_100ms", math.nan)
     if CP.flags & HyundaiFlags.CANFD_LKA_STEER_MSG:
       msgs.append(sign_subscription)

opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc

@@ -594,6 +594,11 @@ BO_ 512 ADRV_0x200: 8 ADRV
  SG_ SET_ME_E1 : 24|8@1+ (1,0) [0|255] "" XXX
  SG_ SET_ME_3A : 32|8@1+ (1,0) [0|255] "" XXX
 
+BO_ 517 DRIVE_MODE_EV: 8 XXX
+ SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
+ SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
+ SG_ DRIVE_MODE : 24|8@1+ (1,0) [0|255] "" XXX
+
 BO_ 593 RADAR_0x251: 16 FRONT_RADAR
  SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
  SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
@@ -1152,6 +1157,7 @@ VAL_ 506 ISLA_AutoUsmSta 0 "None Auto Function (Delete Menu)" 1 "Auto Off" 2 "Au
 VAL_ 506 ISLA_Cntry 0 "Europe/Russia/Australia" 1 "Domestic" 2 "China" 3 "USA" 4 "Canada" 5 "Australia" 6 "Reserved" 7 "Reserved" 8 "Reserved" 9 "Reserved" 10 "Reserved" 11 "Reserved" 12 "Reserved" 13 "Reserved" 14 "Reserved" 15 "Initial Value (default)";
 VAL_ 506 ISLA_AddtnlSign 0 "No Recognition (default)" 1 "School Crossing" 16 "Do Not Pass" 17 "Reserved" 18 "Reserved" 19 "Reserved" 20 "Reserved" 21 "Reserved" 22 "Reserved" 23 "Reserved" 24 "Exit" 25 "Roundabout" 26 "Right Curve" 27 "Left Curve" 28 "Winding Road" 29 "Reserved" 30 "Reserved" 31 "Reserved" 2 "Pedestrian Crossing" 3 "Bicycle Crossing" 4 "Reserved" 5 "Reserved" 6 "Reserved" 7 "Reserved" 8 "Stop" 9 "Yield" 10 "Stop Ahead" 11 "Yield Ahead" 12 "Road Construction Ahead" 13 "Lane Reduction" 14 "Reserved" 15 "Reserved";
 VAL_ 506 ISLA_SchoolZone 0 "No School Zone" 1 "School Zone" 2 "Reserved" 3 "Reserved";
+VAL_ 517 DRIVE_MODE 2 "Normal" 4 "Eco" 5 "Sport";
 VAL_ 687 HOD_Dir_Status 0 "HANDS OFF" 1 "HAND TOUCH (SOFT)" 2 "HAND TOUCH (STRONG)" 3 "HAND GRIP (SOFT)" 4 "HAND GRIP (STRONG)" 5 "RESERVED" 6 "RESERVED";
 VAL_ 698 IFSref_FR_CMR_Sta 0 "None Option (Default)" 1 "Normal" 2 "Blockage Status" 3 "Error Indicator";
 VAL_ 698 IFSref_VehNumVal 0 "No vehicle" 1 "Number of vehicles" 2 "Number of vehicles" 3 "Number of vehicles" 4 "Number of vehicles" 5 "Number of vehicles" 6 "Number of vehicles" 7 "Number of vehicles" 8 "Number of vehicles" 9 "Number of vehicles" 10 "Number of vehicles" 11 "Over than 10 vehicles" 12 "Reserved" 13 "Reserved" 14 "Default" 15 "Error indicator";

openpilot/common/params_keys.h

@@ -74,6 +74,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"QOLLongitudinal", {PERSISTENT, BOOL}},
     {"ReverseCruise", {PERSISTENT, BOOL, "0"}},
     {"ForceStops", {PERSISTENT, BOOL, "0"}},
+    {"DriveModeProfiles", {PERSISTENT, BOOL, "0"}},
+    {"DriveModeAcceleration", {PERSISTENT, BOOL, "1"}},
+    {"DriveModeDeceleration", {PERSISTENT, BOOL, "1"}},
     {"AlwaysAllowUploads", {PERSISTENT, BOOL, "0"}},
     {"UsageStatsState", {PERSISTENT | DONT_LOG, JSON}},
     {"UsageStatsStatus", {CLEAR_ON_MANAGER_START | DONT_LOG, JSON}},

openpilot/selfdrive/controls/plannerd.py

@@ -15,6 +15,7 @@ from openpilot.cereal.services import SERVICE_LIST
 from openpilot.starpilot.speed_limits import history as slc_history
 from openpilot.starpilot.speed_limits.runtime import Action as SlcAction, CruiseEvent as SlcCruiseEvent, Runtime as SlcRuntime
 from openpilot.starpilot.longitudinal.profile_runtime import ProfileHost
+from openpilot.starpilot.longitudinal.drive_mode import observe as observe_drive_mode
 from openpilot.starpilot.longitudinal.lane_change_gap import LaneGapPreferences
 from openpilot.starpilot.longitudinal.experimental_release import ConditionalHandoff
 from openpilot.starpilot.longitudinal.lead_approach import LeadApproachKey
@@ -591,7 +592,8 @@ def starpilot_main():
         # selected response supersedes the old global-only channel.
         global_braking_response = None
         selected_profiles = (profile_host.sample_selected(now_ns, sm['selfdriveState'].personality,
-                              sm['carState'].vEgo, CP, traffic_mode=traffic_mode, legacy=profile_tuning)
+                              sm['carState'].vEgo, CP, traffic_mode=traffic_mode, legacy=profile_tuning,
+                              drive_mode=observe_drive_mode(sm, now_ns=now_ns))
                              if profile_replay and profile_host is not None and
                              _profile_frame_valid(sm, now_ns) else None)
         lane_change_policy = lane_gap_preferences.sample(now_ns) if lane_gap_preferences is not None else None

openpilot/starpilot/car/hyundai/dashboard.py

@@ -1,7 +1,13 @@
-"""Optional Hyundai CAN-FD camera stop-sign observation; never a longitudinal control grant."""
+"""Optional Hyundai CAN-FD camera stop-sign and EV drive-mode observations; never a longitudinal control grant."""
+import time
+
 from opendbc.car.hyundai.values import HyundaiFlags
 from openpilot.starpilot.car.mazda.dashboard import write_stop_sign
 
+DRIVE_MODE_MAX_AGE_NS = 2_000_000_000
+DRIVE_MODE_ECO = 4
+DRIVE_MODE_SPORT = 5
+
 
 def capable(cp):
   return cp.brand == 'hyundai' and bool(int(cp.flags) & HyundaiFlags.CANFD) and not cp.notCar
@@ -10,4 +16,17 @@ def capable(cp):
 def compose(cp, parsed, state, message, *, car_state_stamp):
   if not capable(cp):
     return message
-  return write_stop_sign(parsed, state, message, car_state_stamp=car_state_stamp)
+  message = write_stop_sign(parsed, state, message, car_state_stamp=car_state_stamp)
+  return write_drive_mode(parsed, state, message)
+
+
+def write_drive_mode(parsed, state, message):
+  observation = getattr(parsed, 'drive_mode', None)
+  if type(observation) is not tuple or len(observation) != 2 or not state.canValid or state.canTimeout:
+    return message
+  mode, stamp = observation
+  age = time.clock_gettime_ns(getattr(time, 'CLOCK_BOOTTIME', time.CLOCK_MONOTONIC)) - stamp
+  if 0 <= age <= DRIVE_MODE_MAX_AGE_NS:
+    message.starpilotCarState.ecoGear = mode == DRIVE_MODE_ECO
+    message.starpilotCarState.sportGear = mode == DRIVE_MODE_SPORT
+  return message

openpilot/starpilot/feature_runtime.py

@@ -14,6 +14,7 @@ from openpilot.starpilot.aol.intent import independent_axis_requested, read_sett
 from openpilot.starpilot.aol.vehicle import policy_for as aol_policy_for
 from openpilot.starpilot.feature_capabilities import display_supported, longitudinal_supported
 from openpilot.starpilot.longitudinal.profile_runtime import read_settings as read_profile_settings, selected_profiles_requested, read_global_powertrain_preset
+from openpilot.starpilot.longitudinal.drive_mode import KEY as DRIVE_MODE_KEY
 from openpilot.starpilot.longitudinal.profile_preferences import read_document_value
 from openpilot.starpilot.saved_source import read_saved
 from openpilot.starpilot.speed_limits.runtime_settings import Settings as SlcSettings, read_params as read_slc_settings
@@ -52,7 +53,8 @@ def requested(params, feature: str, *, include_auxiliary: bool = False, saved_sl
     document = read_document_value(params)
     return (read_profile_settings(params) is not None or
             (document.valid and isinstance(document.value, dict) and
-             selected_profiles_requested(document.value)))
+             selected_profiles_requested(document.value)) or
+            (document.valid and _read(params, DRIVE_MODE_KEY, 8) == (b'1', True)))
   if feature in ('slc', 'vision'):
     settings = read_slc_settings(params) if saved_slc is None else saved_slc
     return (settings.enabled or settings.display) if feature == 'slc' else (

openpilot/starpilot/longitudinal/drive_mode.py

@@ -0,0 +1,41 @@
+"""Vehicle drive mode (Eco/Normal/Sport) selects the global acceleration and deceleration profiles."""
+
+KEY = 'DriveModeProfiles'
+ACCELERATION_KEY = 'DriveModeAcceleration'
+DECELERATION_KEY = 'DriveModeDeceleration'
+SOURCE_AGE_NS = 500_000_000
+PROFILES = {
+  'eco': ('eco', 'eco'),
+  'normal': ('standard', 'standard'),
+  'sport': ('sport_plus', 'sport'),
+}
+
+
+def read_enabled(params) -> tuple[bool, bool]:
+  from openpilot.starpilot.saved_source import read_saved
+  if read_saved(params, KEY, 8) != (b'1', True):
+    return False, False
+  return tuple(read_saved(params, key, 8) in ((None, True), (b'1', True)) for key in (ACCELERATION_KEY, DECELERATION_KEY))
+
+
+def observe(sm, *, now_ns: int) -> str | None:
+  try:
+    if not sm.seen['starpilotCarState'] or not sm.valid['starpilotCarState']:
+      return None
+    if not 0 <= now_ns - int(sm.logMonoTime['starpilotCarState']) <= SOURCE_AGE_NS:
+      return None
+    state = sm['starpilotCarState']
+    return 'eco' if state.ecoGear else 'sport' if state.sportGear else 'normal'
+  except (AttributeError, KeyError, TypeError, ValueError):
+    return None
+
+
+def apply(document: dict, drive_mode: str | None, *, acceleration: bool = True, deceleration: bool = True) -> dict:
+  if drive_mode not in PROFILES:
+    return document
+  selected_acceleration, selected_deceleration = PROFILES[drive_mode]
+  if acceleration:
+    document = {**document, 'selectedAccelerationProfile': selected_acceleration}
+  if deceleration:
+    document = {**document, 'selectedDecelerationProfile': selected_deceleration}
+  return document

openpilot/starpilot/longitudinal/profile_runtime.py

@@ -8,9 +8,11 @@ from dataclasses import astuple, dataclass
 from openpilot.common.constants import CV
 from opendbc.car.structs import car
 from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from openpilot.starpilot.longitudinal import drive_mode as drive_mode_profiles
 from openpilot.starpilot.longitudinal.accel_profile import A_CRUISE_MAX_VALS_TRAFFIC_ALL, interpolate_accel_profile
 from openpilot.starpilot.longitudinal.profile_document import (
-  DEFAULT_DECELERATION_PROFILE, active_personality_id, interpolate_category_curve, is_truck_fingerprint,
+  DEFAULT_DECELERATION_PROFILE, active_personality_id, default_personality_profiles, interpolate_category_curve,
+  is_truck_fingerprint, profile_document,
 )
 from openpilot.starpilot.longitudinal.profile_preferences import (
   FOLLOW_MIN_SECONDS, FOLLOW_MAX_SECONDS, NAMES, SCALARS, TRAFFIC_JERK_SUFFIXES,
@@ -368,6 +370,7 @@ class ProfileHost:
     self.disabled = False
     self.selected_document: dict | None = None
     self.selected_powertrain: GlobalPowertrainPreset | None = None
+    self.selected_drive_mode = (False, False)
     self.selected_attempt_ns = -self.REFRESH_NS
     self.selected_success_ns = -1
     self.settings: ProfileSettings | TrafficSettings | None = None
@@ -380,10 +383,11 @@ class ProfileHost:
     self.global_last_success_ns = -1
 
   def sample_selected(self, now_ns: int, personality, v_ego: float, CP, *, traffic_mode: bool | None = False,
-                      legacy: ProfileTuning | None = None) -> SelectedProfileTuning | None:
+                      legacy: ProfileTuning | None = None, drive_mode: str | None = None) -> SelectedProfileTuning | None:
     if now_ns < 0 or now_ns < self.selected_attempt_ns:
       self.selected_document = None
       self.selected_powertrain = None
+      self.selected_drive_mode = (False, False)
       self.selected_attempt_ns = now_ns - self.REFRESH_NS
       self.selected_success_ns = -1
     if now_ns - self.selected_attempt_ns >= self.REFRESH_NS:
@@ -392,14 +396,22 @@ class ProfileHost:
         saved = read_document_value(self.params)
         self.selected_document = saved.value if saved.valid and isinstance(saved.value, dict) else None
         self.selected_powertrain = read_global_powertrain_preset(self.params, CP)
+        self.selected_drive_mode = drive_mode_profiles.read_enabled(self.params)
         self.selected_success_ns = now_ns if saved.valid else -1
       except (OSError, RuntimeError, ValueError, TypeError, KeyError):
         self.selected_document = None
         self.selected_powertrain = None
+        self.selected_drive_mode = (False, False)
         self.selected_success_ns = -1
     if self.selected_success_ns < 0 or now_ns - self.selected_success_ns > self.MAX_AGE_NS:
       return None
-    return resolve_selected_profiles(self.selected_document, personality, v_ego, CP,
+    document = self.selected_document
+    if any(self.selected_drive_mode) and traffic_mode is False and drive_mode in drive_mode_profiles.PROFILES:
+      if document is None:
+        document = profile_document(default_personality_profiles(False), enabled=False)
+      document = drive_mode_profiles.apply(document, drive_mode, acceleration=self.selected_drive_mode[0],
+                                           deceleration=self.selected_drive_mode[1])
+    return resolve_selected_profiles(document, personality, v_ego, CP,
                                      traffic_mode=traffic_mode, legacy=legacy, global_powertrain=self.selected_powertrain)
 
   def sample_global_braking(self, now_ns: int) -> str | None:

openpilot/starpilot/ui/feature_settings_owner.py

@@ -76,8 +76,11 @@ from openpilot.starpilot.controllers.toyota_cruise import capability as toyota_c
 
 GLOBAL_BOOL_PREFERENCES = frozenset((LEAD_APPROACH_KEY, LEAD_TAKEOFF_KEY, NUDGE_KEY, "DisengageOnAccelerator"))
 
+DRIVE_MODE_KEYS = ("DriveModeProfiles", "DriveModeAcceleration", "DriveModeDeceleration")
+
 BOOL_DEFAULTS = {
   "GMPedalLongitudinal": False, "ForceStops": False, "AlwaysAllowUploads": False, "TurnAssist": True, "HighwaySmoothing": False,
+  "DriveModeProfiles": False, "DriveModeAcceleration": True, "DriveModeDeceleration": True,
   "SubaruAvhStartup": False,
   "SubaruStopStartOff": False,
   "SubaruSNG": False,
@@ -806,6 +809,12 @@ class FeatureSettingsOwner:
                           authorized=authorized, temp_prefix=".turn-assist-")
     return result.committed and result.verified
 
+  def _drive_mode_capable(self) -> bool:
+    from opendbc.car.hyundai.values import HyundaiFlags
+    cp = self.vehicle_params()
+    return (cp is not None and cp.brand == "hyundai" and not cp.notCar and
+            bool(int(cp.flags) & HyundaiFlags.CANFD) and bool(int(cp.flags) & HyundaiFlags.EV))
+
   def _highway_smoothing_capability(self) -> tuple | None:
     from openpilot.starpilot.lateral.controller_selection import policy_for
     cp = self.vehicle_params()
@@ -1255,6 +1264,16 @@ class FeatureSettingsOwner:
       if self._galaxy():
         rows.append(self._bool_row("DisengageOnAccelerator", "Disengage on Accelerator Pedal", self.authority("preferences")))
       document, raw, valid = self._document()
+      if self._drive_mode_capable():
+        match_enabled, _, _ = self._value("DriveModeProfiles")
+        rows.append(self._bool_row("DriveModeProfiles", "Match Drive Mode", allowed,
+                                   reason="Eco, Normal and Sport select Eco, Standard and Sport+ acceleration with Eco, "
+                                          "Standard and Sport deceleration, replacing the selected profiles below. Traffic Mode is unchanged."))
+        if match_enabled == "1":
+          rows.append(self._bool_row("DriveModeAcceleration", "Match Acceleration", allowed,
+                                     reason="Eco, Normal and Sport select Eco, Standard and Sport+ acceleration."))
+          rows.append(self._bool_row("DriveModeDeceleration", "Match Deceleration", allowed,
+                                     reason="Eco, Normal and Sport select Eco, Standard and Sport deceleration."))
       for category, field, label, choices in (
         ('acceleration', 'selectedAccelerationProfile', 'Selected Acceleration Profile',
          ('dom_default', 'standard', 'eco', 'sport', 'sport_plus')),
@@ -1541,7 +1560,7 @@ class FeatureSettingsOwner:
       group = "long"
     elif key in BOOL_DEFAULTS or key in FLOAT_SPECS or key in (SLC_PRIORITY, SLC_SECONDARY, 'SLCAdvanceSeconds'):
       source_key = key
-      long_key = (key in ("CustomPersonalities", "ForceStops", "ForceStopDistanceOffset") or "Personality" in key or
+      long_key = (key in ("CustomPersonalities", "ForceStops", "ForceStopDistanceOffset", *DRIVE_MODE_KEYS) or "Personality" in key or
                   key.endswith(("Follow", "FollowHigh")) or "Jerk" in key)
       if key in ("ShowSpeedLimits", "AlwaysAllowUploads", SLC_PRIORITY, SLC_SECONDARY) or (key == "ForceStops" and request.value == "Off"):
         group = "preferences"