AGNOS12.1, fix Lanemode, stock cruise mode, radar reaction factor.. (#172)

* camera SCC stockLongSupport, fix.. visionTrack velocity * for lanemode test... * fix.. lanemode.. * remove mpc cost * remove lane adjust curve offset and using laneoffset * fix.. * fix unit.. * Revert "remove mpc cost" This reverts commit 1d3ed8ef3212b467a094261ababc2d6f45c1e714. * fix dbc(LFAHDA_CLUSTER) unknown bit. * Revert "remove lane adjust curve offset and using laneoffset" This reverts commit c244173c1196dc2fc0bf92c638a9d5bed961cac1. * fix * remove curveoffset, fix lane delay(0.06)/offset(0.06) setting * fix latSmoothSec(0.13) * a_lead_tau threshold adjust(using radar reaction factor) * AGNOS 12.1 * fix lanemode delay/offset -> 0.05 * fix..
2025-05-18 12:44:49 +09:00 · 2025-05-18 12:44:49 +09:00 · a87f8b3376
commit a87f8b3376
parent b61696fd28
19 changed files with 132 additions and 143 deletions
--- a/common/params_keys.h
+++ b/common/params_keys.h
@ -195,8 +195,6 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
    {"MyDrivingModeAuto", PERSISTENT},
    {"TrafficLightDetectMode", PERSISTENT},
    {"SteerActuatorDelay", PERSISTENT},
-    {"SteerSmoothSec", PERSISTENT},
-    {"SteerLagGain", PERSISTENT },
    {"CruiseOnDist", PERSISTENT},
    {"CruiseMaxVals1", PERSISTENT},
    {"CruiseMaxVals2", PERSISTENT},
@ -230,8 +228,6 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
    {"UseLaneLineCurveSpeed", PERSISTENT},
    {"UseLaneLineSpeedApply", PERSISTENT},
    {"AdjustLaneOffset", PERSISTENT},
-    {"AdjustCurveOffset", PERSISTENT},
-    {"AdjustLaneTime", PERSISTENT},
    {"LaneChangeNeedTorque", PERSISTENT},
    {"MaxAngleFrames", PERSISTENT},
    {"SoftHoldMode", PERSISTENT},
--- a/launch_env.sh
+++ b/launch_env.sh
@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1
 export VECLIB_MAXIMUM_THREADS=1

 if [ -z "$AGNOS_VERSION" ]; then
-  export AGNOS_VERSION="11.13"
+  export AGNOS_VERSION="12.1"
 fi

 export STAGING_ROOT="/data/safe_staging"
--- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py
+++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py
@ -261,12 +261,12 @@ class CarController(CarControllerBase):
      if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
        can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker))

+      if self.camera_scc_params in [2, 3]:
+        self.canfd_toggle_adas(CC, CS)
      if self.CP.openpilotLongitudinalControl:
        self.hyundai_jerk.make_jerk(self.CP, CS, accel, actuators, hud_control)

        if True: #not 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:
            can_sends.extend(hyundaicanfd.create_adrv_messages(self.CP, self.packer, self.CAN, self.frame))
@ -283,7 +283,12 @@ class CarController(CarControllerBase):
          self.accel_last = accel
      else:
        # button presses
-        can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))
+        if self.camera_scc_params == 3: # camera scc but stock long
+          send_button = self.make_spam_button(CC, CS)
+          can_sends.extend(hyundaicanfd.forward_button_message(self.packer, self.CAN, self.frame, CS, send_button, self.MainMode_ACC_trigger, self.LFA_trigger))
+        else:
+          can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))
+        
    else:
      can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.CP, apply_torque, apply_steer_req,
                                                torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
@ -336,6 +341,7 @@ class CarController(CarControllerBase):
    self.frame += 1
    return new_actuators, can_sends

+
  def create_button_messages(self, CC: structs.CarControl, CS: CarState, use_clu11: bool):
    can_sends = []
    if CS.out.brakePressed or CS.out.brakeHoldActive:
--- a/opendbc_repo/opendbc/car/hyundai/carstate.py
+++ b/opendbc_repo/opendbc/car/hyundai/carstate.py
@ -1,6 +1,7 @@
 from collections import deque
 import copy
 import math
+import numpy as np

 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
@ -398,20 +399,22 @@ class CarState(CarStateBase):
      self.main_enabled = True
    # CAN FD cars enable on main button press, set available if no TCS faults preventing engagement
    ret.cruiseState.available = self.main_enabled #cp.vl["TCS"]["ACCEnable"] == 0
+    if self.CP.flags & HyundaiFlags.CAMERA_SCC.value:
+      self.MainMode_ACC = cp_cam.vl["SCC_CONTROL"]["MainMode_ACC"] == 1
+      self.LFA_ICON = cp_cam.vl["LFAHDA_CLUSTER"]["LFA_ICON"]
+      
    if self.CP.openpilotLongitudinalControl:
      # These are not used for engage/disengage since openpilot keeps track of state using the buttons
      ret.cruiseState.enabled = cp.vl["TCS"]["ACC_REQ"] == 1
      ret.cruiseState.standstill = False
-      if self.CP.flags & HyundaiFlags.CAMERA_SCC.value:
-        self.MainMode_ACC = cp_cam.vl["SCC_CONTROL"]["MainMode_ACC"] == 1
-        self.LFA_ICON = cp_cam.vl["LFAHDA_CLUSTER"]["LFA_ICON"]
-        if self.MainMode_ACC:
-          self.main_enabled = True
+      if self.MainMode_ACC:
+        self.main_enabled = True
    else:
      cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp
      ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2)
      if cp_cruise_info.vl["SCC_CONTROL"]["MainMode_ACC"] == 1: # carrot
        ret.cruiseState.available = self.main_enabled = True
+        ret.pcmCruiseGap = int(np.clip(cp_cruise_info.vl["SCC_CONTROL"]["DISTANCE_SETTING"], 1, 4))
      ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["CRUISE_STANDSTILL"] == 1
      ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor
      self.cruise_info = copy.copy(cp_cruise_info.vl["SCC_CONTROL"])
--- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
+++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
@ -408,6 +408,23 @@ def create_tcs_messages(packer, CAN, CS):
    ret.append(packer.make_can_msg("TCS", CAN.CAM, values))
  return ret

+def forward_button_message(packer, CAN, frame, CS, cruise_button, MainMode_ACC_trigger, LFA_trigger):
+  ret = []
+  if frame % 2 == 0:
+    if CS.cruise_buttons_msg is not None:
+      values = CS.cruise_buttons_msg
+      cruise_button_driver = values["CRUISE_BUTTONS"]
+      if cruise_button_driver == 0:
+        values["CRUISE_BUTTONS"] = cruise_button
+      if MainMode_ACC_trigger > 0:
+        #values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1
+        pass
+      elif LFA_trigger > 0:
+        values["LFA_BTN"] = 1
+        
+      ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values))
+  return ret
+
 def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle, left_lane_warning, right_lane_warning, canfd_debug, MainMode_ACC_trigger, LFA_trigger):
  ret = []

--- a/opendbc_repo/opendbc/car/hyundai/interface.py
+++ b/opendbc_repo/opendbc/car/hyundai/interface.py
@ -178,7 +178,7 @@ class CarInterface(CarInterfaceBase):
    # carrot, if camera_scc enabled, enable openpilotLongitudinalControl
    if ret.flags & HyundaiFlags.CAMERA_SCC.value or params.get_int("EnableRadarTracks") > 0:
      ret.radarUnavailable = False
-      ret.openpilotLongitudinalControl = True
+      ret.openpilotLongitudinalControl = True if camera_scc != 3 else False
      print(f"$$$OenpilotLongitudinalControl = True, CAMERA_SCC({ret.flags & HyundaiFlags.CAMERA_SCC.value}) or RadarTracks{params.get_int('EnableRadarTracks')}")
    else:
      print(f"$$$OenpilotLongitudinalControl = {alpha_long}")
--- a/opendbc_repo/opendbc/dbc/hyundai_canfd.dbc
+++ b/opendbc_repo/opendbc/dbc/hyundai_canfd.dbc
@ -504,6 +504,7 @@ BO_ 480 LFAHDA_CLUSTER: 16 ADRV
 SG_ NEW_SIGNAL_2 : 30|1@0+ (1,0) [0|1] "" XXX
 SG_ NEW_SIGNAL_3 : 49|1@0+ (1,0) [0|1] "" XXX
 SG_ NEW_SIGNAL_4 : 24|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEW_SIGNAL_5 : 25|1@0+ (1,0) [0|1] "" XXX

 BO_ 490 ADRV_0x1ea: 32 ADRV
 SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
--- a/opendbc_repo/opendbc/safety/safety/safety_hyundai_canfd.h
+++ b/opendbc_repo/opendbc/safety/safety/safety_hyundai_canfd.h
@ -40,6 +40,7 @@ const CanMsg HYUNDAI_CANFD_HDA2_LONG_TX_MSGS[] = {
  {0x1CF, 1, 8},  // CRUISE_BUTTON
  {0x1CF, 2, 8},  // CRUISE_BUTTON
  {0x1AA, 0, 16}, // CRUISE_ALT_BUTTONS , carrot
+  {0x1AA, 1, 16}, // CRUISE_ALT_BUTTONS , carrot
  {0x1AA, 2, 16}, // CRUISE_ALT_BUTTONS , carrot
  {0x2A4, 0, 24}, // CAM_0x2A4
  {0x51, 0, 32},  // ADRV_0x51
@ -62,9 +63,11 @@ const CanMsg HYUNDAI_CANFD_HDA2_LONG_TX_MSGS[] = {
  {0x345, 0, 8},  // ADRV_0x345
  {0x1DA, 0, 32}, // ADRV_0x1da

+  {0x362, 0, 32}, // CAM_0x362
  {0x362, 1, 32}, // CAM_0x362
  {0x2a4, 1, 24}, // CAM_0x2a4

+  {0x110, 0, 32}, // LKAS_ALT (272)
  {0x110, 1, 32}, // LKAS_ALT (272)

  {0x50, 1, 16}, // 
@ -173,6 +176,21 @@ RxCheck hyundai_canfd_hda2_rx_checks[] = {
  HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(1)  // TODO: carrot: canival no 0x1cf
  HYUNDAI_CANFD_SCC_ADDR_CHECK(1)
 };
+RxCheck hyundai_canfd_hda2_rx_checks_scc2[] = {
+  HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
+  HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(0)  // TODO: carrot: canival no 0x1cf
+  HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
+};
+RxCheck hyundai_canfd_hda2_alt_buttons_rx_checks[] = {
+  HYUNDAI_CANFD_COMMON_RX_CHECKS(1)
+  HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(1)
+  HYUNDAI_CANFD_SCC_ADDR_CHECK(1)
+};
+RxCheck hyundai_canfd_hda2_alt_buttons_rx_checks_scc2[] = {
+  HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
+  HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(0)
+  HYUNDAI_CANFD_SCC_ADDR_CHECK(2)
+};
 RxCheck hyundai_canfd_hda2_long_rx_checks[] = {
  HYUNDAI_CANFD_COMMON_RX_CHECKS(1)
  HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(1)  // TODO: carrot: canival no 0x1cf
@ -181,11 +199,6 @@ RxCheck hyundai_canfd_hda2_long_rx_checks_scc2[] = {
  HYUNDAI_CANFD_COMMON_RX_CHECKS(0)
  HYUNDAI_CANFD_BUTTONS_ADDR_CHECK(0)  
 };
-RxCheck hyundai_canfd_hda2_alt_buttons_rx_checks[] = {
-  HYUNDAI_CANFD_COMMON_RX_CHECKS(1)
-  HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(1)
-  HYUNDAI_CANFD_SCC_ADDR_CHECK(1)
-};
 RxCheck hyundai_canfd_hda2_long_alt_buttons_rx_checks[] = {
  HYUNDAI_CANFD_COMMON_RX_CHECKS(1)
  HYUNDAI_CANFD_ALT_BUTTONS_ADDR_CHECK(1)
@ -196,7 +209,6 @@ RxCheck hyundai_canfd_hda2_long_alt_buttons_rx_checks_scc2[] = {
 };


-
 const int HYUNDAI_PARAM_CANFD_ALT_BUTTONS = 32;
 const int HYUNDAI_PARAM_CANFD_HDA2_ALT_STEERING = 128;
 bool hyundai_canfd_alt_buttons = false;
@ -538,14 +550,26 @@ static safety_config hyundai_canfd_init(uint16_t param) {
                                        BUILD_SAFETY_CFG(hyundai_canfd_long_rx_checks, HYUNDAI_CANFD_HDA1_TX_MSGS);
    }
  } else {
-    if (hyundai_canfd_hda2) {
+    print("hyundai safety canfd_hda2 stock");
+    if (hyundai_camera_scc) print("camera_scc \n");
+    else print("no camera_scc \n");
+    if (hyundai_canfd_hda2 && hyundai_camera_scc) {
+      if (hyundai_canfd_alt_buttons) { // carrot : for CANIVAL 4TH HDA2
+        ret = hyundai_canfd_hda2_alt_steering ? BUILD_SAFETY_CFG(hyundai_canfd_hda2_alt_buttons_rx_checks_scc2, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS) : \
+          BUILD_SAFETY_CFG(hyundai_canfd_hda2_alt_buttons_rx_checks_scc2, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS);
+      }
+      else {
+        ret = hyundai_canfd_hda2_alt_steering ? BUILD_SAFETY_CFG(hyundai_canfd_hda2_rx_checks_scc2, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS) : \
+          BUILD_SAFETY_CFG(hyundai_canfd_hda2_rx_checks_scc2, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS);
+      }
+    }else if (hyundai_canfd_hda2) {
        if (hyundai_canfd_alt_buttons) { // carrot : for CANIVAL 4TH HDA2
-            ret = hyundai_canfd_hda2_alt_steering ? BUILD_SAFETY_CFG(hyundai_canfd_hda2_alt_buttons_rx_checks, HYUNDAI_CANFD_HDA2_ALT_STEERING_TX_MSGS) : \
-                BUILD_SAFETY_CFG(hyundai_canfd_hda2_alt_buttons_rx_checks, HYUNDAI_CANFD_HDA2_TX_MSGS);
+            ret = hyundai_canfd_hda2_alt_steering ? BUILD_SAFETY_CFG(hyundai_canfd_hda2_alt_buttons_rx_checks, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS) : \
+                BUILD_SAFETY_CFG(hyundai_canfd_hda2_alt_buttons_rx_checks, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS);
        }
        else {
-            ret = hyundai_canfd_hda2_alt_steering ? BUILD_SAFETY_CFG(hyundai_canfd_hda2_rx_checks, HYUNDAI_CANFD_HDA2_ALT_STEERING_TX_MSGS) : \
-                BUILD_SAFETY_CFG(hyundai_canfd_hda2_rx_checks, HYUNDAI_CANFD_HDA2_TX_MSGS);
+            ret = hyundai_canfd_hda2_alt_steering ? BUILD_SAFETY_CFG(hyundai_canfd_hda2_rx_checks, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS) : \
+                BUILD_SAFETY_CFG(hyundai_canfd_hda2_rx_checks, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS);
        }
    } else if (!hyundai_camera_scc) {
      static RxCheck hyundai_canfd_radar_scc_alt_buttons_rx_checks[] = {
--- a/selfdrive/carrot_settings.json
+++ b/selfdrive/carrot_settings.json
@ -222,32 +222,6 @@
      "default": 30,
      "unit": 1
    },
-    {
-      "group": "조향튜닝",
-      "name": "SteerSmoothSec",
-      "title": "조향필터링타임x0.01(13)",
-      "descr": "값이커지면 필터링을 더 많이 함.",
-      "egroup": "LAT",
-      "etitle": "SteerSmoothSecx0.01(13)",
-      "edescr": "",
-      "min": 1,
-      "max": 100,
-      "default": 13,
-      "unit": 1
-    },
-    {
-      "group": "조향튜닝",
-      "name": "SteerLagGain",
-      "title": "조향래그게인x0.01(200)",
-      "descr": "레인모드 시험용",
-      "egroup": "LAT",
-      "etitle": "SteerLagGainx0.01(200)",
-      "edescr": "",
-      "min": 1,
-      "max": 300,
-      "default": 200,
-      "unit": 10
-    },
    {
      "group": "크루즈",
      "name": "CruiseOnDist",
@ -1033,41 +1007,15 @@
      "group": "조향일반",
      "name": "AdjustLaneOffset",
      "title": "레인모드: 차선조절옵셋 (0)cm",
-      "descr": "도로경계쪽으로 주행합니다.",
+      "descr": "도로경계쪽/커브안쪽으로 보정합니다.",
      "egroup": "LAT",
      "etitle": "LaneLine: AdjustLaneOffset (0)cm",
-      "edescr": "",
+      "edescr": "to roadedge offset",
      "min": 0,
      "max": 500,
      "default": 0,
      "unit": 10
    },
-    {
-      "group": "조향일반",
-      "name": "AdjustCurveOffset",
-      "title": "레인모드: 커브조절옵셋 (0)cm",
-      "descr": "커브시 안쪽으로 돌도록합니다.",
-      "egroup": "LAT",
-      "etitle": "LaneLine: AdjustCurveOffset (0)cm",
-      "edescr": "",
-      "min": 0,
-      "max": 500,
-      "default": 0,
-      "unit": 10
-    },
-    {
-      "group": "조향일반",
-      "name": "AdjustLaneTime",
-      "title": "레인모드: 시간지연보상 (13)x0.01",
-      "descr": "레인제어기의 지연시간을 보상하여, 빠른 조향을 유도함",
-      "egroup": "LAT",
-      "etitle": "LaneLine: AdjustLaneTime (13)x0.01",
-      "edescr": "LaneLine controller delay compensation",
-      "min": 0,
-      "max": 50,
-      "default": 13,
-      "unit": 1
-    },
    {
      "group": "조향일반",
      "name": "LaneChangeNeedTorque",
@ -1111,12 +1059,12 @@
      "group": "시작",
      "name": "HyundaiCameraSCC",
      "title": "HYUNDAI: CAMERA SCC",
-      "descr": "SCC가 카메라쪽에 있거나, 배선 개조시 1,2로 설정, 2:Sync cruise state(canfd)",
+      "descr": "카메라SCC:1, 2:Sync cruise state(canfd), 3:순정크루즈",
      "egroup": "START",
      "etitle": "HYUNDAI: CAMERA SCC(0)",
-      "edescr": "SCC's CAN line connected to CAM, 2:Sync ruise state(canfd)",
+      "edescr": "SCC's CAN line connected to CAM, 2:Sync ruise state(canfd), 3:Stock Long",
      "min": 0,
-      "max": 2,
+      "max": 3,
      "default": 0,
      "unit": 1
    },
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@ -25,6 +25,7 @@ from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.common.realtime import DT_CTRL, DT_MDL
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS

 State = log.SelfdriveState.OpenpilotState
 LaneChangeState = log.LaneChangeState
@ -133,9 +134,9 @@ class Controls:
    curve_speed_abs = abs(self.sm['carrotMan'].vTurnSpeed)
    self.lanefull_mode_enabled = (lat_plan.useLaneLines and self.params.get_int("UseLaneLineSpeedApply") > 0 and
                                  curve_speed_abs > self.params.get_int("UseLaneLineCurveSpeed"))
-    lat_smooth_seconds = self.params.get_float("SteerSmoothSec") * 0.01
+    lat_smooth_seconds = LAT_SMOOTH_SECONDS #self.params.get_float("SteerSmoothSec") * 0.01
    steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01
-    lag_gain = self.params.get_float("SteerLagGain") * 0.01
+    mpc_delay_offset = 0.05
    if steer_actuator_delay == 0.0:
      steer_actuator_delay = self.sm['liveDelay'].lateralDelay 

@ -155,7 +156,7 @@ class Controls:
          alpha = 1 - np.exp(-DT_CTRL / tau) if tau > 0 else 1
          return alpha * val + (1 - alpha) * prev_val

-        curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay + lat_smooth_seconds, lat_plan.distances, lag_gain)
+        curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay + lat_smooth_seconds + mpc_delay_offset, lat_plan.distances)

        new_desired_curvature = smooth_value(curvature, self.desired_curvature, lat_smooth_seconds)
    else:
--- a/selfdrive/controls/lib/drive_helpers.py
+++ b/selfdrive/controls/lib/drive_helpers.py
@ -26,7 +26,7 @@ def apply_deadzone(error, deadzone):
    error = 0.
  return error

-def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, steer_actuator_delay, distances, lag_gain):
+def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, steer_actuator_delay, distances):
  if len(psis) != CONTROL_N:
    psis = [0.0]*CONTROL_N
    curvatures = [0.0]*CONTROL_N
@ -44,8 +44,7 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, steer_actuator_delay
  distance = max(np.interp(delay, ModelConstants.T_IDXS[:CONTROL_N], distances), 0.001)
  #average_curvature_desired = psi / (v_ego * delay)
  average_curvature_desired = psi / distance
-  #desired_curvature = 2 * average_curvature_desired - current_curvature_desired
-  desired_curvature = lag_gain * average_curvature_desired - current_curvature_desired
+  desired_curvature = 2 * average_curvature_desired - current_curvature_desired

  # This is the "desired rate of the setpoint" not an actual desired rate
  max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
--- a/selfdrive/controls/lib/lane_planner_2.py
+++ b/selfdrive/controls/lib/lane_planner_2.py
@ -158,7 +158,7 @@ class LanePlanner:
      #self.lane_width_right_filtered.x = self.lane_width_right #바로적용

    self.adjustLaneOffset = float(self.params.get_int("AdjustLaneOffset")) * 0.01
-    self.adjustCurveOffset = float(self.params.get_int("AdjustCurveOffset")) * 0.01
+    self.adjustCurveOffset = self.adjustLaneOffset #float(self.params.get_int("AdjustCurveOffset")) * 0.01
    ADJUST_OFFSET_LIMIT = 0.4 #max(self.adjustLaneOffset, self.adjustCurveOffset)
    offset_curve = 0.0
    ## curve offset
@ -229,7 +229,7 @@ class LanePlanner:
    #  self.d_prob, self.lanefull_mode,
    #  self.lane_width_left_filtered.x, self.lane_width, self.lane_width_right_filtered.x)

-    adjustLaneTime = self.params.get_int("AdjustLaneTime")
+    adjustLaneTime = 0.05 #self.params.get_int("AdjustLaneTime")
    laneline_active = False
    if self.lanefull_mode and self.d_prob > 0.3:
      laneline_active = True
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@ -45,8 +45,8 @@ class Track:
    self.jLead = j_lead
    
    self.measured = measured   # measured or estimate
-
-    if abs(self.aLead) < 0.5 and abs(j_lead) < 0.5:
+    a_lead_threshold = 0.5 * self.radar_reaction_factor
+    if abs(self.aLead) < a_lead_threshold and abs(j_lead) < 0.5:
      self.aLeadTau.x = _LEAD_ACCEL_TAU * self.radar_reaction_factor
    else:
      self.aLeadTau.update(0.0)
@ -299,7 +299,7 @@ class VisionTrack:
      self.yRel = float(-lead_msg.y[0])
      dPath = self.yRel + np.interp(self.dRel, md.position.x, md.position.y)
      a_lead_vision = lead_msg.a[0]
-      if self.cnt < 2 or self.prob < 0.99:
+      if self.cnt < 2 or self.prob < 0.97:
        self.vRel = lead_v_rel_pred
        self.vLead = float(v_ego + lead_v_rel_pred)
        self.aLead = a_lead_vision
@ -309,7 +309,9 @@ class VisionTrack:
        v_rel = self.vRel * (1. - self.alpha) + v_rel * self.alpha

        #self.vRel = lead_v_rel_pred if self.mixRadarInfo == 3 else (lead_v_rel_pred + self.vRel) / 2
-        self.vRel = (lead_v_rel_pred + v_rel) / 2
+        model_weight = np.interp(self.prob, [0.97, 1.0], [0.4, 0.0])  # prob가 높으면 v_rel(dRel미분값)에 가중치를 줌.
+        self.vRel = float(lead_v_rel_pred * model_weight + v_rel * (1. - model_weight))
+        #self.vRel = (lead_v_rel_pred + v_rel) / 2
        self.vLead = float(v_ego + self.vRel)

        a_lead = (self.vLead - self.vLead_last) / self.radar_ts * 0.2 #0.5 -> 0.2 vel 미분적용을 줄임.
--- a/selfdrive/modeld/modeld.py
+++ b/selfdrive/modeld/modeld.py
@ -258,7 +258,7 @@ def main(demo=False):
    frame += 1
    if frame % 100 == 0:
      custom_lat_delay = params.get_float("SteerActuatorDelay") * 0.01
-      lat_smooth_seconds = params.get_float("SteerSmoothSec") * 0.01
+      #lat_smooth_seconds = params.get_float("SteerSmoothSec") * 0.01
      
    if custom_lat_delay > 0.0:
      lat_delay = custom_lat_delay + lat_smooth_seconds
--- a/selfdrive/ui/qt/offroad/settings.cc
+++ b/selfdrive/ui/qt/offroad/settings.cc
@ -676,8 +676,6 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
  latLongToggles->addItem(new CValueControl("UseLaneLineSpeed", "Laneline mode speed(0)", "Laneline mode, lat_mpc control used", "../assets/offroad/icon_logic.png", 0, 200, 5));
  latLongToggles->addItem(new CValueControl("UseLaneLineCurveSpeed", "Laneline mode curve speed(0)", "Laneline mode, high speed only", "../assets/offroad/icon_logic.png", 0, 200, 5));
  latLongToggles->addItem(new CValueControl("AdjustLaneOffset", "AdjustLaneOffset(0)cm", "", "../assets/offroad/icon_logic.png", 0, 500, 5));
-  latLongToggles->addItem(new CValueControl("AdjustCurveOffset", "AdjustLaneCurveOffset(0)cm", "", "../assets/offroad/icon_logic.png", 0, 500, 5));
-  latLongToggles->addItem(new CValueControl("AdjustLaneTime", "AdjustLaneTimeOffset(5)x0.01s", "", "../assets/offroad/icon_logic.png", 0, 20, 1));
  latLongToggles->addItem(new CValueControl("CustomSR", "LAT: SteerRatiox0.1(0)", "Custom SteerRatio", "../assets/offroad/icon_logic.png", 0, 300, 1));
  latLongToggles->addItem(new CValueControl("SteerRatioRate", "LAT: SteerRatioRatex0.01(100)", "SteerRatio apply rate", "../assets/offroad/icon_logic.png", 30, 170, 1));
  latLongToggles->addItem(new CValueControl("PathOffset", "LAT: PathOffset", "(-)left, (+)right", "../assets/offroad/icon_logic.png", -150, 150, 1));
@ -706,7 +704,6 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
  //latLongToggles->addItem(new CValueControl("CruiseMinVals", "DECEL:(120)", "감속도를 설정합니다.(x0.01m/s^2)", "../assets/offroad/icon_road.png", 50, 250, 5));
  latLongToggles->addItem(new CValueControl("MaxAngleFrames", "MaxAngleFrames(89)", "89:기본, 스티어계기판에러시 85~87", "../assets/offroad/icon_logic.png", 80, 100, 1));
  latLongToggles->addItem(new CValueControl("SteerActuatorDelay", "LAT:SteerActuatorDelay(30)", "x0.01, 0:LiveDelay", "../assets/offroad/icon_logic.png", 0, 100, 1));
-  latLongToggles->addItem(new CValueControl("SteerSmoothSec", "LAT:SteerSmoothSec(13)", "x0.01", "../assets/offroad/icon_logic.png", 1, 100, 1));
  latLongToggles->addItem(new CValueControl("LateralTorqueCustom", "LAT: TorqueCustom(0)", "", "../assets/offroad/icon_logic.png", 0, 2, 1));
  latLongToggles->addItem(new CValueControl("LateralTorqueAccelFactor", "LAT: TorqueAccelFactor(2500)", "", "../assets/offroad/icon_logic.png", 1000, 6000, 10));
  latLongToggles->addItem(new CValueControl("LateralTorqueFriction", "LAT: TorqueFriction(100)", "", "../assets/offroad/icon_logic.png", 0, 1000, 10));
@ -799,7 +796,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
  });

  startToggles->addItem(selectCarBtn);
-  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("HyundaiCameraSCC", "HYUNDAI: CAMERA SCC", "1:Connect the SCC's CAN line to CAM, 2:Sync Cruise state, 3:StockLong", "../assets/offroad/icon_shell.png", 0, 3, 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));
--- a/system/hardware/tici/agnos.json
+++ b/system/hardware/tici/agnos.json
@ -56,29 +56,29 @@
  },
  {
    "name": "boot",
-    "url": "https://commadist.azureedge.net/agnosupdate/boot-9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892.img.xz",
-    "hash": "9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892",
-    "hash_raw": "9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892",
+    "url": "https://commadist.azureedge.net/agnosupdate/boot-3d8e848796924081f5a6b3d745808b1117ae2ec41c03f2d41ee2e75633bd6425.img.xz",
+    "hash": "3d8e848796924081f5a6b3d745808b1117ae2ec41c03f2d41ee2e75633bd6425",
+    "hash_raw": "3d8e848796924081f5a6b3d745808b1117ae2ec41c03f2d41ee2e75633bd6425",
    "size": 18479104,
    "sparse": false,
    "full_check": true,
    "has_ab": true,
-    "ondevice_hash": "41d31b862fec1b87879b508c405adb9d7b5c0a3324f7350bd904f451605b06cf"
+    "ondevice_hash": "2075104847d1c96a06f07e85efb9f48d0e792d75a059047eae7ba4b463ffeadf"
  },
  {
    "name": "system",
-    "url": "https://commadist.azureedge.net/agnosupdate/system-02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde.img.xz",
-    "hash": "c56256a64e6d7e16886e39a4263ffb686ed0f03d3a665c3552f54a39723f8824",
-    "hash_raw": "02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde",
-    "size": 4404019200,
+    "url": "https://commadist.azureedge.net/agnosupdate/system-b22bd239c6f9527596fd49b98b7d521a563f99b90953ce021ee36567498e99c7.img.xz",
+    "hash": "cb9bfde1e995b97f728f5d5ad8d7a0f7a01544db5d138ead9b2350f222640939",
+    "hash_raw": "b22bd239c6f9527596fd49b98b7d521a563f99b90953ce021ee36567498e99c7",
+    "size": 5368709120,
    "sparse": true,
    "full_check": false,
    "has_ab": true,
-    "ondevice_hash": "ed2e11f52beb8559223bf9fb989fd4ef5d2ce66eeb11ae0053fff8e41903a533",
+    "ondevice_hash": "e92a1f34158c60364c8d47b8ebbb6e59edf8d4865cd5edfeb2355d6f54f617fc",
    "alt": {
-      "hash": "02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde",
-      "url": "https://commadist.azureedge.net/agnosupdate/system-02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde.img",
-      "size": 4404019200
+      "hash": "b22bd239c6f9527596fd49b98b7d521a563f99b90953ce021ee36567498e99c7",
+      "url": "https://commadist.azureedge.net/agnosupdate/system-b22bd239c6f9527596fd49b98b7d521a563f99b90953ce021ee36567498e99c7.img",
+      "size": 5368709120
    }
  }
 ]
--- a/system/hardware/tici/all-partitions.json
+++ b/system/hardware/tici/all-partitions.json
@ -339,62 +339,62 @@
  },
  {
    "name": "boot",
-    "url": "https://commadist.azureedge.net/agnosupdate/boot-9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892.img.xz",
-    "hash": "9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892",
-    "hash_raw": "9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892",
+    "url": "https://commadist.azureedge.net/agnosupdate/boot-3d8e848796924081f5a6b3d745808b1117ae2ec41c03f2d41ee2e75633bd6425.img.xz",
+    "hash": "3d8e848796924081f5a6b3d745808b1117ae2ec41c03f2d41ee2e75633bd6425",
+    "hash_raw": "3d8e848796924081f5a6b3d745808b1117ae2ec41c03f2d41ee2e75633bd6425",
    "size": 18479104,
    "sparse": false,
    "full_check": true,
    "has_ab": true,
-    "ondevice_hash": "41d31b862fec1b87879b508c405adb9d7b5c0a3324f7350bd904f451605b06cf"
+    "ondevice_hash": "2075104847d1c96a06f07e85efb9f48d0e792d75a059047eae7ba4b463ffeadf"
  },
  {
    "name": "system",
-    "url": "https://commadist.azureedge.net/agnosupdate/system-02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde.img.xz",
-    "hash": "c56256a64e6d7e16886e39a4263ffb686ed0f03d3a665c3552f54a39723f8824",
-    "hash_raw": "02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde",
-    "size": 4404019200,
+    "url": "https://commadist.azureedge.net/agnosupdate/system-b22bd239c6f9527596fd49b98b7d521a563f99b90953ce021ee36567498e99c7.img.xz",
+    "hash": "cb9bfde1e995b97f728f5d5ad8d7a0f7a01544db5d138ead9b2350f222640939",
+    "hash_raw": "b22bd239c6f9527596fd49b98b7d521a563f99b90953ce021ee36567498e99c7",
+    "size": 5368709120,
    "sparse": true,
    "full_check": false,
    "has_ab": true,
-    "ondevice_hash": "ed2e11f52beb8559223bf9fb989fd4ef5d2ce66eeb11ae0053fff8e41903a533",
+    "ondevice_hash": "e92a1f34158c60364c8d47b8ebbb6e59edf8d4865cd5edfeb2355d6f54f617fc",
    "alt": {
-      "hash": "02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde",
-      "url": "https://commadist.azureedge.net/agnosupdate/system-02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde.img",
-      "size": 4404019200
+      "hash": "b22bd239c6f9527596fd49b98b7d521a563f99b90953ce021ee36567498e99c7",
+      "url": "https://commadist.azureedge.net/agnosupdate/system-b22bd239c6f9527596fd49b98b7d521a563f99b90953ce021ee36567498e99c7.img",
+      "size": 5368709120
    }
  },
  {
    "name": "userdata_90",
-    "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-175a5d3353daa5e7b7d9939cb51a2f1d7e6312b4708ad654c351da2f1ef4f108.img.xz",
-    "hash": "ff01a0ca5a2ea6661f836248043a211cd8d71c3269c139cb574b56855fabc3f4",
-    "hash_raw": "175a5d3353daa5e7b7d9939cb51a2f1d7e6312b4708ad654c351da2f1ef4f108",
+    "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-4bb7239f7e82c846e4d2584c0c433f03c582a80950de4094e6c190563d6d84ac.img.xz",
+    "hash": "b18001a2a87caa070fabf6321f8215ac353d6444564e3f86329b4dccc039ce54",
+    "hash_raw": "4bb7239f7e82c846e4d2584c0c433f03c582a80950de4094e6c190563d6d84ac",
    "size": 96636764160,
    "sparse": true,
    "full_check": true,
    "has_ab": false,
-    "ondevice_hash": "2f3d69e5015a45a18c3553f2edc5706aacd6d84a4b3d5010a3d76a1a3aa910b0"
+    "ondevice_hash": "15ce16f2349d5b4d5fec6ad1e36222b1ae744ed10b8930bc9af75bd244dccb3c"
  },
  {
    "name": "userdata_89",
-    "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-61bdaf82d3036af6e45e86adbaab02918b41debd5b58b6708d7987084d514d1b.img.xz",
-    "hash": "714970777e02bb53a71640735bdb84b3071ecbc0346b978ce12eb667d75634ec",
-    "hash_raw": "61bdaf82d3036af6e45e86adbaab02918b41debd5b58b6708d7987084d514d1b",
+    "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-e36b59bf9ff755b6ca488df2ba1e20da8f7dab6b8843129f3fdcccd7ff2ff7d8.img.xz",
+    "hash": "12682cf54596ab1bd1c2464c4ca85888e4e06b47af5ff7d0432399e9907e2f64",
+    "hash_raw": "e36b59bf9ff755b6ca488df2ba1e20da8f7dab6b8843129f3fdcccd7ff2ff7d8",
    "size": 95563022336,
    "sparse": true,
    "full_check": true,
    "has_ab": false,
-    "ondevice_hash": "95e6889a808b8d266660990e67e917cf3b63179f23588565af7f2fa54f70ac76"
+    "ondevice_hash": "e4df9dea47ff04967d971263d50c17460ef240457e8d814e7c4f409f7493eb8a"
  },
  {
    "name": "userdata_30",
-    "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-a40553d3fd339cb0107f1cc55fd532820f192a7a9a90d05243ad00fcbf804997.img.xz",
-    "hash": "33e5ab398620f147b885a9627b2608591bd9e1c9aa481eb705dc86707d706ea2",
-    "hash_raw": "a40553d3fd339cb0107f1cc55fd532820f192a7a9a90d05243ad00fcbf804997",
+    "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-fe1d86f5322c675c58b3ae9753a4670abf44a25746bf6ac822aed108bb577282.img.xz",
+    "hash": "fa471703be0f0647617d183312d5209d23407f1628e4ab0934e6ec54b1a6b263",
+    "hash_raw": "fe1d86f5322c675c58b3ae9753a4670abf44a25746bf6ac822aed108bb577282",
    "size": 32212254720,
    "sparse": true,
    "full_check": true,
    "has_ab": false,
-    "ondevice_hash": "cd6291dea40968123f7af0b831cbfbbd6e515b676f2e427ae47ff358f6ac148e"
+    "ondevice_hash": "0b5b2402c9caa1ed7b832818e66580c974251e735bda91f2f226c41499d5616e"
  }
 ]
--- a/system/hardware/tici/hardware.py
+++ b/system/hardware/tici/hardware.py
@ -297,13 +297,11 @@ class Tici(HardwareBase):
      return None

  def get_modem_temperatures(self):
-    if self.get_device_type() == "mici":
-      return []
    timeout = 0.2  # Default timeout is too short
    try:
      modem = self.get_modem()
      temps = modem.Command("AT+QTEMP", math.ceil(timeout), dbus_interface=MM_MODEM, timeout=timeout)
-      return list(map(int, temps.split(' ')[1].split(',')))
+      return list(filter(lambda t: t != 255, map(int, temps.split(' ')[1].split(','))))
    except Exception:
      return []

@ -335,6 +333,7 @@ class Tici(HardwareBase):
    return ThermalConfig(cpu=[ThermalZone(f"cpu{i}-silver-usr") for i in range(4)] +
                             [ThermalZone(f"cpu{i}-gold-usr") for i in range(4)],
                         gpu=[ThermalZone("gpu0-usr"), ThermalZone("gpu1-usr")],
+                         dsp=ThermalZone("compute-hvx-usr"),
                         memory=ThermalZone("ddr-usr"),
                         pmic=[ThermalZone("pm8998_tz"), ThermalZone("pm8005_tz")],
                         intake=intake,
--- a/system/manager/manager.py
+++ b/system/manager/manager.py
@ -124,8 +124,6 @@ def get_default_params():
    ("UseLaneLineCurveSpeed", "0"),
    ("UseLaneLineSpeedApply", "0"),
    ("AdjustLaneOffset", "0"),
-    ("AdjustCurveOffset", "0"),
-    ("AdjustLaneTime", "13"),
    ("LaneChangeNeedTorque", "0"),
    ("MaxAngleFrames", "89"),
    ("LateralTorqueCustom", "0"),
@ -145,8 +143,6 @@ def get_default_params():
    ("CustomSteerDeltaDown", "0"),
    ("SpeedFromPCM", "2"),
    ("SteerActuatorDelay", "0"),
-    ("SteerSmoothSec", "13"),
-    ("SteerLagGain", "200"),
    ("MaxTimeOffroadMin", "60"),
    ("DisableDM", "0"),
    ("RecordRoadCam", "0"),