diff --git a/common/params_keys.h b/common/params_keys.h index 37f3ceac..d0ef4c53 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -151,7 +151,6 @@ inline static std::unordered_map keys = { {"ShowRouteInfo", PERSISTENT }, {"ShowPathMode", PERSISTENT}, {"ShowPathColor", PERSISTENT}, - {"ShowPathModeCruiseOff", PERSISTENT}, {"ShowPathColorCruiseOff", PERSISTENT}, {"ShowPathModeLane", PERSISTENT}, {"ShowPathColorLane", PERSISTENT}, @@ -192,6 +191,7 @@ inline static std::unordered_map keys = { {"ComfortBrake", PERSISTENT}, {"JLeadFactor3", PERSISTENT}, {"CruiseButtonMode", PERSISTENT}, + {"CancelButtonMode", PERSISTENT}, {"LfaButtonMode", PERSISTENT}, {"CruiseButtonTest1", PERSISTENT}, {"CruiseButtonTest2", PERSISTENT}, diff --git a/launch_openpilot.sh b/launch_openpilot.sh index d6e3424c..abc35aab 100755 --- a/launch_openpilot.sh +++ b/launch_openpilot.sh @@ -1,3 +1,6 @@ #!/usr/bin/env bash - +if [[ "$(cat /data/params/d/EnableConnect)" == "2" ]]; then + export API_HOST="https://api.carrotpilot.app" + export ATHENA_HOST="wss://athena.carrotpilot.app" +fi exec ./launch_chffrplus.sh diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 6957eab7..07ef642b 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -311,7 +311,7 @@ class CarState(CarStateBase): cruise_button = [Buttons.NONE] if self.cruise_buttons_alt: 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"] + cruise_button = [Buttons.LFA_BUTTON] if lfa_button > 0 else [cp.vl["CRUISE_BUTTON_ALT"]["CruiseSwState"]] elif self.HAS_LFA_BUTTON and cp.vl["BCM_PO_11"]["LFA_Pressed"] == 1: # for K5 cruise_button = [Buttons.LFA_BUTTON] else: @@ -461,7 +461,7 @@ class CarState(CarStateBase): if self.CP.flags & HyundaiFlags.CAMERA_SCC.value: self.MainMode_ACC = cp_cam.vl["SCC_CONTROL"]["MainMode_ACC"] == 1 self.ACCMode = cp_cam.vl["SCC_CONTROL"]["ACCMode"] - self.LFA_ICON = cp_cam.vl["LFAHDA_CLUSTER"]["LFA_ICON"] + self.LFA_ICON = cp_cam.vl["LFAHDA_CLUSTER"]["HDA_LFA_SymSta"] if self.CP.openpilotLongitudinalControl: # These are not used for engage/disengage since openpilot keeps track of state using the buttons @@ -524,6 +524,8 @@ class CarState(CarStateBase): self.tcs_info_373 = cp.vl["TCS"] ret.gearStep = cp.vl["GEAR"]["GEAR_STEP"] if self.GEAR else 0 + if 1 <= ret.gearStep <= 8 and ret.gearShifter == GearShifter.unknown: + ret.gearShifter = GearShifter.drive ret.gearStep = cp.vl["GEAR_ALT"]["GEAR_STEP"] if self.GEAR_ALT else ret.gearStep if cp_alt and self.CP.flags & HyundaiFlags.CAMERA_SCC: diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index 4b57a5b0..dfff3f5e 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): + 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 + values["HDA_CntrlModSta"] = 2 if long_active else 0 + values["HDA_LFA_SymSta"] = 2 if lat_active else 0 + + # else: return [] return [packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)] @@ -511,7 +514,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle values["NAV_ICON"] = 2 if nav_active else 0 values["HDA_ICON"] = 5 if hdp_active else 2 if cruise_enabled else 1 if main_enabled else 0 values["LFA_ICON"] = 5 if hdp_active else 2 if lat_active else 1 if lat_enabled else 0 - values["LKA_ICON"] = 4 if lat_active else 3 if lat_enabled else 1 + values["LKA_ICON"] = 4 if lat_active else 3 if lat_enabled else 0 values["FCA_ALT_ICON"] = 0 if values["ALERTS_2"] in [1, 2, 5]: diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index 21ff06a7..c9e92478 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -229,7 +229,7 @@ class CarInterface(CarInterfaceBase): # Dashcam cars are missing a test route, or otherwise need validation # TODO: Optima Hybrid 2017 uses a different SCC12 checksum - ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, } + #ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, } return ret diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 04cb48dd..820936b2 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -748,7 +748,7 @@ class CAR(Platforms): ) KIA_K5_HEV = HyundaiPlatformConfig( [HyundaiCarDocs("Kia K5 Hybrid 2017", "All", car_parts=CarParts.common([CarHarness.hyundai_c]))], - CarSpecs(mass=1515, wheelbase=2.80, steerRatio=15.5, tireStiffnessFactor=0.7), + CarSpecs(mass=1705, wheelbase=2.80, steerRatio=15.5, tireStiffnessFactor=0.7), flags=HyundaiFlags.HYBRID | HyundaiFlags.LEGACY, ) KIA_K5_HEV_2022 = HyundaiPlatformConfig( diff --git a/opendbc_repo/opendbc/car/rivian/radar_interface.py b/opendbc_repo/opendbc/car/rivian/radar_interface.py index 6e1cad36..323faea6 100644 --- a/opendbc_repo/opendbc/car/rivian/radar_interface.py +++ b/opendbc_repo/opendbc/car/rivian/radar_interface.py @@ -62,7 +62,7 @@ class RadarInterface(RadarInterfaceBase): self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST'] self.pts[addr].vRel = msg['REL_SPEED'] self.pts[addr].aRel = float('nan') - self.pts[addr].yvRel = float('nan') + self.pts[addr].yvRel = 0 #float('nan') else: del self.pts[addr] diff --git a/opendbc_repo/opendbc/car/toyota/radar_interface.py b/opendbc_repo/opendbc/car/toyota/radar_interface.py index 2779eb4c..76dbbc44 100755 --- a/opendbc_repo/opendbc/car/toyota/radar_interface.py +++ b/opendbc_repo/opendbc/car/toyota/radar_interface.py @@ -83,7 +83,7 @@ class RadarInterface(RadarInterfaceBase): self.pts[ii].vRel = cpt['REL_SPEED'] self.pts[ii].vLead = self.pts[ii].vRel + self.v_ego self.pts[ii].aRel = float('nan') - self.pts[ii].yvRel = float('nan') + self.pts[ii].yvRel = 0 #float('nan') self.pts[ii].measured = bool(cpt['VALID']) else: if ii in self.pts: diff --git a/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc b/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc index 563eaebd..1b98b9f6 100644 --- a/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc +++ b/opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc @@ -492,17 +492,21 @@ BO_ 474 ADRV_0x1da: 32 ADRV SG_ SET_ME_22 : 31|8@0+ (1,0) [0|255] "" XXX SG_ SET_ME_41 : 47|8@0+ (1,0) [0|255] "" XXX -BO_ 480 LFAHDA_CLUSTER: 16 ADRV +BO_ 480 LFAHDA_CLUSTER: 16 FR_CMR SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" 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_ 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 - + SG_ HDA_OptUsmSta : 24|3@1+ (1,0) [0|0] "" CGW + SG_ LFA_OptUsmSta : 27|3@1+ (1,0) [0|0] "" CGW + SG_ HDA_CntrlModSta : 30|2@1+ (1,0) [0|0] "" CGW + SG_ HDA_InfoPUDis : 32|3@1+ (1,0) [0|0] "" CGW + SG_ HDA_AutoSetSpdSta : 35|2@1+ (1,0) [0|0] "" CGW + SG_ HDA_AutoSetSpdUpdtSta : 37|2@1+ (1,0) [0|0] "" CGW + SG_ HDA_AutoSetSpdVal : 39|8@1+ (1,0) [0|0] "km/h" CGW + SG_ HDA_LFA_SymSta : 47|2@1+ (1,0) [0|0] "" CGW + SG_ HDA_LFA_WrnSnd : 49|2@1+ (1,0) [0|0] "" Dummy + SG_ HDA_InfoPUDis1 : 51|3@1+ (1,0) [0|7] "" CLU,CGW + SG_ HDA_TDMRMDclReq : 54|2@1+ (1,0) [0|3] "" Dummy + BO_ 490 ADRV_0x1ea: 32 ADRV SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX @@ -975,6 +979,16 @@ VAL_ 426 CRUISE_BUTTONS 0 "none" 1 "res_accel" 2 "set_decel" 3 "gap_distance" 4 VAL_ 463 CRUISE_BUTTONS 0 "none" 1 "res_accel" 2 "set_decel" 3 "gap_distance" 4 "pause_resume" ; VAL_ 463 RIGHT_PADDLE 0 "Not Pulled" 1 "Pulled"; VAL_ 463 LEFT_PADDLE 0 "Not Pulled" 1 "Pulled"; +VAL_ 480 HDA_OptUsmSta 0 "Not Applied" 1 "Function Off" 2 "Function On" 3 "Reserved" 4 "Reserved" 5 "Reserved" 6 "Reserved" 7 "Invalid (Fail)" ; +VAL_ 480 LFA_OptUsmSta 0 "Not Applied" 1 "Function Off" 2 "Function On" 3 "Reserved" 4 "Reserved" 5 "Reserved" 6 "Reserved" 7 "Invalid (Fail)" ; +VAL_ 480 HDA_CntrlModSta 0 "System Deactive (Default)" 1 "System Ready" 2 "System Active" 3 "Reserved" ; +VAL_ 480 HDA_InfoPUDis 0 "No pop-up" 1 "system start pop-up" 2 "system auto disengaged pop-up by highway off" 3 "system auto disengaged pop-up" 4 "system Fail pop-up" 5 "Hands-off pop up" 6 "Hands-off pop up w/ sound" 7 "System Automatic off" ; +VAL_ 480 HDA_AutoSetSpdSta 0 "Auto Set Speed Off" 1 "Auto Set Speed On" 2 "Reserved" 3 "Error indicator" ; +VAL_ 480 HDA_AutoSetSpdUpdtSta 0 "Auto Set Speed Update Off" 1 "Auto Set Speed Update On" 2 "Reserved" 3 "Error indicator" ; +VAL_ 480 HDA_LFA_SymSta 0 "Off" 1 "Gray" 2 "Green" 3 "Green blink" ; +VAL_ 480 HDA_LFA_WrnSnd 0 "Off " 1 "Additional Warning Sound" 2 "Reserved" 3 "Error indicator" ; +VAL_ 480 HDA_InfoPUDis1 0 "No Pop-up" 1 "System Automatic off (LFA)" 2 "System Automatic off (HDA)" 3 "Reserved" 4 "Reserved" 5 "Reserved" 6 "Not Used" 7 "Error Indicator" ; +VAL_ 480 HDA_TDMRMDclReq 0 "Not automatically deactivated state of LFA (default)" 1 "Automatically deactivated state of LFA" 2 "Reserved" 3 "Reserved" ; VAL_ 676 LEFT_LANE_LINE 0 "Not Detected" 1 "Low Confidence" 2 "Medium Confidence" 3 "High Confidence"; VAL_ 676 RIGHT_LANE_LINE 0 "Not Detected" 1 "Low Confidence" 2 "Medium Confidence" 3 "High Confidence"; VAL_ 736 MSLA_STATUS 0 "disabled" 1 "active" 2 "paused"; diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 3dc5ed59..f02048aa 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -170,6 +170,7 @@ class VCruiseCarrot: self._cruise_speed_unit = 10 self._cruise_speed_unit_basic = 1 self._cruise_button_mode = 2 + self._cancel_button_mode = 0 self._lfa_button_mode = 0 self._gas_pressed_count = 0 @@ -257,6 +258,7 @@ class VCruiseCarrot: 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._cancel_button_mode = self.params.get_int("CancelButtonMode") self._lfa_button_mode = self.params.get_int("LfaButtonMode") self.autoRoadSpeedLimitOffset = self.params.get_int("AutoRoadSpeedLimitOffset") self.autoNaviSpeedSafetyFactor = self.params.get_float("AutoNaviSpeedSafetyFactor") * 0.01 @@ -408,7 +410,9 @@ class VCruiseCarrot: if bt == ButtonType.accelCruise: button_kph += SPEED_UP_UNIT if is_metric else SPEED_UP_UNIT * CV.MPH_TO_KPH elif bt == ButtonType.decelCruise: - button_kph -= SPEED_DOWN_UNIT if is_metric else SPEED_DOWN_UNIT * CV.MPH_TO_KPH + #button_kph -= SPEED_DOWN_UNIT if is_metric else SPEED_DOWN_UNIT * CV.MPH_TO_KPH + unit = SPEED_DOWN_UNIT * 1 if is_metric else CV.MPH_TO_KPH + button_kph = math.floor((button_kph - 0.01) / unit) * unit button_type = bt self.long_pressed = False self.button_cnt = 0 @@ -549,9 +553,9 @@ class VCruiseCarrot: print("lfaButton") elif button_type == ButtonType.cancel: self._paddle_decel_active = False - #if self._cruise_cancel_state: - # self._lat_enabled = not self._lat_enabled - # self._add_log("Lateral " + "enabled" if self._lat_enabled else "disabled") + if self._cancel_button_mode in [1]: + self._lat_enabled = False + self._add_log("Lateral " + "enabled" if self._lat_enabled else "disabled") self._cruise_cancel_state = True #self._v_cruise_kph_at_brake = 0 else: @@ -573,6 +577,7 @@ class VCruiseCarrot: self._lat_enabled = False self._paddle_decel_active = False self.params.put_bool_nonblocking("ExperimentalMode", not self.params.get_bool("ExperimentalMode")) + self._add_log("Lateral " + "enabled" if self._lat_enabled else "disabled") if self._paddle_mode > 0 and button_type in [ButtonType.paddleLeft, ButtonType.paddleRight]: # paddle button self._cruise_control(-2, -1, "Cruise off & Ready (paddle)") @@ -744,8 +749,11 @@ class VCruiseCarrot: elif self._paddle_decel_active: if self.xState in [3]: self._paddle_decel_active = False + v_cruise_kph = self.v_ego_kph_set elif self.d_rel > 0: self._paddle_decel_active = False + v_cruise_kph = self.v_ego_kph_set + if self._gas_pressed_count > self._gas_tok_timer: if CS.aEgo < -0.5: diff --git a/selfdrive/carrot/carrot_man.py b/selfdrive/carrot/carrot_man.py index b8359bac..37bbb161 100644 --- a/selfdrive/carrot/carrot_man.py +++ b/selfdrive/carrot/carrot_man.py @@ -375,7 +375,7 @@ class CarrotMan: def carrot_navi_route(self): if self.carrot_serv.active_carrot > 1: - if self.navd_active: + if False and self.navd_active: # mabox always active self.navd_active = False self.params.remove("NavDestination") if not self.navi_points_active or not SHAPELY_AVAILABLE or (self.carrot_serv.active_carrot <= 1 and not self.navd_active): @@ -836,9 +836,10 @@ class CarrotMan: self.navd_active = True # 경로수신 -> carrotman active되고 약간의 시간지연이 발생함.. - self.carrot_serv.active_count = 80 - self.carrot_serv.active_sdi_count = self.carrot_serv.active_sdi_count_max - self.carrot_serv.active_carrot = 2 + if not from_navd: + self.carrot_serv.active_count = 80 + self.carrot_serv.active_sdi_count = self.carrot_serv.active_sdi_count_max + self.carrot_serv.active_carrot = 2 coords = [{"latitude": c.latitude, "longitude": c.longitude} for c in coords] #print("navdNaviPoints=", self.navi_points) @@ -1620,6 +1621,7 @@ class CarrotServ: self.nTBTTurnType = self.nTBTTurnTypeNext = -1 self.roadcate = 8 self.nGoPosDist = 0 + if self.active_carrot <= 1 or self.active_kisa_count > 0: self.update_nav_instruction(sm) if self.xSpdType < 0 or (self.xSpdType not in [100,101] and self.xSpdDist <= 0) or (self.xSpdType in [100,101] and self.xSpdDist < -250): @@ -1793,7 +1795,7 @@ class CarrotServ: pm.send('carrotMan', msg) inst = messaging.new_message('navInstructionCarrot') - if self.active_carrot > 1: + if self.active_carrot > 1 and self.active_kisa_count <= 0: inst.valid = True instruction = inst.navInstructionCarrot diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index 7e323f34..84a541d5 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -638,6 +638,19 @@ "default": 0, "unit": 1 }, + { + "group": "버튼설정", + "name": "CancelButtonMode", + "title": "캔슬버튼작동모드(0)", + "descr": "0:Long control, 1: Long + Lateral", + "egroup": "BUTN", + "etitle": "Cancel Button Mode(0)", + "edescr": "0:Long control, 1: Long + Lateral", + "min": 0, + "max": 1, + "default": 0, + "unit": 1 + }, { "group": "버튼설정", "name": "LfaButtonMode", @@ -928,12 +941,12 @@ "group": "시작", "name": "EnableConnect", "title": "EnableConnect", - "descr": "콤마커넥트를 사용합니다\n 상황에 따라 BAN될 수 있습니다\n 재부팅 필요", + "descr": "(1:콤마, 2:당근)커넥트를 사용합니다\n 상황에 따라 BAN될 수 있습니다\n 재부팅 필요", "egroup": "START", "etitle": "EnableConnect", - "edescr": "Using Comma Connect. May be banned depending on the situation. Reboot required.", + "edescr": "Using (1:comma, 2:carrot) Connect. May be banned depending on the situation. Reboot required.", "min": 0, - "max": 1, + "max": 2, "default": 0, "unit": 1 }, @@ -1146,19 +1159,6 @@ "default": 12, "unit": 1 }, - { - "group": "화면패스", - "name": "ShowPathModeCruiseOff", - "title": "패스종류,크루즈OFF(9)", - "descr": "0:일반,1,2:사각,3,4:^^,5,6:사각,7,8:^^,9,10,11:smooth^^,12:smooth2^^,13:3줄,14:2줄,15:1줄", - "egroup": "DISP", - "etitle": "Show Path Mode:CruiseOFF(9)", - "edescr": "0:stock,1,2:REC,3,4:^^,5,6:REC,7,8:^^,9,10,11:smooth^^,12:smooth2^^,13:3lines,14:2lines,15:1lines", - "min": 0, - "max": 15, - "default": 9, - "unit": 1 - }, { "group": "화면패스", "name": "ShowPathColorCruiseOff", diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 66a15e31..15c44747 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -56,6 +56,11 @@ class Controls: self.desired_curvature = 0.0 self.yStd = 0.0 + self.lead_left_dRel = None + self.lead_left_Lat = None + self.lead_right_dRel = None + self.lead_right_Lat = None + self.LoC = LongControl(self.CP) self.VM = VehicleModel(self.CP) self.LaC: LatControl @@ -253,20 +258,35 @@ class Controls: return None valid_leads = [ lead for lead in leads - #if lead.status and abs(lead.dPath) < 3.5 and lead.vLead > 2.0 and 5 < lead.dRel < 100 - if lead.status and abs(lead.dPath) < 4.2 and ((lead.vLead >= 2.0 and 5 < lead.dRel < 100) or (lead.vLead < 2.0 and 3 < lead.dRel < 30)) + if lead.status and abs(lead.dPath) < 4.2 and lead.vLead > 2.0 and 2 < lead.dRel < 130 + #if lead.status and abs(lead.dPath) < 4.2 and ((lead.vLead >= 2.0 and 5 < lead.dRel < 100) or (lead.vLead < 2.0 and 3 < lead.dRel < 30)) ] return min(valid_leads, key=lambda l: l.dRel) if valid_leads else None lead_left = _find_closest_lead(radarState.leadsLeft, road_edge_left) lead_right = _find_closest_lead(radarState.leadsRight, road_edge_right) if lead_left is not None: - hudControl.leadLeftDist = lead_left.dRel - hudControl.leadLeftLat = abs(lead_left.dPath) - #print(f"Lead left: {lead_left.dRel:.2f}m, {lead_left.dPath:.2f}m, {lead_left.vRel:.2f}m/s") + if self.lead_left_dRel is None: + self.lead_left_dRel = lead_left.dRel + self.lead_left_Lat = abs(lead_left.dPath) + else: + self.lead_left_dRel = self.lead_left_dRel * 0.98 + lead_left.dRel * 0.02 + self.lead_left_Lat = self.lead_left_Lat * 0.98 + abs(lead_left.dPath) * 0.02 + hudControl.leadLeftDist = self.lead_left_dRel + hudControl.leadLeftLat = self.lead_left_Lat + else: + self.lead_left_dRel = None if lead_right is not None: - hudControl.leadRightDist = lead_right.dRel - hudControl.leadRightLat = abs(lead_right.dPath) + if self.lead_right_dRel is None: + self.lead_right_dRel = lead_right.dRel + self.lead_right_Lat = abs(lead_right.dPath) + else: + self.lead_right_dRel = self.lead_right_dRel * 0.98 + lead_right.dRel * 0.02 + self.lead_right_Lat = self.lead_right_Lat * 0.98 + abs(lead_right.dPath) * 0.02 + hudControl.leadRightDist = self.lead_right_dRel + hudControl.leadRightLat = self.lead_right_Lat + else: + self.lead_right_dRel = None hudControl.rightLaneVisible = True diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 3ea60ec2..d5bb59cb 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -32,11 +32,10 @@ class Track: self.cnt = 0 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): + def update(self, md, pt, ready, radar_reaction_factor): #pt_yRel = -leads_v3[0].y[0] if track_id in [0, 1] and pt.yRel == 0 and self.ready and leads_v3[0].prob > 0.5 else pt.yRel self.dRel = pt.dRel @@ -53,9 +52,14 @@ class Track: if ready: self.dPath = self.yRel + np.interp(self.dRel, md.position.x, md.position.y) - a_lead_threshold = 0.5 * self.radar_reaction_factor + if self.cnt == 0: + self.yRel_filtered = self.yRel + else: + self.yRel_filtered = self.yRel_filtered * 0.98 + self.yRel * 0.02 + + a_lead_threshold = 0.5 * radar_reaction_factor if abs(self.aLead) < a_lead_threshold and abs(self.jLead) < 0.5: - self.aLeadTau.x = _LEAD_ACCEL_TAU * self.radar_reaction_factor + self.aLeadTau.x = _LEAD_ACCEL_TAU * radar_reaction_factor else: self.aLeadTau.update(0.0) @@ -105,11 +109,14 @@ 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(offset_vision_dist - c.dRel) > max_offset_vision_dist: - return -1e6 + #if abs(offset_vision_dist - c.dRel) > max_offset_vision_dist: + # return -1e6 #if abs(lead.v[0] - c.vLead) > max_offset_vision_vel: # return -1e6 + + #if abs(c.yRel + c.yvLead * radar_lat_factor + lead.y[0]) > 3.0: # lead.y[0]는 반대.. + # 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]) @@ -129,11 +136,21 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks best_score = score best_track = c + if best_track is not None and offset_vision_dist - best_track.dRel > max_offset_vision_dist: + best_track = None + + #if best_track is not None and lead.v[0] - best_track.vLead > max_offset_vision_vel: + # best_track = None + + if best_track is not None and abs(best_track.yRel + best_track.yvLead * radar_lat_factor + lead.y[0]) > 3.0: # lead.y[0]는 반대.. + best_track = None + if best_track is not None: - if abs(lead.v[0] - best_track.vLead) > max_offset_vision_vel: + + if 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): + # 직전에 사용되었던것이라면 재사용, 2초간 유지된다면 정지차로 간주. + if best_track.selected_count < 1 and best_track.is_stopped_car_count < int(2.0/DT_MDL): best_track = None if best_track is not None: @@ -174,6 +191,7 @@ def get_lead_side(v_ego, tracks, md, lane_width, model_v_ego, radar_lat_factor = leadCenter = {'status': False} leadLeft = {'status': False} leadRight = {'status': False} + leadCutIn = {'status': False} ## SCC레이더는 일단 보관하고 리스트에서 삭제... track_scc = tracks.get(0) @@ -186,7 +204,7 @@ def get_lead_side(v_ego, tracks, md, lane_width, model_v_ego, radar_lat_factor = md_y = md.position.y md_x = md.position.x else: - return [[],[],[],leadCenter,leadLeft,leadRight] + return [[],[],[],leadCenter,leadLeft,leadRight,leadCutIn] leads_center = {} leads_left = {} @@ -195,7 +213,7 @@ def get_lead_side(v_ego, tracks, md, lane_width, model_v_ego, radar_lat_factor = for c in tracks.values(): # d_y : path_y - traks_y 의 diff값 # yRel값은 왼쪽이 +값, lead.y[0]값은 왼쪽이 -값 - d_y = c.yRel + np.interp(c.dRel, md_x, md_y) + c.yvLead * radar_lat_factor + d_y = c.yRel_filtered + np.interp(c.dRel, md_x, md_y) + c.yvLead * radar_lat_factor if abs(d_y) < lane_width / 2 * 0.8: if c.cnt > 6: ld = c.get_RadarState(lead_msg.prob, float(-lead_msg.y[0])) @@ -207,6 +225,10 @@ def get_lead_side(v_ego, tracks, md, lane_width, model_v_ego, radar_lat_factor = ld = c.get_RadarState(0, 0) leads_left[c.dRel] = ld + if abs(d_y) < 2.3 and 4 < c.dRel < 20.0 and c.vLead > 4.0: + if leadCutIn['status'] is False or c.dRel < leadCutIn['dRel']: + leadCutIn = c.get_RadarState(lead_msg.prob) + if False: #lead_msg.prob > 0.5: # center에 비젼데이터 안넣음.. ld = get_RadarState_from_vision(md, lead_msg, v_ego, model_v_ego) leads_center[ld['dRel']] = ld @@ -228,6 +250,7 @@ def get_lead_side(v_ego, tracks, md, lane_width, model_v_ego, radar_lat_factor = leadRight = min((lead for dRel, lead in leads_right.items() if lead['dRel'] > 5.0 and abs(lead['dPath']) < 3.5), key=lambda x: x['dRel'], default=leadRight) leadCenter = min((lead for dRel, lead in leads_center.items() if lead['vLead'] > 5 and lead['radar']), key=lambda x: x['dRel'], default=leadCenter) + #filtered_leads_left = {dRel: lead for dRel, lead in leads_left.items() if lead['dRel'] > 5.0} #if filtered_leads_left: # dRel_min = min(filtered_leads_left.keys()) @@ -238,7 +261,7 @@ def get_lead_side(v_ego, tracks, md, lane_width, model_v_ego, radar_lat_factor = # dRel_min = min(filtered_leads_right.keys()) # leadRight = filtered_leads_right[dRel_min] - return [ll, lc, lr, leadCenter, leadLeft, leadRight] + return [ll, lc, lr, leadCenter, leadLeft, leadRight, leadCutIn] def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, model_v_ego: float, low_speed_override: bool = True) -> dict[str, Any]: @@ -417,7 +440,8 @@ class RadarD: self.enable_radar_tracks = self.params.get_int("EnableRadarTracks") self.enable_corner_radar = self.params.get_int("EnableCornerRadar") self.radar_lat_factor = self.params.get_float("RadarLatFactor") * 0.01 - + self.radar_reaction_factor = self.params.get_float("RadarReactionFactor") * 0.01 + self.detect_cut_in = self.radar_lat_factor > 0 leads_v3 = sm['modelV2'].leadsV3 if sm.recv_frame['carState'] != self.last_v_ego_frame: @@ -433,7 +457,7 @@ class RadarD: if track_id not in self.tracks: self.tracks[track_id] = Track(track_id) - self.tracks[track_id].update(sm['modelV2'], pt, self.ready) + self.tracks[track_id].update(sm['modelV2'], pt, self.ready, self.radar_reaction_factor) for tid in list(self.tracks.keys()): if tid not in valid_ids: @@ -466,9 +490,19 @@ class RadarD: self.radar_state.leadOne, self.radar_detected = self.get_lead(sm['carState'], sm['modelV2'], self.tracks, 0, leads_v3[0], model_v_ego, low_speed_override=False) self.radar_state.leadTwo, _ = self.get_lead(sm['carState'], sm['modelV2'], self.tracks, 1, leads_v3[1], model_v_ego, low_speed_override=False) - ll, lc, lr, leadCenter, self.radar_state.leadLeft, self.radar_state.leadRight = get_lead_side(self.v_ego, self.tracks, sm['modelV2'], 3.2, model_v_ego, self.radar_lat_factor) + ll, lc, lr, leadCenter, self.radar_state.leadLeft, self.radar_state.leadRight, leadCutIn = get_lead_side(self.v_ego, self.tracks, sm['modelV2'], 3.2, model_v_ego, self.radar_lat_factor) - if leadCenter is not None and leadCenter["status"]: + if leadCutIn is not None and leadCutIn["status"] and self.detect_cut_in: + if self.radar_state.leadOne.status: + if leadCutIn["dRel"] < self.radar_state.leadOne.dRel: + leadCutIn["modelProb"] = 0.03 + self.radar_state.leadOne = leadCutIn + self.radar_detected = True + else: + self.radar_detected = True + leadCutIn["modelProb"] = 0.03 + self.radar_state.leadOne = leadCutIn + elif leadCenter is not None and leadCenter["status"]: if self.radar_detected: if leadCenter["dRel"] < self.radar_state.leadOne.dRel: leadCenter["modelProb"] = 0.01 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 67a44aa6..76a8615c 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -164,14 +164,14 @@ class ModelState: if prepare_only: return None - self.vision_output = self.vision_run(**self.vision_inputs).contiguous().realize().uop.base.buffer.numpy() + self.vision_output = self.vision_run(**self.vision_inputs).numpy().flatten() vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(self.vision_output, self.vision_output_slices)) self.full_features_buffer[0,:-1] = self.full_features_buffer[0,1:] self.full_features_buffer[0,-1] = vision_outputs_dict['hidden_state'][0, :] self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs] - self.policy_output = self.policy_run(**self.policy_inputs).contiguous().realize().uop.base.buffer.numpy() + self.policy_output = self.policy_run(**self.policy_inputs).numpy().flatten() policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(self.policy_output, self.policy_output_slices)) # TODO model only uses last value now diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx index eb786600..9317fa26 100644 Binary files a/selfdrive/modeld/models/driving_policy.onnx and b/selfdrive/modeld/models/driving_policy.onnx differ diff --git a/selfdrive/modeld/models/driving_vision.onnx b/selfdrive/modeld/models/driving_vision.onnx index ccaa3dff..ae08dbba 100644 Binary files a/selfdrive/modeld/models/driving_vision.onnx and b/selfdrive/modeld/models/driving_vision.onnx differ diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py index b89de275..d5a5dc1b 100644 --- a/selfdrive/modeld/parse_model_outputs.py +++ b/selfdrive/modeld/parse_model_outputs.py @@ -93,9 +93,6 @@ class Parser: self.parse_binary_crossentropy('lane_lines_prob', outs) self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH)) self.parse_binary_crossentropy('meta', outs) - self.parse_binary_crossentropy('lead_prob', outs) - self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION, - out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH)) return outs def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: @@ -106,6 +103,9 @@ class Parser: if 'desired_curvature' in outs: self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,)) self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,)) + self.parse_binary_crossentropy('lead_prob', outs) + self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION, + out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH)) return outs def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: diff --git a/selfdrive/ui/carrot.cc b/selfdrive/ui/carrot.cc index 3807ea7f..bc82bf6c 100644 --- a/selfdrive/ui/carrot.cc +++ b/selfdrive/ui/carrot.cc @@ -764,7 +764,7 @@ public: NVGcolor rcolor = isLeadSCC() ? COLOR_RED : COLOR_ORANGE; NVGcolor pcolor = !isRadarDetected() ? ((trafficState == 1) ? rcolor : COLOR_GREEN) : isRadarDetected() ? rcolor : COLOR_BLUE; bool show_path_end = true; - if (show_path_end && !isLeadDetected()) { + if (false && show_path_end && !isLeadDetected()) { px[0] = path_x - path_width / 2; px[1] = path_x + path_width / 2; px[2] = path_x + path_width / 2; @@ -1145,9 +1145,9 @@ protected: int remaining_minutes = (int)nGoPosTime / 60; local->tm_min += remaining_minutes; mktime(local); - sprintf(str, "도착: %.1f분(%02d:%02d)", (float)nGoPosTime / 60., local->tm_hour, local->tm_min); + sprintf(str, "%s: %.1f%s(%02d:%02d)", tr("ETA").toStdString().c_str(), (float)nGoPosTime / 60., tr("MIN").toStdString().c_str(), local->tm_hour, local->tm_min); ui_draw_text(s, tbt_x + 190, tbt_y + 80, str, 50, COLOR_WHITE, BOLD); - sprintf(str, "%.1fkm", nGoPosDist / 1000.); + sprintf(str, "%.1f%s", nGoPosDist / 1000. * ((s->scene.is_metric)?1:KM_TO_MILE), (s->scene.is_metric) ? "km" : "mile"); ui_draw_text(s, tbt_x + 190 + 120, tbt_y + 130, str, 50, COLOR_WHITE, BOLD); } return 0; @@ -1382,7 +1382,6 @@ private: int show_path_color_normal = 14; int show_path_mode_lane = 13; int show_path_color_lane = 14; - int show_path_mode_cruise_off = 13; int show_path_color_cruise_off = 14; float show_path_width = 1.0; Params params; @@ -1602,19 +1601,16 @@ protected: auto selfdrive_state = sm["selfdriveState"].getSelfdriveState(); longActive = selfdrive_state.getEnabled(); - if (longActive == false) { - show_path_mode = show_path_mode_cruise_off; - show_path_color = show_path_color_cruise_off; + if (active_lane_line) { + show_path_mode = show_path_mode_lane; + show_path_color = show_path_color_lane; } else { - if (active_lane_line) { - show_path_mode = show_path_mode_lane; - show_path_color = show_path_color_lane; - } - else { - show_path_mode = show_path_mode_normal; - show_path_color = show_path_color_normal; - } + show_path_mode = show_path_mode_normal; + show_path_color = show_path_color_normal; + } + if (longActive == false) { + show_path_color = show_path_color_cruise_off; } if (show_path_mode == 0) { @@ -1639,7 +1635,6 @@ public: show_path_color_normal = params.getInt("ShowPathColor"); show_path_mode_lane = params.getInt("ShowPathModeLane"); show_path_color_lane = params.getInt("ShowPathColorLane"); - show_path_mode_cruise_off = params.getInt("ShowPathModeCruiseOff"); show_path_color_cruise_off = params.getInt("ShowPathColorCruiseOff"); } if (!make_data(s)) return; diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 4b874294..483c2247 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -660,6 +660,7 @@ 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("CancelButtonMode", "Button: Cancel Button Mode", "0:Long,1:Long+Lat", 0, 1, 1)); cruiseToggles->addItem(new CValueControl("LfaButtonMode", "Button: LFA Button Mode", "0:Normal,1:Decel&Stop&LeadCarReady", 0, 1, 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)); @@ -747,7 +748,6 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) { //dispToggles->addItem(new CValueControl("ShowDmInfo", "DM Info", "0:None,1:Display,-1:Disable(Reboot)", -1, 1, 1)); pathToggles = new ListWidget(this); - pathToggles->addItem(new CValueControl("ShowPathModeCruiseOff", "Path Mode: Cruise OFFF", "0:Normal,1,2:Rec,3,4:^^,5,6:Rec,7,8:^^,9,10,11,12:Smooth^^", 0, 15, 1)); pathToggles->addItem(new CValueControl("ShowPathColorCruiseOff", "Path Color: Cruise OFF", "(+10:Stroke)0:Red,1:Orange,2:Yellow,3:Green,4:Blue,5:Indigo,6:Violet,7:Brown,8:White,9:Black", 0, 19, 1)); pathToggles->addItem(new CValueControl("ShowPathMode", "Path Mode: Laneless", "0:Normal,1,2:Rec,3,4:^^,5,6:Rec,7,8:^^,9,10,11,12:Smooth^^", 0, 15, 1)); pathToggles->addItem(new CValueControl("ShowPathColor", "Path Color: Laneless", "(+10:Stroke)0:Red,1:Orange,2:Yellow,3:Green,4:Blue,5:Indigo,6:Violet,7:Brown,8:White,9:Black", 0, 19, 1)); @@ -822,7 +822,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) { startToggles->addItem(new CValueControl("SoundVolumeAdjust", "Sound Volume(100%)", "", 5, 200, 5)); startToggles->addItem(new CValueControl("SoundVolumeAdjustEngage", "Sound Volume, Engage(10%)", "", 5, 200, 5)); startToggles->addItem(new CValueControl("MaxTimeOffroadMin", "Power off time (min)", "", 1, 600, 10)); - startToggles->addItem(new CValueControl("EnableConnect", "EnableConnect", "Your device may be banned by Comma", 0, 1, 1)); + startToggles->addItem(new CValueControl("EnableConnect", "EnableConnect", "Your device may be banned by Comma", 0, 2, 1)); startToggles->addItem(new CValueControl("MapboxStyle", "Mapbox Style(0)", "", 0, 2, 1)); startToggles->addItem(new CValueControl("RecordRoadCam", "Record Road camera(0)", "1:RoadCam, 2:RoadCam+WideRoadCam", 0, 2, 1)); startToggles->addItem(new CValueControl("HDPuse", "Use HDP(CCNC)(0)", "1:While Using APN, 2:Always", 0, 2, 1)); diff --git a/selfdrive/ui/qt/screenrecorder/screenrecorder.cc b/selfdrive/ui/qt/screenrecorder/screenrecorder.cc index fb3a524d..22e4ef97 100644 --- a/selfdrive/ui/qt/screenrecorder/screenrecorder.cc +++ b/selfdrive/ui/qt/screenrecorder/screenrecorder.cc @@ -172,7 +172,7 @@ void ScreenRecoder::update_screen() { if(recording) { - if(milliseconds() - started > 1000*60*3) { + if(milliseconds() - started > 1000*60*20) { // 20 minutes stop(); start(); return; diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index a9c6acec..c323b9c0 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -759,6 +759,17 @@ Firehose Mode allows you to maximize your training data uploads to improve openp 신호대기 + + TurnInfoDrawer + + ETA + 도착 + + + MIN + 분 + + PrimeAdWidget diff --git a/system/athena/registration.py b/system/athena/registration.py index 964fbff5..70aedc3e 100755 --- a/system/athena/registration.py +++ b/system/athena/registration.py @@ -33,6 +33,8 @@ def register(show_spinner=False) -> str | None: """ params = Params() + + #return UNREGISTERED_DONGLE_ID # for c3lite, clone dongle_id: str | None = params.get("DongleId", encoding='utf8') if dongle_id is None and Path(Paths.persist_root()+"/comma/dongle_id").is_file(): # not all devices will have this; added early in comma 3X production (2/28/24) diff --git a/system/loggerd/uploader.py b/system/loggerd/uploader.py index 6e6df611..50882c56 100755 --- a/system/loggerd/uploader.py +++ b/system/loggerd/uploader.py @@ -86,6 +86,8 @@ class Uploader: self.immediate_folders = ["crash/", "boot/"] self.immediate_priority = {"qlog": 0, "qlog.zst": 0, "qcamera.ts": 1} + if (self.params.get_int("EnableConnect") == 2): + self.immediate_priority.update({"rlog": 0, "rlog.zst": 0}) def list_upload_files(self, metered: bool) -> Iterator[tuple[str, str, str]]: r = self.params.get("AthenadRecentlyViewedRoutes", encoding="utf8") diff --git a/system/manager/manager.py b/system/manager/manager.py index 49c5e324..39e3be2f 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -51,7 +51,6 @@ def get_default_params(): ("ShowRouteInfo", "1"), ("ShowPathMode", "9"), ("ShowPathColor", "13"), - ("ShowPathModeCruiseOff", "0"), ("ShowPathColorCruiseOff", "19"), ("ShowPathModeLane", "14"), ("ShowPathColorLane", "13"), @@ -92,6 +91,7 @@ def get_default_params(): ("StopDistanceCarrot", "550"), ("JLeadFactor3", "0"), ("CruiseButtonMode", "0"), + ("CancelButtonMode", "0"), ("LfaButtonMode", "0"), ("CruiseButtonTest1", "8"), ("CruiseButtonTest2", "30"),