mawei/carrotpilot
1
0
Fork 0
mirror of https://github.com/ajouatom/openpilot.git synced 2026-02-18 13:03:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

casper, k5, stopped car

Browse Source
This commit is contained in:
ajouatom
2025-08-04 12:57:06 +09:00
parent 1bc2b23ec1
commit a345912302
10 changed files with 88 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
common/params_keys.h
View File

@@ -197,6 +197,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
{"CruiseButtonTest2", PERSISTENT},
{"CruiseButtonTest3", PERSISTENT},
{"CruiseSpeedUnit", PERSISTENT},
{"CruiseSpeedUnitBasic", PERSISTENT},
{"CruiseSpeed1", PERSISTENT},
{"CruiseSpeed2", PERSISTENT},
{"CruiseSpeed3", PERSISTENT},

4
opendbc_repo/opendbc/car/hyundai/carcontroller.py
View File

@@ -286,8 +286,8 @@ class CarController(CarControllerBase):
can_sends.extend(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS))
# LFA and HDA icons
if self.frame % 5 == 0 and (not hda2 or hda2_long):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, CS, self.CAN, CC.longActive, CC.latActive))
if self.frame % 5 == 0 and camera_scc:
can_sends.extend(hyundaicanfd.create_lfahda_cluster(self.packer, CS, self.CAN, CC.longActive, CC.latActive))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:

34
opendbc_repo/opendbc/car/hyundai/carstate.py
View File

@@ -62,7 +62,7 @@ class CarState(CarStateBase):
self.cruise_info = {}
self.lfa_info = {}
self.lfa_alt_info = {}
self.lfahda_cluster_info = {}
self.lfahda_cluster_info = None
self.adrv_info_161 = None
self.adrv_info_200 = None
self.adrv_info_1ea = None
@@ -107,11 +107,19 @@ class CarState(CarStateBase):
fingerprints_str = Params().get("FingerPrints", encoding='utf-8')
fingerprints = ast.literal_eval(fingerprints_str)
#print("fingerprints =", fingerprints)
bus_cruise = 2 if self.CP.flags & HyundaiFlags.CAMERA_SCC else 0
self.SCC11 = True if 1056 in fingerprints[bus_cruise] else False
self.SCC12 = True if 1057 in fingerprints[bus_cruise] else False
self.SCC13 = True if 1290 in fingerprints[bus_cruise] else False
self.SCC14 = True if 905 in fingerprints[bus_cruise] else False
ecu_disabled = False
if self.CP.openpilotLongitudinalControl and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC):
ecu_disabled = True
if ecu_disabled:
self.SCC11 = self.SCC12 = self.SCC13 = self.SCC14 = False
else:
bus_cruise = 2 if self.CP.flags & HyundaiFlags.CAMERA_SCC else 0
self.SCC11 = True if 1056 in fingerprints[bus_cruise] else False
self.SCC12 = True if 1057 in fingerprints[bus_cruise] else False
self.SCC13 = True if 1290 in fingerprints[bus_cruise] else False
self.SCC14 = True if 905 in fingerprints[bus_cruise] else False
self.HAS_LFA_BUTTON = True if 913 in fingerprints[0] else False
self.CRUISE_BUTTON_ALT = True if 1007 in fingerprints[0] else False
@@ -123,6 +131,7 @@ class CarState(CarStateBase):
self.ADRV_0x200 = True if 0x200 in fingerprints[cam_bus] else False
self.ADRV_0x1ea = True if 0x1ea in fingerprints[cam_bus] else False
self.ADRV_0x160 = True if 0x160 in fingerprints[cam_bus] else False
self.LFAHDA_CLUSTER = True if 480 in fingerprints[cam_bus] else False
self.HDA_INFO_4A3 = True if 0x4a3 in fingerprints[pt_bus] else False
self.NEW_MSG_4B4 = True if 0x4b4 in fingerprints[pt_bus] else False
self.GEAR = True if 69 in fingerprints[pt_bus] else False
@@ -296,14 +305,14 @@ class CarState(CarStateBase):
prev_cruise_buttons = self.cruise_buttons[-1]
#self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
#carrot {{
if (self.CRUISE_BUTTON_ALT and cp.vl["CRUISE_BUTTON_ALT"]["SET_ME_1"] == 1):
self.cruise_buttons_alt = True
#if self.CRUISE_BUTTON_ALT and cp.vl["CRUISE_BUTTON_ALT"]["SET_ME_1"] == 1:
# self.cruise_buttons_alt = True
cruise_button = [Buttons.NONE]
if self.cruise_buttons_alt:
lfa_button = cp.vl.get("CRUISE_BUTTON_LFA", {}).get("CruiseSwLfa", 0)
lfa_button = cp.vl["CRUISE_BUTTON_LFA"]["CruiseSwLfa"]
cruise_button = [Buttons.LFA_BUTTON] if lfa_button > 0 else cp.vl_all["CRUISE_BUTTON_ALT"]["CruiseSwState"]
elif self.HAS_LFA_BUTTON and cp.vl.get("BCM_PO_11", {}).get("LFA_Pressed", 0):
elif self.HAS_LFA_BUTTON and cp.vl["BCM_PO_11"]["LFA_Pressed"] == 1: # for K5
cruise_button = [Buttons.LFA_BUTTON]
else:
cruise_button = cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]
@@ -477,7 +486,10 @@ class CarState(CarStateBase):
self.lfa_info = copy.copy(cp_cam.vl["LFA"])
if self.CP.flags & HyundaiFlags.ANGLE_CONTROL.value:
self.lfa_alt_info = copy.copy(cp_cam.vl["LFA_ALT"])
self.lfahda_cluster_info = copy.copy(cp_cam.vl["LFAHDA_CLUSTER"])
if self.LFAHDA_CLUSTER:
self.lfahda_cluster_info = cp_cam.vl["LFAHDA_CLUSTER"]
corner = False
self.adrv_info_161 = cp_cam.vl["ADRV_0x161"] if self.CCNC_0x161 else None
self.adrv_info_162 = cp_cam.vl["CCNC_0x162"] if self.CCNC_0x162 else None

11
opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
View File

@@ -268,10 +268,13 @@ def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_lfahda_cluster(packer, CS, CAN, long_active, lat_active):
values = {} #CS.lfahda_cluster_info
values["HDA_ICON"] = 1 if long_active else 0
values["LFA_ICON"] = 2 if lat_active else 0
return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)
if CS.lfahda_cluster_info is not None:
values = {} #
values["HDA_ICON"] = 1 if long_active else 0
values["LFA_ICON"] = 2 if lat_active else 0
else:
return []
return [packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)]
def create_acc_control_scc2(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, hyundai_jerk, CS):

12
opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc
View File

@@ -495,13 +495,13 @@ BO_ 474 ADRV_0x1da: 32 ADRV
BO_ 480 LFAHDA_CLUSTER: 16 ADRV
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
SG_ HDA_ICON : 31|1@1+ (1,0) [0|1] "" XXX
SG_ OptUsmState : 26|3@0+ (1,0) [0|7] "" XXX
SG_ HDA_ICON : 30|2@1+ (1,0) [0|3] "" XXX
SG_ LFA_State : 34|3@0+ (1,0) [0|7] "" XXX
SG_ LFA_ICON : 47|2@1+ (1,0) [0|3] "" XXX
SG_ NEW_SIGNAL_1 : 32|3@1+ (1,0) [0|7] "" XXX
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
SG_ LFA_WrnSnd : 49|2@1+ (1,0) [0|3] "" XXX
SG_ HDA_InfoPopup : 51|3@1+ (1,0) [0|7] "" XXX
SG_ LFA_Deactive : 54|2@1+ (1,0) [0|3] "" XXX
BO_ 490 ADRV_0x1ea: 32 ADRV
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX

8
selfdrive/car/cruise.py
View File

@@ -168,6 +168,7 @@ class VCruiseCarrot:
self.v_ego_kph_set = 0
self._cruise_speed_min, self._cruise_speed_max = 5, 161
self._cruise_speed_unit = 10
self._cruise_speed_unit_basic = 1
self._cruise_button_mode = 2
self._lfa_button_mode = 0
@@ -253,6 +254,7 @@ class VCruiseCarrot:
self.speed_from_pcm = self.params.get_int("SpeedFromPCM")
self._cruise_speed_unit = self.params.get_int("CruiseSpeedUnit")
self._cruise_speed_unit_basic = self.params.get_int("CruiseSpeedUnitBasic")
self._paddle_mode = self.params.get_int("PaddleMode")
self._cruise_button_mode = self.params.get_int("CruiseButtonMode")
self._lfa_button_mode = self.params.get_int("LfaButtonMode")
@@ -371,8 +373,8 @@ class VCruiseCarrot:
button_type = 0
buttonEvents = CS.buttonEvents
SPEED_UP_UNIT = self._cruise_speed_unit
SPEED_DOWN_UNIT = self._cruise_speed_unit # if self._cruise_button_mode in [1, 2, 3] else 1
SPEED_UP_UNIT = self._cruise_speed_unit_basic
SPEED_DOWN_UNIT = self._cruise_speed_unit if self._cruise_button_mode in [1, 2, 3] else self._cruise_speed_unit_basic
V_CRUISE_DELTA = 10
is_metric = self.is_metric
@@ -600,7 +602,7 @@ class VCruiseCarrot:
elif v_cruise_kph < 30: #self.nRoadLimitSpeed:
v_cruise_kph = 30 #self.nRoadLimitSpeed
else:
for speed in range (40, 160, max(self._cruise_speed_unit, 10)):
for speed in range (40, 160, self._cruise_speed_unit):
if v_cruise_kph < speed:
v_cruise_kph = speed
break

27
selfdrive/carrot_settings.json
View File

@@ -693,11 +693,24 @@
{
"group": "버튼설정",
"name": "CruiseSpeedUnit",
"title": "크루즈버튼속도단위(10)",
"descr": "크루즈버튼 속도조절 단위",
"title": "크루즈버튼속도단위 확장(10)",
"descr": "크루즈버튼 속도조절 단위(모드1,2,3, 엑셀톡등)",
"egroup": "BUTN",
"etitle": "Cruise Speed Unit(10)",
"edescr": "SET/DECEL up/down speed unit",
"etitle": "Cruise Speed Unit Extra(10)",
"edescr": "SET/DECEL up/down speed unit(mode1,2,3, GasTok)",
"min": 1,
"max": 100,
"default": 10,
"unit": 1
},
{
"group": "버튼설정",
"name": "CruiseSpeedUnitBasic",
"title": "크루즈버튼속도단위 기본(1)",
"descr": "크루즈버튼 속도조절 단위(기본단위)",
"egroup": "BUTN",
"etitle": "Cruise Speed Unit Baic(10)",
"edescr": "SET/DECEL up/down speed unit(Basic unit)",
"min": 1,
"max": 100,
"default": 10,
@@ -1306,10 +1319,10 @@
"group": "시작",
"name": "HyundaiCameraSCC",
"title": "HYUNDAI: CAMERA SCC",
"descr": "1:롱컨차량\n 2:크루즈 상태 동기화(canfd 차량)\n 3:순정크루즈(hda2 롱컨차량)",
"descr": "1:롱컨차량\n 2:크루즈 상태 동기화\n 3:순정크루즈(CANFD롱컨배선)",
"egroup": "START",
"etitle": "HYUNDAI: CAMERA SCC(0)",
"edescr": "1: Long-con Vehicle, 2: Cruise State Sync (CAN-FD Vehicle), 3: Stock Cruise (HDA2 Longitudinal Control Vehicle)",
"edescr": "1: Long-con Vehicle, 2: Cruise State Sync (CAN-FD Vehicle), 3: Stock Cruise (CANFD Long harness)",
"min": 0,
"max": 3,
"default": 0,
@@ -1684,7 +1697,7 @@
"name": "TurnSpeedControlMode",
"title": "턴속도제어방법",
"descr": "0: 속도제어안함\n 1: 비젼\n 2: 비젼+경로(TBT)\n 3: 경로(항상)",
"egroup": "SPEED",
"egroup": "SPEED",
"etitle": "TurnSpeedControlMode",
"edescr": "0:not use, 1:vision, 2:vision+route, 3:route(always)",
"min": 0,

26
selfdrive/controls/radard.py
View File

@@ -33,6 +33,8 @@ class Track:
self.aLeadTau = FirstOrderFilter(_LEAD_ACCEL_TAU, 0.45, DT_MDL)
self.radar_reaction_factor = Params().get_float("RadarReactionFactor") * 0.01
self.is_stopped_car_count = 0
self.selected_count = 0
def update(self, md, pt, ready):
@@ -103,12 +105,11 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks
max_offset_vision_vel = max(lead.v[0] * np.interp(lead.prob, [0.8, 0.98], [0.3, 0.5]), 5.0) # 확률이 낮으면 속도오차를 줄임.
def prob(c):
#if abs(c.dRel - offset_vision_dist) > max_offset_vision_dist:
if abs(offset_vision_dist - c.dRel) > max_offset_vision_dist: # vision 측정한것보다 레이더 거리나 너무 낮으면 버림
if abs(offset_vision_dist - c.dRel) > max_offset_vision_dist:
return -1e6
if lead.prob < 0.5 or abs(lead.v[0] - c.vLead) > max_offset_vision_vel: # or c.vLead < 3:
return -1e6
#if abs(lead.v[0] - c.vLead) > max_offset_vision_vel:
# return -1e6
prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0])
prob_y = laplacian_pdf(c.yRel + c.yvLead * radar_lat_factor, -lead.y[0], lead.yStd[0])
@@ -127,7 +128,22 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks
if score > best_score:
best_score = score
best_track = c
return best_track if best_score > -1e6 else None
if best_track is not None:
if abs(lead.v[0] - best_track.vLead) > max_offset_vision_vel:
best_track.is_stopped_car_count += 1
# 직전에 사용되었던것이라면 재사용, 3초간 유지된다면 정지차로 간주.
if best_track.selected_count < 1 and best_track.is_stopped_car_count < int(3.0/DT_MDL):
best_track = None
if best_track is not None:
best_track.selected_count += 1
for c in tracks.values():
if c is not best_track:
c.selected_count = 0
return best_track
def get_RadarState_from_vision(md, lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float):
lead_v_rel_pred = lead_msg.v[0] - model_v_ego

3
selfdrive/ui/qt/offroad/settings.cc
View File

@@ -661,7 +661,8 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
cruiseToggles = new ListWidget(this);
cruiseToggles->addItem(new CValueControl("CruiseButtonMode", "Button: Cruise Button Mode", "0:Normal,1:User1,2:User2", 0, 2, 1));
cruiseToggles->addItem(new CValueControl("LfaButtonMode", "Button: LFA Button Mode", "0:Normal,1:Decel&Stop&LeadCarReady", 0, 1, 1));
cruiseToggles->addItem(new CValueControl("CruiseSpeedUnit", "Button: Cruise Speed Unit", "", 1, 20, 1));
cruiseToggles->addItem(new CValueControl("CruiseSpeedUnitBasic", "Button: Cruise Speed Unit(Basic)", "", 1, 20, 1));
cruiseToggles->addItem(new CValueControl("CruiseSpeedUnit", "Button: Cruise Speed Unit(Extra)", "", 1, 20, 1));
cruiseToggles->addItem(new CValueControl("CruiseEcoControl", "CRUISE: Eco control(4km/h)", "Temporarily increasing the set speed to improve fuel efficiency.", 0, 10, 1));
cruiseToggles->addItem(new CValueControl("AutoSpeedUptoRoadSpeedLimit", "CRUISE: Auto speed up (0%)", "Auto speed up based on the lead car up to RoadSpeedLimit.", 0, 200, 10));
cruiseToggles->addItem(new CValueControl("TFollowGap1", "GAP1: Apply TFollow (110)x0.01s", "", 70, 300, 5));

1
system/manager/manager.py
View File

@@ -97,6 +97,7 @@ def get_default_params():
("CruiseButtonTest2", "30"),
("CruiseButtonTest3", "1"),
("CruiseSpeedUnit", "10"),
("CruiseSpeedUnitBasic", "1"),
("CruiseSpeed1", "30"),
("CruiseSpeed2", "50"),
("CruiseSpeed3", "80"),