fix Lanemode. (#162)

* test socketmaster...

* remove LongVelocityControl

* fix.. doorOpen, seatBelt...

* fix..  steerTemp alert

* torqued liveDelay

* sync cruise_state, fix.. angle_steer

* tr7

* Revert "tr7"

This reverts commit ebbabdbfc8917efa0bae12a150c6841965d59675.

* fix....

* fix..

* fix..

* Hyundai: Sonata 2024 Car Port (#161)

* fix..

* fix... angle curve scale

* fix.. sonata

* add.. j_lead clip

* fix.. jlead clip

* fix.. angle torque...

* fix.. mdps..

* fix..

* fix lanemode

* fix..

* fix.. lanemode

* ff

* fix..

* fix..

* fix..

---------

Co-authored-by: Kirito3481 <47619491+Kirito3481@users.noreply.github.com>

opendbc_repo/opendbc/car/fingerprints.py

@@ -209,6 +209,7 @@ MIGRATION = {
   "HYUNDAI PALISADE 2020": HYUNDAI.HYUNDAI_PALISADE,
   "HYUNDAI VELOSTER 2019": HYUNDAI.HYUNDAI_VELOSTER,
   "HYUNDAI SONATA HYBRID 2021": HYUNDAI.HYUNDAI_SONATA_HYBRID,
+  "HYUNDAI SONATA 2024": HYUNDAI.HYUNDAI_SONATA_2024,
   "HYUNDAI IONIQ 5 2022": HYUNDAI.HYUNDAI_IONIQ_5,
   "HYUNDAI IONIQ 5 PE (NE1)": HYUNDAI.HYUNDAI_IONIQ_5_PE,
   "HYUNDAI IONIQ 6 2023": HYUNDAI.HYUNDAI_IONIQ_6,

opendbc_repo/opendbc/car/hyundai/carcontroller.py

@@ -94,6 +94,7 @@ class CarController(CarControllerBase):
     self.LFA_trigger = 0
 
     self.activeCarrot = 0
+    self.camera_scc_params = Params().get_int("HyundaiCameraSCC")
 
   def update(self, CC, CS, now_nanos):
 
@@ -120,7 +121,7 @@ class CarController(CarControllerBase):
       self.speed_from_pcm = params.get_int("SpeedFromPCM")
 
       self.canfd_debug = params.get_int("CanfdDebug")
-
+      self.camera_scc_params = params.get_int("HyundaiCameraSCC")
 
     actuators = CC.actuators
     hud_control = CC.hudControl
@@ -151,7 +152,7 @@ class CarController(CarControllerBase):
       else:
         curv = abs(actuators.curvature)
         y_std = actuators.yStd
-        curvature_threshold = np.interp(y_std, [0.0, 0.25], [0.5, 0.006])
+        curvature_threshold = np.interp(y_std, [0.0, 0.2], [0.5, 0.006])
 
         curve_scale = np.clip(curv / curvature_threshold, 0.0, 1.0)
         torque_pts = [
@@ -159,7 +160,7 @@ class CarController(CarControllerBase):
           (1 - curve_scale) * self.angle_max_torque + curve_scale * 50,
           self.angle_max_torque
         ]        
-        base_max_torque = np.interp(CS.out.vEgo * CV.MS_TO_KPH, [0, 20, 30], torque_pts)
+        base_max_torque = np.interp(CS.out.vEgo * CV.MS_TO_KPH, [0, 30, 60], torque_pts)
       
       target_torque = np.interp(abs(actuators.curvature), [0.0, 0.003, 0.006], [0.5 * base_max_torque, 0.75 * base_max_torque, base_max_torque])
 
@@ -264,7 +265,7 @@ class CarController(CarControllerBase):
         self.hyundai_jerk.make_jerk(self.CP, CS, accel, actuators, hud_control)
 
         if True: #not camera_scc:
-          if camera_scc:
+          if self.camera_scc_params == 2:
             self.canfd_toggle_adas(CC, CS)
           can_sends.extend(hyundaicanfd.create_ccnc_messages(self.CP, self.packer, self.CAN, self.frame, CC, CS, hud_control, apply_angle, left_lane_warning, right_lane_warning, self.canfd_debug, self.MainMode_ACC_trigger, self.LFA_trigger))
           if hda2:

opendbc_repo/opendbc/car/hyundai/fingerprints.py

@@ -225,6 +225,15 @@ FW_VERSIONS = {
       b'\xf1\x00LFF LKAS AT USA LHD 1.01 1.02 95740-C1000 E52',
     ],
   },
+  CAR.HYUNDAI_SONATA_2024: {
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00DN8_ RDR -----      1.00 1.00 99110-L1800         ',
+    ],
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00DN8 MFC  AT KOR LHD 1.00 1.01 99211-L1800 230512',
+      b'\xf1\x00DN8 MFC  AT USA LHD 1.00 1.01 99211-L1800 230512',
+    ],
+  },
   CAR.HYUNDAI_TUCSON: {
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00TL__ FCA F-CUP      1.00 1.01 99110-D3500         ',

opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py

@@ -97,7 +97,7 @@ def create_steering_messages_camera_scc(frame, packer, CP, CAN, CC, lat_active,
       values["LKA_ACTIVE"] = 1 if CS.lfa_info["STEER_REQ"] == 1 else 0
 
   if frame % 1000 < 40:
-    values["STEERING_COL_TORQUE"] += 100
+    values["STEERING_COL_TORQUE"] += 120
   ret.append(packer.make_can_msg("MDPS", CAN.CAM, values))
 
   if frame % 10 == 0:
@@ -422,20 +422,13 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle
         values["DATA102"] = 0  # steer_temp관련없음
         ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values))
 
-      """
       if CS.cruise_buttons_msg is not None:
         values = CS.cruise_buttons_msg
         if MainMode_ACC_trigger > 0:
-          #values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1
+          values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1
-          pass
         elif LFA_trigger > 0:
-          pass
+          values["LFA_BTN"] = 1
-          #values["LFA_BTN"] = 1
-          #values["COUNTER"] = (values["COUNTER"] + 1) % 256
-          #ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.ECAN, values))
-          #ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.ECAN, values))
         ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values))
-      """
 
 
     if frame % 5 == 0:

opendbc_repo/opendbc/car/hyundai/values.py

@@ -348,6 +348,11 @@ class CAR(Platforms):
 
     flags=HyundaiFlags.UNSUPPORTED_LONGITUDINAL | HyundaiFlags.TCU_GEARS,
   )
+  HYUNDAI_SONATA_2024 = HyundaiCanFDPlatformConfig(
+    [HyundaiCarDocs("Hyundai Sonata 2024-25", "All", car_parts=CarParts.common([CarHarness.hyundai_a]))],
+    CarSpecs(mass=1556, wheelbase=2.84, steerRatio=12.81),
+    flags=HyundaiFlags.CAMERA_SCC,
+  )
   HYUNDAI_STARIA_4TH_GEN = HyundaiCanFDPlatformConfig(
     [HyundaiCarDocs("Hyundai Staria 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))],
     CarSpecs(mass=2205, wheelbase=3.273, steerRatio=11.94),  # https://www.hyundai.com/content/dam/hyundai/au/en/models/staria-load/premium-pip-update-2023/spec-sheet/STARIA_Load_Spec-Table_March_2023_v3.1.pdf

opendbc_repo/opendbc/car/torque_data/substitute.toml

@@ -45,6 +45,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
 "GENESIS_G90" = "GENESIS_G70"
 "GENESIS_G80" = "GENESIS_G70"
 "GENESIS_G70_2020" = "HYUNDAI_SONATA"
+"HYUNDAI_SONATA_2024" = "HYUNDAI_SONATA"
 
 "HONDA_FREED" = "HONDA_ODYSSEY"
 "HONDA_CRV_EU" = "HONDA_CRV"

opendbc_repo/opendbc/dbc/hyundai_canfd.dbc

@@ -137,6 +137,9 @@ BO_ 234 MDPS: 24 XXX
  SG_ STEERING_ANGLE_2 : 128|16@1- (-0.1,0) [0|65535] "deg" XXX
  SG_ LFA2_ACTIVE : 145|2@0+ (1,0) [0|3] "" XXX
  SG_ LFA2_FAULT : 149|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEW_SIGNAL_4 : 159|8@0+ (1,0) [0|255] "" XXX
+ SG_ NEW_SIGNAL_5 : 167|8@0+ (1,0) [0|255] "" XXX
+ SG_ NEW_SIGNAL_6 : 175|8@0+ (1,0) [0|255] "" XXX
 
 BO_ 256 ACCELERATOR_BRAKE_ALT: 32 XXX
  SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
@@ -480,6 +483,7 @@ BO_ 463 CRUISE_BUTTONS: 8 XXX
  SG_ CRUISE_BUTTONS : 16|3@1+ (1,0) [0|3] "" XXX
  SG_ RIGHT_PADDLE : 25|1@1+ (1,0) [0|1] "" XXX
  SG_ LEFT_PADDLE : 27|1@1+ (1,0) [0|1] "" XXX
+ SG_ SET_ME_1_ : 40|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 474 ADRV_0x1da: 32 ADRV
  SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX

scripts/add/events_ko.py

@@ -314,7 +314,7 @@ def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM
 
 def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
   text = "크루즈 버튼으로 활성화됩니다"
-  if CP.carName == "honda":
+  if CP.brand == "honda":
     text = "메인 스위치로 활성화됩니다"
   return NoEntryAlert(text)
 

selfdrive/carrot/carrot_functions.py

@@ -212,7 +212,7 @@ class CarrotPlanner:
       self.jerk_factor_apply = self.jerk_factor * self.dynamicTFollowLC   # 차선변경시 jerk factor를 줄여 aggresive하게
     elif lead.status:      
       if self.dynamicTFollow > 0.0:
-        gap_dist_adjust = np.clip((desired_follow_distance - lead.dRel) * self.dynamicTFollow, - 0.1, 1.0)
+        gap_dist_adjust = np.clip((desired_follow_distance - lead.dRel) * self.dynamicTFollow, - 0.1, 1.0) * 0.1
         t_follow += gap_dist_adjust
         if gap_dist_adjust < 0:
           self.jerk_factor_apply = self.jerk_factor * 0.5 # 전방차량을 따라갈때는 aggressive하게.

selfdrive/carrot_settings.json

@@ -1098,12 +1098,12 @@
       "group": "시작",
       "name": "HyundaiCameraSCC",
       "title": "HYUNDAI: CAMERA SCC",
-      "descr": "SCC배선 개조시 1로 설정",
+      "descr": "SCC가 카메라쪽에 있거나, 배선 개조시 1,2로 설정, 2:Sync cruise state(canfd)",
       "egroup": "START",
       "etitle": "HYUNDAI: CAMERA SCC(0)",
-      "edescr": "",
+      "edescr": "SCC's CAN line connected to CAM, 2:Sync ruise state(canfd)",
       "min": 0,
-      "max": 1,
+      "max": 2,
       "default": 0,
       "unit": 1
     },
@@ -1248,7 +1248,7 @@
       "min": 0,
       "max": 100,
       "default": 0,
-      "unit": 5
+      "unit": 1
     },
     {
       "group": "차량간격",

selfdrive/controls/controlsd.py

@@ -140,7 +140,7 @@ class Controls:
 
     if len(model_v2.position.yStd) > 0:
       yStd = np.interp(steer_actuator_delay + lat_smooth_seconds, ModelConstants.T_IDXS, model_v2.position.yStd)
-      self.yStd = yStd * 0.1 + self.yStd * 0.9
+      self.yStd = yStd * 0.02 + self.yStd * 0.98
     else:
       self.yStd = 0.0
     
@@ -154,8 +154,8 @@ class Controls:
           alpha = 1 - np.exp(-DT_CTRL / tau) if tau > 0 else 1
           return alpha * val + (1 - alpha) * prev_val
 
-        t_since_plan = (self.sm.frame - self.sm.recv_frame['lateralPlan']) * DT_CTRL
+        curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay + lat_smooth_seconds)
-        curvature = np.interp(steer_actuator_delay + lat_smooth_seconds + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], lat_plan.curvatures)          
+
         new_desired_curvature = smooth_value(curvature, self.desired_curvature, lat_smooth_seconds)
     else:
       new_desired_curvature = model_v2.action.desiredCurvature

selfdrive/locationd/torqued.py

@@ -52,7 +52,7 @@ class TorqueBuckets(PointBuckets):
 class TorqueEstimator(ParameterEstimator):
   def __init__(self, CP, decimated=False, track_all_points=False):
     self.hist_len = int(HISTORY / DT_MDL)
-    self.lag = Params().get_float("SteerActuatorDelay") * 0.01 #CP.steerActuatorDelay + .2  # from controlsd
+    self.lag = 0.0
     self.track_all_points = track_all_points  # for offline analysis, without max lateral accel or max steer torque filters
     if decimated:
       self.min_bucket_points = MIN_BUCKET_POINTS / 10
@@ -171,6 +171,8 @@ class TorqueEstimator(ParameterEstimator):
       # TODO: check if high aEgo affects resulting lateral accel
       self.raw_points["vego"].append(msg.vEgo)
       self.raw_points["steer_override"].append(msg.steeringPressed)
+    elif which == "liveDelay":
+      self.lag = msg.lateralDelay
     elif which == "liveLocationKalman":
       if len(self.raw_points['steer_torque']) == self.hist_len:
         yaw_rate = msg.angularVelocityCalibrated.value[2]
@@ -231,7 +233,7 @@ def main(demo=False):
   config_realtime_process([0, 1, 2, 3], 5)
 
   pm = messaging.PubMaster(['liveTorqueParameters'])
-  sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman'], poll='liveLocationKalman')
+  sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveDelay', 'liveLocationKalman'], poll='liveLocationKalman')
 
   params = Params()
   estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams))

selfdrive/ui/qt/offroad/settings.cc

@@ -785,7 +785,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
   });
 
   startToggles->addItem(selectCarBtn);
-  startToggles->addItem(new ParamControl("HyundaiCameraSCC", "HYUNDAI: CAMERA SCC", "Connect the SCC's CAN line to CAM", "../assets/offroad/icon_shell.png", this));
+  startToggles->addItem(new CValueControl("HyundaiCameraSCC", "HYUNDAI: CAMERA SCC", "1:Connect the SCC's CAN line to CAM, 2:Sync Cruise state", "../assets/offroad/icon_shell.png", 0, 2, 1));
   startToggles->addItem(new CValueControl("EnableRadarTracks", "Enable Radar Track", "1:Enable RadarTrack, -1,2:Disable use HKG SCC radar at all times", "../assets/offroad/icon_shell.png", -1, 2, 1));
   startToggles->addItem(new CValueControl("CanfdHDA2", "CANFD: HDA2 mode", "1:HDA2,2:HDA2+BSM", "../assets/offroad/icon_shell.png", 0, 2, 1));
   startToggles->addItem(new CValueControl("AutoCruiseControl", "Auto Cruise control", "Softhold, Auto Cruise ON/OFF control", "../assets/offroad/icon_road.png", 0, 3, 1));

system/updated/updated.py

@@ -9,6 +9,7 @@ import signal
 import fcntl
 import time
 import threading
+import gc
 from collections import defaultdict
 from pathlib import Path
 
@@ -71,9 +72,13 @@ def read_time_from_param(params, param) -> datetime.datetime | None:
     pass
   return None
 
-def run(cmd: list[str], cwd: str = None) -> str:
+def run_org(cmd: list[str], cwd: str = None) -> str:
   return subprocess.check_output(cmd, cwd=cwd, stderr=subprocess.STDOUT, encoding='utf8')
 
+def run(cmd: list[str], cwd: str = None) -> str:
+  proc = subprocess.run(cmd, cwd=cwd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, encoding='utf8', check=True)
+  return proc.stdout
+
 
 def set_consistent_flag(consistent: bool) -> None:
   os.sync()
@@ -492,6 +497,9 @@ def main() -> None:
         cloudlog.exception("uncaught updated exception, shouldn't happen")
         exception = str(e)
         OVERLAY_INIT.unlink(missing_ok=True)
+      finally:
+        gc.collect()
+
 
       try:
         params.put("UpdaterState", "idle")