From a87f8b33768911cbac30d953822a67c1395f0f76 Mon Sep 17 00:00:00 2001 From: carrot <43668841+ajouatom@users.noreply.github.com> Date: Sun, 18 May 2025 12:44:49 +0900 Subject: [PATCH] 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.. --- common/params_keys.h | 4 -- launch_env.sh | 2 +- .../opendbc/car/hyundai/carcontroller.py | 12 +++- opendbc_repo/opendbc/car/hyundai/carstate.py | 13 ++-- .../opendbc/car/hyundai/hyundaicanfd.py | 17 +++++ opendbc_repo/opendbc/car/hyundai/interface.py | 2 +- opendbc_repo/opendbc/dbc/hyundai_canfd.dbc | 1 + .../safety/safety/safety_hyundai_canfd.h | 46 ++++++++++---- selfdrive/carrot_settings.json | 62 ++----------------- selfdrive/controls/controlsd.py | 7 ++- selfdrive/controls/lib/drive_helpers.py | 5 +- selfdrive/controls/lib/lane_planner_2.py | 4 +- selfdrive/controls/radard.py | 10 +-- selfdrive/modeld/modeld.py | 2 +- selfdrive/ui/qt/offroad/settings.cc | 5 +- system/hardware/tici/agnos.json | 24 +++---- system/hardware/tici/all-partitions.json | 50 +++++++-------- system/hardware/tici/hardware.py | 5 +- system/manager/manager.py | 4 -- 19 files changed, 132 insertions(+), 143 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index 8fea6f7..a241636 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -195,8 +195,6 @@ inline static std::unordered_map 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 keys = { {"UseLaneLineCurveSpeed", PERSISTENT}, {"UseLaneLineSpeedApply", PERSISTENT}, {"AdjustLaneOffset", PERSISTENT}, - {"AdjustCurveOffset", PERSISTENT}, - {"AdjustLaneTime", PERSISTENT}, {"LaneChangeNeedTorque", PERSISTENT}, {"MaxAngleFrames", PERSISTENT}, {"SoftHoldMode", PERSISTENT}, diff --git a/launch_env.sh b/launch_env.sh index aff451e..73a7a89 100755 --- 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" diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 4fcac18..445e219 100644 --- 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: diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index ff93abc..e8bb17d 100644 --- 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"]) diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index c7da3d8..135caaf 100644 --- 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 = [] diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index f45908e..5a83fa9 100644 --- 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}") diff --git a/opendbc_repo/opendbc/dbc/hyundai_canfd.dbc b/opendbc_repo/opendbc/dbc/hyundai_canfd.dbc index 1cd5c42..7af3431 100644 --- 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 diff --git a/opendbc_repo/opendbc/safety/safety/safety_hyundai_canfd.h b/opendbc_repo/opendbc/safety/safety/safety_hyundai_canfd.h index abc52c2..8abae81 100644 --- 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[] = { diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index 69aea47..be14778 100644 --- 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 }, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index bcf14d8..509371f 100644 --- 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: diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 1a0b352..a6cd667 100644 --- 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 diff --git a/selfdrive/controls/lib/lane_planner_2.py b/selfdrive/controls/lib/lane_planner_2.py index ec93907..ae6d0e3 100644 --- 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 diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index ed4fb1a..f73a485 100644 --- 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 미분적용을 줄임. diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index ae31219..30ba697 100755 --- 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 diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index aa59987..58bc7ca 100644 --- 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)); diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index 1a3cd38..a415851 100644 --- 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 } } ] \ No newline at end of file diff --git a/system/hardware/tici/all-partitions.json b/system/hardware/tici/all-partitions.json index 4e24b22..be303b7 100644 --- 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" } -] +] \ No newline at end of file diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index 15480ba..b2d00a4 100644 --- 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, diff --git a/system/manager/manager.py b/system/manager/manager.py index 4229357..772f019 100755 --- 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"),