change laneChange settings, lfa button mode, ev3 start.. (#176)

* paddle Decel sound...

* fix angle torque.. more stronger

* add lfa decel mode...

* fix..

* cruise, e2e decel limit to COMFORT BRAKE

* fix.. lanemode change...

* fix.. lfa decel..

* remove lfa undecel...

* revert cruise braking

* fix..

* add toyota

* fix.. emergency steer..

* fix.. vision track..

* fix..

* Revert "fix.."

This reverts commit 30c5a46cdf15d0db9194f1456dee3751d4784602.

* fix.. lanechange option...

* fix.. typo

* add ev3
This commit is contained in:
carrot 2025-05-24 21:40:49 +09:00 committed by GitHub
parent 2316f2f168
commit d3ae10dc1d
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
17 changed files with 124 additions and 38 deletions

View File

@ -186,6 +186,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
{"ComfortBrake", PERSISTENT}, {"ComfortBrake", PERSISTENT},
{"JLeadFactor3", PERSISTENT}, {"JLeadFactor3", PERSISTENT},
{"CruiseButtonMode", PERSISTENT}, {"CruiseButtonMode", PERSISTENT},
{"LfaButtonMode", PERSISTENT},
{"CruiseButtonTest1", PERSISTENT}, {"CruiseButtonTest1", PERSISTENT},
{"CruiseButtonTest2", PERSISTENT}, {"CruiseButtonTest2", PERSISTENT},
{"CruiseButtonTest3", PERSISTENT}, {"CruiseButtonTest3", PERSISTENT},
@ -230,6 +231,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
{"UseLaneLineSpeedApply", PERSISTENT}, {"UseLaneLineSpeedApply", PERSISTENT},
{"AdjustLaneOffset", PERSISTENT}, {"AdjustLaneOffset", PERSISTENT},
{"LaneChangeNeedTorque", PERSISTENT}, {"LaneChangeNeedTorque", PERSISTENT},
{"LaneChangeBsd", PERSISTENT},
{"MaxAngleFrames", PERSISTENT}, {"MaxAngleFrames", PERSISTENT},
{"SoftHoldMode", PERSISTENT}, {"SoftHoldMode", PERSISTENT},
{"LatMpcPathCost", PERSISTENT}, {"LatMpcPathCost", PERSISTENT},

View File

@ -248,6 +248,7 @@ MIGRATION = {
"KIA EV6 PE (CV1)": HYUNDAI.KIA_EV6_PE, "KIA EV6 PE (CV1)": HYUNDAI.KIA_EV6_PE,
"KIA CARNIVAL 4TH GEN": HYUNDAI.KIA_CARNIVAL_4TH_GEN, "KIA CARNIVAL 4TH GEN": HYUNDAI.KIA_CARNIVAL_4TH_GEN,
"KIA EV9 (MV)": HYUNDAI.KIA_EV9, "KIA EV9 (MV)": HYUNDAI.KIA_EV9,
"KIA EV3 (SV1)": HYUNDAI.KIA_EV3,
"GENESIS GV60 ELECTRIC 1ST GEN": HYUNDAI.GENESIS_GV60_EV_1ST_GEN, "GENESIS GV60 ELECTRIC 1ST GEN": HYUNDAI.GENESIS_GV60_EV_1ST_GEN,
"GENESIS G70 2018": HYUNDAI.GENESIS_G70, "GENESIS G70 2018": HYUNDAI.GENESIS_G70,
"GENESIS G70 2020": HYUNDAI.GENESIS_G70_2020, "GENESIS G70 2020": HYUNDAI.GENESIS_G70_2020,

View File

@ -160,7 +160,8 @@ class CarController(CarControllerBase):
(1 - curve_scale) * self.angle_max_torque + curve_scale * 50, (1 - curve_scale) * self.angle_max_torque + curve_scale * 50,
self.angle_max_torque self.angle_max_torque
] ]
base_max_torque = np.interp(CS.out.vEgo * CV.MS_TO_KPH, [0, 30, 60], torque_pts) #base_max_torque = np.interp(CS.out.vEgo * CV.MS_TO_KPH, [0, 30, 60], torque_pts)
base_max_torque = np.interp(CS.out.vEgo * CV.MS_TO_KPH, [0, 20, 30], 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]) 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])

View File

@ -1267,6 +1267,12 @@ FW_VERSIONS = {
b'\xf1\x00MV MFC AT EUR LHD 1.00 1.02 99211-DO000 230616', b'\xf1\x00MV MFC AT EUR LHD 1.00 1.02 99211-DO000 230616',
], ],
}, },
CAR.KIA_EV3: { # (SV1)
(Ecu.fwdRadar, 0x7d0, None): [
],
(Ecu.fwdCamera, 0x7c4, None): [
],
},
CAR.HYUNDAI_SANTAFE_MX5: { # (MX5) CAR.HYUNDAI_SANTAFE_MX5: { # (MX5)
(Ecu.fwdRadar, 0x7d0, None): [ (Ecu.fwdRadar, 0x7d0, None): [
], ],

View File

@ -85,7 +85,7 @@ def create_steering_messages_camera_scc(frame, packer, CP, CAN, CC, lat_active,
if CP.extFlags & HyundaiExtFlags.CANFD_161.value: if CP.extFlags & HyundaiExtFlags.CANFD_161.value:
if CS.adrv_info_161 is not None: if CS.adrv_info_161 is not None:
values = CS.adrv_info_161 values = CS.adrv_info_161
emergency_steering = values["ALERTS_2"] in [11, 12, 13, 14, 15, 21, 22, 23, 24, 25, 26] emergency_steering = values["ALERTS_1"] in [11, 12, 13, 14, 15, 21, 22, 23, 24, 25, 26]
ret = [] ret = []

View File

@ -780,7 +780,13 @@ class CAR(Platforms):
CarSpecs(mass=2625, wheelbase=3.1, steerRatio=16.02), CarSpecs(mass=2625, wheelbase=3.1, steerRatio=16.02),
flags=HyundaiFlags.EV | HyundaiFlags.ANGLE_CONTROL, flags=HyundaiFlags.EV | HyundaiFlags.ANGLE_CONTROL,
) )
KIA_EV3 = HyundaiCanFDPlatformConfig(
[
HyundaiCarDocs("KIA EV3 (SV1)", car_parts=CarParts.common([CarHarness.hyundai_n])),
],
CarSpecs(mass=2055, wheelbase=2.90, steerRatio=16.0, tireStiffnessFactor=0.65),
flags=HyundaiFlags.EV,
)
class Buttons: class Buttons:
NONE = 0 NONE = 0

View File

@ -50,6 +50,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
"KIA_SORENTO" = [2.464854685101844, 1.5335274218367956, 0.12056170567599558] "KIA_SORENTO" = [2.464854685101844, 1.5335274218367956, 0.12056170567599558]
"KIA_STINGER" = [2.7499043387418967, 1.849652021986449, 0.12048334239559202] "KIA_STINGER" = [2.7499043387418967, 1.849652021986449, 0.12048334239559202]
"KIA_EV9" = [1.75, 1.75, 0.15] "KIA_EV9" = [1.75, 1.75, 0.15]
"KIA_EV3" = [3.2, 2.093457, 0.005]
"LEXUS_ES_TSS2" = [2.0357564999999997, 1.999082295195227, 0.101533] "LEXUS_ES_TSS2" = [2.0357564999999997, 1.999082295195227, 0.101533]
"LEXUS_NX" = [2.3525924753753613, 1.9731412277641067, 0.15168101064205927] "LEXUS_NX" = [2.3525924753753613, 1.9731412277641067, 0.15168101064205927]
"LEXUS_NX_TSS2" = [2.4331999786982936, 2.1045680431705414, 0.14099899317761067] "LEXUS_NX_TSS2" = [2.4331999786982936, 2.1045680431705414, 0.14099899317761067]

View File

@ -620,3 +620,13 @@ SECOC_CAR = CAR.with_flags(ToyotaFlags.SECOC)
NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER) NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER)
DBC = CAR.create_dbc_map() DBC = CAR.create_dbc_map()
if __name__ == "__main__":
cars = []
for platform in CAR:
for doc in platform.config.car_docs:
cars.append(doc.name)
cars.sort()
for c in cars:
print(c)

View File

@ -48,6 +48,7 @@ class CarSpecificEvents:
self.frame = 0 self.frame = 0
self.mute_door = False self.mute_door = False
self.mute_seatbelt = False self.mute_seatbelt = False
self.vCruise_prev = 250
def update_params(self): def update_params(self):
if self.frame % 100 == 0: if self.frame % 100 == 0:
@ -170,6 +171,12 @@ class CarSpecificEvents:
else: else:
events = self.create_common_events(CS, CS_prev) events = self.create_common_events(CS, CS_prev)
if CC.enabled:
if self.vCruise_prev == 0 and CS.vCruise > 0:
events.add(EventName.audioPrompt)
self.vCruise_prev = CS.vCruise
return events return events
def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears=None, pcm_enable=True, def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears=None, pcm_enable=True,
@ -269,4 +276,5 @@ class CarSpecificEvents:
events.add(EventName.buttonCancel) events.add(EventName.buttonCancel)
if CS.softHoldActive > 0: if CS.softHoldActive > 0:
events.add(EventName.softHold) events.add(EventName.softHold)
return events return events

View File

@ -169,6 +169,7 @@ class VCruiseCarrot:
self._cruise_speed_min, self._cruise_speed_max = 5, 161 self._cruise_speed_min, self._cruise_speed_max = 5, 161
self._cruise_speed_unit = 10 self._cruise_speed_unit = 10
self._cruise_button_mode = 2 self._cruise_button_mode = 2
self._lfa_button_mode = 0
self._gas_pressed_count = 0 self._gas_pressed_count = 0
self._gas_pressed_count_last = 0 self._gas_pressed_count_last = 0
@ -250,6 +251,7 @@ class VCruiseCarrot:
self._cruise_speed_unit = self.params.get_int("CruiseSpeedUnit") self._cruise_speed_unit = self.params.get_int("CruiseSpeedUnit")
self._paddle_mode = self.params.get_int("PaddleMode") self._paddle_mode = self.params.get_int("PaddleMode")
self._cruise_button_mode = self.params.get_int("CruiseButtonMode") self._cruise_button_mode = self.params.get_int("CruiseButtonMode")
self._lfa_button_mode = self.params.get_int("LfaButtonMode")
self.cruiseOnDist = self.params.get_float("CruiseOnDist") * 0.01 self.cruiseOnDist = self.params.get_float("CruiseOnDist") * 0.01
def update_v_cruise(self, CS, sm, is_metric): def update_v_cruise(self, CS, sm, is_metric):
@ -507,8 +509,14 @@ class VCruiseCarrot:
self.params.put_int_nonblocking('LongitudinalPersonality', personality) self.params.put_int_nonblocking('LongitudinalPersonality', personality)
#self.events.append(EventName.personalityChanged) #self.events.append(EventName.personalityChanged)
elif button_type == ButtonType.lfaButton: elif button_type == ButtonType.lfaButton:
self._lat_enabled = not self._lat_enabled if self._lfa_button_mode == 0:
self._add_log("Lateral " + "enabled" if self._lat_enabled else "disabled") self._lat_enabled = not self._lat_enabled
self._add_log("Lateral " + "enabled" if self._lat_enabled else "disabled")
else:
if False: #CC.enabled and self._paddle_decel_active: # 수정필요...
self._paddle_decel_active = False
else:
self._paddle_decel_active = True
print("lfaButton") print("lfaButton")
elif button_type == ButtonType.cancel: elif button_type == ButtonType.cancel:
self._paddle_decel_active = False self._paddle_decel_active = False
@ -615,6 +623,7 @@ class VCruiseCarrot:
def _update_cruise_state(self, CS, CC, v_cruise_kph): def _update_cruise_state(self, CS, CC, v_cruise_kph):
if not CC.enabled: if not CC.enabled:
self._pause_auto_speed_up = False
if self._brake_pressed_count == -1 and self._soft_hold_active > 0: if self._brake_pressed_count == -1 and self._soft_hold_active > 0:
self._soft_hold_active = 2 self._soft_hold_active = 2
self._cruise_control(1, -1, "Cruise on (soft hold)") self._cruise_control(1, -1, "Cruise on (soft hold)")

View File

@ -547,6 +547,19 @@
"default": 0, "default": 0,
"unit": 1 "unit": 1
}, },
{
"group": "버튼설정",
"name": "LfaButtonMode",
"title": "LFA버튼작동모드(0)",
"descr": "0:일반,1:감속정지&Ready",
"egroup": "BUTN",
"etitle": "LFA Button Mode(0)",
"edescr": "0:General,1:Decel&Stop, leadCar ready",
"min": 0,
"max": 1,
"default": 0,
"unit": 1
},
{ {
"group": "버튼설정", "group": "버튼설정",
"name": "CruiseButtonTest1", "name": "CruiseButtonTest1",
@ -603,10 +616,10 @@
"group": "버튼설정", "group": "버튼설정",
"name": "PaddleMode", "name": "PaddleMode",
"title": "패들시프트모드(0)", "title": "패들시프트모드(0)",
"descr": "회생제동작동후, 0:크루즈ON, 1:크루즈대기, 2: 자동감속", "descr": "회생패들작동 후, 0:크루즈ON, 1:크루즈대기, 2: 자동감속",
"egroup": "BUTN", "egroup": "BUTN",
"etitle": "PaddleShift Mode(0)", "etitle": "PaddleShift Mode(0)",
"edescr": "After Regen, 0:Cruise ON, 1:Cruise Ready, 2: decel & cruise ready", "edescr": "After Regen paddle, 0:Cruise ON, 1:Cruise Ready, 2: decel & cruise ready",
"min": 0, "min": 0,
"max": 2, "max": 2,
"default": 1, "default": 1,
@ -1046,12 +1059,25 @@
"group": "조향일반", "group": "조향일반",
"name": "LaneChangeNeedTorque", "name": "LaneChangeNeedTorque",
"title": "차선변경 조향토크 사용", "title": "차선변경 조향토크 사용",
"descr": "1: 차선변경시 조향토크를 줘야 작동함, 2: 깜박이 차선변경안함, 3: BSD무시함, 4:토크우선", "descr": "0: 즉시 차선변경, 1: 토크필요, -1:자동차선변경사용안함",
"egroup": "LAT", "egroup": "LAT",
"etitle": "LaneChange use steering torque", "etitle": "LaneChange use steering torque",
"edescr": "1: need torque, 2:no lanechange, 3:ignore BSD, 4:torque always", "edescr": "1: need torque",
"min": 0, "min": -1,
"max": 4, "max": 1,
"default": 0,
"unit": 1
},
{
"group": "조향일반",
"name": "LaneChangeBsd",
"title": "차선변경 BSD",
"descr": "0:BSD검출(조향토크허용), 1: 조향토크불허, -1: BSD사용안함",
"egroup": "LAT",
"etitle": "LaneChange BSD",
"edescr": "0:BSD detect(allow steer torque), 1: block steer torque, -1: ignore BSD",
"min": -1,
"max": 1,
"default": 0, "default": 0,
"unit": 1 "unit": 1
}, },

View File

@ -92,6 +92,7 @@ class ExistCounter:
class DesireHelper: class DesireHelper:
def __init__(self): def __init__(self):
self.params = Params() self.params = Params()
self.frame = 0
self.lane_change_state = LaneChangeState.off self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0 self.lane_change_timer = 0.0
@ -127,8 +128,7 @@ class DesireHelper:
self.object_detected_count = 0 self.object_detected_count = 0
self.laneChangeNeedTorque = 0 self.laneChangeNeedTorque = 0
self.ignore_bsd = False self.laneChangeBsd = 0
self.torque_always = False
self.driver_blinker_state = BLINKER_NONE self.driver_blinker_state = BLINKER_NONE
self.atc_type = "" self.atc_type = ""
@ -169,7 +169,10 @@ class DesireHelper:
def update(self, carstate, modeldata, lateral_active, lane_change_prob, carrotMan, radarState): def update(self, carstate, modeldata, lateral_active, lane_change_prob, carrotMan, radarState):
self.laneChangeNeedTorque = self.params.get_int("LaneChangeNeedTorque") if self.frame % 100 == 0:
self.laneChangeNeedTorque = self.params.get_int("LaneChangeNeedTorque")
self.laneChangeBsd = self.params.get_int("LaneChangeBsd")
self.frame += 1
self.carrot_lane_change_count = max(0, self.carrot_lane_change_count - 1) self.carrot_lane_change_count = max(0, self.carrot_lane_change_count - 1)
@ -181,13 +184,11 @@ class DesireHelper:
driver_blinker_changed = driver_blinker_state != self.driver_blinker_state driver_blinker_changed = driver_blinker_state != self.driver_blinker_state
self.driver_blinker_state = driver_blinker_state self.driver_blinker_state = driver_blinker_state
driver_desire_enabled = driver_blinker_state in [BLINKER_LEFT, BLINKER_RIGHT] driver_desire_enabled = driver_blinker_state in [BLINKER_LEFT, BLINKER_RIGHT]
if self.laneChangeNeedTorque == 2: if self.laneChangeNeedTorque < 0: # 운전자가 깜박이 켜도 차선변경 안함.
driver_desire_enabled = False driver_desire_enabled = False
if self.laneChangeNeedTorque == 3: ignore_bsd = True if self.laneChangeBsd < 0 else False
self.ignore_bsd = True block_lanechange_bsd = True if self.laneChangeBsd == 1 else False
if self.laneChangeNeedTorque == 4:
self.torque_always = True
self.blindspot_detected_counter = max(0, self.blindspot_detected_counter - 1) self.blindspot_detected_counter = max(0, self.blindspot_detected_counter - 1)
@ -304,7 +305,7 @@ class DesireHelper:
torque_applied = carstate.steeringPressed and torque_cond torque_applied = carstate.steeringPressed and torque_cond
blindspot_detected = blindspot_cond blindspot_detected = blindspot_cond
if blindspot_detected and not self.ignore_bsd: if blindspot_detected and not ignore_bsd:
self.blindspot_detected_counter = int(1.5 / DT_MDL) self.blindspot_detected_counter = int(1.5 / DT_MDL)
# BSD검출시.. 아래 두줄로 자동차선변경 해제함.. 위험해서 자동차선변경기능은 안하는걸로... # BSD검출시.. 아래 두줄로 자동차선변경 해제함.. 위험해서 자동차선변경기능은 안하는걸로...
#self.lane_change_state = LaneChangeState.off #self.lane_change_state = LaneChangeState.off
@ -313,19 +314,19 @@ class DesireHelper:
self.lane_change_state = LaneChangeState.off self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none self.lane_change_direction = LaneChangeDirection.none
else: else:
if self.blindspot_detected_counter > 0 or self.torque_always: if lane_available:
if torque_applied and lane_available or self.torque_always: if self.blindspot_detected_counter > 0 and not ignore_bsd: # BSD검출시
if torque_applied and not block_lanechange_bsd:
self.lane_change_state = LaneChangeState.laneChangeStarting
elif self.laneChangeNeedTorque > 0: # 조향토크필요
if torque_applied:
self.lane_change_state = LaneChangeState.laneChangeStarting
elif driver_desire_enabled:
self.lane_change_state = LaneChangeState.laneChangeStarting self.lane_change_state = LaneChangeState.laneChangeStarting
elif self.laneChangeNeedTorque == 1: # 1: need torque, 2: no lanechange, 3: ignore bsd # ATC작동인경우 차선이 나타나거나 차선이 생기면 차선변경 시작
if torque_applied and lane_available: # lane_appeared: 차선이 생기는건 안함.. 위험.
elif torque_applied or auto_lane_change_available:
self.lane_change_state = LaneChangeState.laneChangeStarting self.lane_change_state = LaneChangeState.laneChangeStarting
# 운전자가 깜박이켠경우는 바로 차선변경 시작
elif driver_desire_enabled and lane_available:
self.lane_change_state = LaneChangeState.laneChangeStarting
# ATC작동인경우 차선이 나타나거나 차선이 생기면 차선변경 시작
# lane_appeared: 차선이 생기는건 안함.. 위험.
elif torque_applied or auto_lane_change_available:
self.lane_change_state = LaneChangeState.laneChangeStarting
# LaneChangeState.laneChangeStarting # LaneChangeState.laneChangeStarting
elif self.lane_change_state == LaneChangeState.laneChangeStarting: elif self.lane_change_state == LaneChangeState.laneChangeStarting:

View File

@ -70,6 +70,7 @@ class LanePlanner:
self.lane_offset_filtered = FirstOrderFilter(0.0, 2.0, DT_MDL) self.lane_offset_filtered = FirstOrderFilter(0.0, 2.0, DT_MDL)
self.lanefull_mode = False self.lanefull_mode = False
self.d_prob_count = 0
self.params = Params() self.params = Params()
@ -231,7 +232,8 @@ class LanePlanner:
adjustLaneTime = 0.05 #self.params.get_int("AdjustLaneTime") * 0.01 adjustLaneTime = 0.05 #self.params.get_int("AdjustLaneTime") * 0.01
laneline_active = False laneline_active = False
if self.lanefull_mode and self.d_prob > 0.3: self.d_prob_count = self.d_prob_count + 1 if self.d_prob > 0.3 else 0
if self.lanefull_mode and self.d_prob_count > int(1 / DT_MDL):
laneline_active = True laneline_active = True
use_dist_mode = False ## 아무리생각해봐도.. 같은 방법인듯... use_dist_mode = False ## 아무리생각해봐도.. 같은 방법인듯...
if use_dist_mode: if use_dist_mode:

View File

@ -422,6 +422,8 @@ class LongitudinalMpc:
if v_cruise == 0 and self.source == 'cruise': if v_cruise == 0 and self.source == 'cruise':
self.params[:,0] = - carrot.autoNaviSpeedDecelRate self.params[:,0] = - carrot.autoNaviSpeedDecelRate
#elif self.source in ['cruise', 'e2e']:
# self.params[:,0] = - COMFORT_BRAKE
# These are not used in ACC mode # These are not used in ACC mode
x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0 x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0

View File

@ -299,7 +299,7 @@ class VisionTrack:
self.yRel = float(-lead_msg.y[0]) self.yRel = float(-lead_msg.y[0])
dPath = self.yRel + np.interp(self.dRel, md.position.x, md.position.y) dPath = self.yRel + np.interp(self.dRel, md.position.x, md.position.y)
a_lead_vision = lead_msg.a[0] a_lead_vision = lead_msg.a[0]
if self.cnt < 2 or self.prob < 0.97: if self.cnt < 20 or self.prob < 0.97: # 레이더측정시 cnt는 0, 레이더사라지고 1초간 비젼데이터 그대로 사용
self.vRel = lead_v_rel_pred self.vRel = lead_v_rel_pred
self.vLead = float(v_ego + lead_v_rel_pred) self.vLead = float(v_ego + lead_v_rel_pred)
self.aLead = a_lead_vision self.aLead = a_lead_vision
@ -366,6 +366,8 @@ class RadarD:
self.params = Params() self.params = Params()
self.enable_radar_tracks = self.params.get_int("EnableRadarTracks") self.enable_radar_tracks = self.params.get_int("EnableRadarTracks")
self.radar_detected = False
def update(self, sm: messaging.SubMaster, rr: car.RadarData): def update(self, sm: messaging.SubMaster, rr: car.RadarData):
self.ready = sm.seen['modelV2'] self.ready = sm.seen['modelV2']
@ -426,13 +428,16 @@ class RadarD:
if len(leads_v3) > 1: if len(leads_v3) > 1:
if model_updated: if model_updated:
if self.radar_detected:
self.vision_tracks[0].cnt = 0
self.vision_tracks[1].cnt = 0
self.vision_tracks[0].update(leads_v3[0], model_v_ego, self.v_ego, sm['modelV2']) self.vision_tracks[0].update(leads_v3[0], model_v_ego, self.v_ego, sm['modelV2'])
self.vision_tracks[1].update(leads_v3[1], model_v_ego, self.v_ego, sm['modelV2']) self.vision_tracks[1].update(leads_v3[1], model_v_ego, self.v_ego, sm['modelV2'])
#self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=False) #self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=False)
#self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False) #self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False)
self.radar_state.leadOne = self.get_lead(sm['modelV2'], self.tracks, 0, leads_v3[0], model_v_ego, low_speed_override=False) self.radar_state.leadOne, self.radar_detected = self.get_lead(sm['modelV2'], self.tracks, 0, leads_v3[0], model_v_ego, low_speed_override=False)
self.radar_state.leadTwo = self.get_lead(sm['modelV2'], self.tracks, 1, leads_v3[1], model_v_ego, low_speed_override=False) self.radar_state.leadTwo, _ = self.get_lead(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'], # ll, lc, lr, leadCenter, self.radar_state.leadLeft, self.radar_state.leadRight = get_lead_side(self.v_ego, self.tracks, sm['modelV2'],
# sm['lateralPlan'].laneWidth, model_v_ego) # sm['lateralPlan'].laneWidth, model_v_ego)
@ -482,9 +487,11 @@ class RadarD:
# track = None # track = None
lead_dict = {'status': False} lead_dict = {'status': False}
radar = False
if track is not None: if track is not None:
#lead_dict = track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel, self.vision_tracks[0].vLat) #lead_dict = track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel, self.vision_tracks[0].vLat)
lead_dict = track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel) lead_dict = track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel)
radar = True
elif (track is None) and ready and (lead_msg.prob > .8): elif (track is None) and ready and (lead_msg.prob > .8):
#if self.mixRadarInfo == 4 and v_ego * 3.6 > 30 and lead_msg.prob < 0.99: ## #if self.mixRadarInfo == 4 and v_ego * 3.6 > 30 and lead_msg.prob < 0.99: ##
# pass # pass
@ -501,7 +508,7 @@ class RadarD:
#lead_dict = closest_track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel, self.vision_tracks[0].vLat) #lead_dict = closest_track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel, self.vision_tracks[0].vLat)
lead_dict = closest_track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel) lead_dict = closest_track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel)
return lead_dict return lead_dict, radar
# fuses camera and radar data for best lead detection # fuses camera and radar data for best lead detection
def main() -> None: def main() -> None:

View File

@ -654,6 +654,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
cruiseToggles = new ListWidget(this); cruiseToggles = new ListWidget(this);
cruiseToggles->addItem(new CValueControl("CruiseButtonMode", "Button: Cruise Button Mode", "0:Normal,1:User1,2:User2", "../assets/offroad/icon_road.png", 0, 2, 1)); cruiseToggles->addItem(new CValueControl("CruiseButtonMode", "Button: Cruise Button Mode", "0:Normal,1:User1,2:User2", "../assets/offroad/icon_road.png", 0, 2, 1));
cruiseToggles->addItem(new CValueControl("LfaButtonMode", "Button: LFA Button Mode", "0:Normal,1:Decel&Stop&LeadCarReady", "../assets/offroad/icon_road.png", 0, 1, 1));
cruiseToggles->addItem(new CValueControl("CruiseSpeedUnit", "Button: Cruise Speed Unit", "", "../assets/offroad/icon_road.png", 1, 20, 1)); cruiseToggles->addItem(new CValueControl("CruiseSpeedUnit", "Button: Cruise Speed Unit", "", "../assets/offroad/icon_road.png", 1, 20, 1));
cruiseToggles->addItem(new CValueControl("CruiseEcoControl", "CRUISE: Eco control(4km/h)", "Temporarily increasing the set speed to improve fuel efficiency.", "../assets/offroad/icon_road.png", 0, 10, 1)); cruiseToggles->addItem(new CValueControl("CruiseEcoControl", "CRUISE: Eco control(4km/h)", "Temporarily increasing the set speed to improve fuel efficiency.", "../assets/offroad/icon_road.png", 0, 10, 1));
//cruiseToggles->addItem(new CValueControl("CruiseSpeedMin", "CRUISE: Speed Lower limit(10)", "Cruise control MIN speed", "../assets/offroad/icon_road.png", 5, 50, 1)); //cruiseToggles->addItem(new CValueControl("CruiseSpeedMin", "CRUISE: Speed Lower limit(10)", "Cruise control MIN speed", "../assets/offroad/icon_road.png", 5, 50, 1));
@ -694,7 +695,8 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
latLongToggles->addItem(new CValueControl("RadarReactionFactor", "LONG: Radar reaction factor(100)", "", "../assets/offroad/icon_logic.png", 0, 200, 10)); latLongToggles->addItem(new CValueControl("RadarReactionFactor", "LONG: Radar reaction factor(100)", "", "../assets/offroad/icon_logic.png", 0, 200, 10));
//latLongToggles->addItem(new CValueControl("StartAccelApply", "LONG: StartingAccel 2.0x(0)%", "정지->출발시 가속도의 가속율을 지정합니다 0: 사용안함.", "../assets/offroad/icon_road.png", 0, 100, 10)); //latLongToggles->addItem(new CValueControl("StartAccelApply", "LONG: StartingAccel 2.0x(0)%", "정지->출발시 가속도의 가속율을 지정합니다 0: 사용안함.", "../assets/offroad/icon_road.png", 0, 100, 10));
//latLongToggles->addItem(new CValueControl("StopAccelApply", "LONG: StoppingAccel -2.0x(0)%", "정지유지시 브레이크압을 조정합니다. 0: 사용안함. ", "../assets/offroad/icon_road.png", 0, 100, 10)); //latLongToggles->addItem(new CValueControl("StopAccelApply", "LONG: StoppingAccel -2.0x(0)%", "정지유지시 브레이크압을 조정합니다. 0: 사용안함. ", "../assets/offroad/icon_road.png", 0, 100, 10));
latLongToggles->addItem(new CValueControl("LaneChangeNeedTorque", "LaneChange need torque", "1:need torque, 2:no lanechange, 3:ignore BSD, 4:torque always", "../assets/offroad/icon_logic.png", 0, 4, 1)); latLongToggles->addItem(new CValueControl("LaneChangeNeedTorque", "LaneChange need torque", "-1:Disable lanechange, 0: no need torque, 1:need torque", "../assets/offroad/icon_logic.png", -1, 1, 1));
latLongToggles->addItem(new CValueControl("LaneChangeBsd", "LaneChange Bsd", "-1:ignore bsd, 0:BSD detect, 1: block steer torque", "../assets/offroad/icon_logic.png", -1, 1, 1));
latLongToggles->addItem(new CValueControl("StoppingAccel", "LONG: StoppingStartAccelx0.01(-40)", "", "../assets/offroad/icon_logic.png", -100, 0, 5)); latLongToggles->addItem(new CValueControl("StoppingAccel", "LONG: StoppingStartAccelx0.01(-40)", "", "../assets/offroad/icon_logic.png", -100, 0, 5));
latLongToggles->addItem(new CValueControl("StopDistanceCarrot", "LONG: StopDistance (600)cm", "", "../assets/offroad/icon_logic.png", 300, 1000, 10)); latLongToggles->addItem(new CValueControl("StopDistanceCarrot", "LONG: StopDistance (600)cm", "", "../assets/offroad/icon_logic.png", 300, 1000, 10));
//latLongToggles->addItem(new CValueControl("TraffStopDistanceAdjust", "LONG: TrafficStopDistance adjust(150)cm", "", "../assets/offroad/icon_road.png", -1000, 1000, 10)); //latLongToggles->addItem(new CValueControl("TraffStopDistanceAdjust", "LONG: TrafficStopDistance adjust(150)cm", "", "../assets/offroad/icon_road.png", -1000, 1000, 10));

View File

@ -85,7 +85,8 @@ def get_default_params():
("StoppingAccel", "0"), ("StoppingAccel", "0"),
("StopDistanceCarrot", "550"), ("StopDistanceCarrot", "550"),
("JLeadFactor3", "0"), ("JLeadFactor3", "0"),
("CruiseButtonMode", "2"), ("CruiseButtonMode", "0"),
("LfaButtonMode", "0"),
("CruiseButtonTest1", "8"), ("CruiseButtonTest1", "8"),
("CruiseButtonTest2", "30"), ("CruiseButtonTest2", "30"),
("CruiseButtonTest3", "1"), ("CruiseButtonTest3", "1"),
@ -125,6 +126,7 @@ def get_default_params():
("UseLaneLineSpeedApply", "0"), ("UseLaneLineSpeedApply", "0"),
("AdjustLaneOffset", "0"), ("AdjustLaneOffset", "0"),
("LaneChangeNeedTorque", "0"), ("LaneChangeNeedTorque", "0"),
("LaneChangeBsd", "0"),
("MaxAngleFrames", "89"), ("MaxAngleFrames", "89"),
("LateralTorqueCustom", "0"), ("LateralTorqueCustom", "0"),
("LateralTorqueAccelFactor", "2500"), ("LateralTorqueAccelFactor", "2500"),