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>
This commit is contained in:
carrot
GitHub
parent
5e13f23049
commit
36f281ac9f
@ -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,
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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 ',
|
||||
|
||||
@ -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
|
||||
pass
|
||||
values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1
|
||||
elif LFA_trigger > 0:
|
||||
pass
|
||||
#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))
|
||||
values["LFA_BTN"] = 1
|
||||
ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values))
|
||||
"""
|
||||
|
||||
|
||||
if frame % 5 == 0:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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"
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
@ -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하게.
|
||||
|
||||
@ -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": "차량간격",
|
||||
|
||||
@ -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 = np.interp(steer_actuator_delay + lat_smooth_seconds + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], lat_plan.curvatures)
|
||||
curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay + lat_smooth_seconds)
|
||||
|
||||
new_desired_curvature = smooth_value(curvature, self.desired_curvature, lat_smooth_seconds)
|
||||
else:
|
||||
new_desired_curvature = model_v2.action.desiredCurvature
|
||||
|
||||
@ -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))
|
||||
|
||||
@ -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));
|
||||
|
||||
@ -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")
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user