diff --git a/common/params_keys.h b/common/params_keys.h index f70887dc..37f3ceac 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -197,6 +197,7 @@ inline static std::unordered_map keys = { {"CruiseButtonTest2", PERSISTENT}, {"CruiseButtonTest3", PERSISTENT}, {"CruiseSpeedUnit", PERSISTENT}, + {"CruiseSpeedUnitBasic", PERSISTENT}, {"CruiseSpeed1", PERSISTENT}, {"CruiseSpeed2", PERSISTENT}, {"CruiseSpeed3", PERSISTENT}, diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 77ecf6f5..55bb891b 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -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: diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 34c26668..6957eab7 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -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 diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index c106f5f8..4b57a5b0 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -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): diff --git a/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc b/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc index 3c51209c..563eaebd 100644 --- a/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc +++ b/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc @@ -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 diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 37fb7442..3dc5ed59 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -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 diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index 802144f5..7e323f34 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -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, diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index ac410218..3ea60ec2 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -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 diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 6d499a71..4b874294 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -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)); diff --git a/system/manager/manager.py b/system/manager/manager.py index c9607976..49c5e324 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -97,6 +97,7 @@ def get_default_params(): ("CruiseButtonTest2", "30"), ("CruiseButtonTest3", "1"), ("CruiseSpeedUnit", "10"), + ("CruiseSpeedUnitBasic", "1"), ("CruiseSpeed1", "30"), ("CruiseSpeed2", "50"), ("CruiseSpeed3", "80"),